humoto
task_tagangularvel.h
Go to the documentation of this file.
1 /**
2  @file
3  @author Jan Michalczyk
4  @copyright 2014-2017 INRIA. Licensed under the Apache License, Version 2.0.
5  (see @ref LICENSE or http://www.apache.org/licenses/LICENSE-2.0)
6 
7  @brief
8 */
9 
10 #pragma once
11 
12 namespace humoto
13 {
14  namespace pepper_ik
15  {
16  /**
17  * @brief Tag angular velocity
18  */
19  template <int t_features>
21  {
22  #define HUMOTO_CONFIG_ENTRIES \
23  HUMOTO_CONFIG_PARENT_CLASS(TaskAB) \
24  HUMOTO_CONFIG_SCALAR_(k_angular_velocity_gain) \
25  HUMOTO_CONFIG_SCALAR_(tag_string_id)
26  #include HUMOTO_CONFIG_DEFINE_ACCESSORS
27 
28 
29  protected:
31  std::string tag_string_id_;
32 
34 
35 
36  protected:
37  virtual void setDefaults()
38  {
40  k_angular_velocity_gain_ = 0.0;
41  }
42 
43 
44  /**
45  * @brief Log task.
46  *
47  * @param[in] logger logger
48  * @param[in] parent parent
49  * @param[in] name name
50  */
52  const LogEntryName &parent = LogEntryName(),
53  const std::string &name = "task") const
54  {
55  LogEntryName subname = parent; subname.add(name);
56  TaskAB::logTask(logger, subname, "");
57  logger.log(LogEntryName(subname).add("k_angular_velocity_gain"), k_angular_velocity_gain_);
58  logger.log(LogEntryName(subname).add("tag_string_id"), tag_string_id_);
59  }
60 
61 
62  public:
63  TaskTagAngularVelocity(const std::string& tag_string_id = "",
64  const double gain = 1.0,
65  const double k_angular_velocity_gain = 1.0)
66  : TaskAB(std::string("TaskTagAngularVelocity_") + tag_string_id, gain)
67  {
68  k_angular_velocity_gain_ = k_angular_velocity_gain;
69  tag_string_id_ = tag_string_id;
70  }
71 
72 
73  /// @copydoc humoto::TaskBase::form
74  void form(const humoto::SolutionStructure &sol_structure,
75  const humoto::Model &model_base,
76  const humoto::ControlProblem &control_problem)
77  {
78  const Model<t_features>& model =
79  dynamic_cast <const Model<t_features>& >(model_base);
80 
81  const WholeBodyController<t_features>& wb_controller =
82  dynamic_cast <const WholeBodyController<t_features>& >(control_problem);
83 
84  if(!tag_)
85  {
86  tag_ = model.getLinkTag(tag_string_id_);
87  }
88 
89  Eigen::MatrixXd &A = getA();
90  Eigen::VectorXd &b = getB();
91 
92  model.getTagOrientationJacobian(A, tag_);
93 
94  b.noalias() = k_angular_velocity_gain_ *
95  wb_controller.getTagVelocityInGlobal(model, tag_string_id_,
96  wb_controller.getTagVelocityInLocal(tag_string_id_, rbdl::SpatialType::COMPLETE),
98 
99  if(!isApproximatelyEqual(1.0, getGain()))
100  {
101  A*=getGain();
102  b*=getGain();
103  }
104  }
105  };
106  }//pepper
107 }//humoto
boost::shared_ptr< const TagLink > TagLinkPtr
Definition: tag.h:44
Abstract base class (for control problems)
virtual void setDefaults()
Set members to their default values.
#define HUMOTO_LOCAL
Definition: export_import.h:26
virtual void logTask(humoto::Logger &logger, const LogEntryName &parent=LogEntryName(), const std::string &name="task") const
Log task.
virtual void logTask(humoto::Logger &logger, const LogEntryName &parent=LogEntryName(), const std::string &name="task") const
Log task.
Definition: task.h:347
STL namespace.
#define HUMOTO_GLOBAL_LOGGER_IF_DEFINED
Definition: logger.h:997
Analog of &#39;sol_structure&#39; struct in Octave code. [determine_solution_structure.m].
Definition: solution.h:33
virtual void setDefaults()
Set members to their default values.
Definition: task.h:332
Represents log entry name.
Definition: logger.h:169
TaskTagAngularVelocity(const std::string &tag_string_id="", const double gain=1.0, const double k_angular_velocity_gain=1.0)
Threaded logger: any data sent to this logger is wrapped in a message and pushed to a queue...
Definition: logger.h:555
The root namespace of HuMoTo.
Definition: config.h:12
rbdl::TagLinkPtr getLinkTag(const std::string &id) const
Get tag.
Definition: model.h:451
Instances of this class are passed to a virtual method &#39;humoto::TaskBase::form()&#39;, so even though this class is basically useless in its present form we cannot avoid its definition using a template.
Definition: model.h:41
LogEntryName & add(const char *name)
extends entry name with a subname
Definition: logger.h:232
bool isApproximatelyEqual(const double var1, const double var2, const double tol=humoto::g_generic_tolerance)
Returns true if the difference between two given variables is below the given tolerance.
Definition: utility.h:87
Task: A*x = b.
Definition: task.h:542
void form(const humoto::SolutionStructure &sol_structure, const humoto::Model &model_base, const humoto::ControlProblem &control_problem)
Form the task.
void getTagOrientationJacobian(Eigen::MatrixXd &jacobian, const rbdl::TagLinkPtr tag) const
Get tag orientation Jacobian.
Definition: model.h:489