humoto
task_tagpose.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 pose
18  */
19  template <int t_features>
21  {
22  #define HUMOTO_CONFIG_ENTRIES \
23  HUMOTO_CONFIG_PARENT_CLASS(TaskAB) \
24  HUMOTO_CONFIG_SCALAR_(k_pose_gain) \
25  HUMOTO_CONFIG_SCALAR_(tag_string_id)
26  #include HUMOTO_CONFIG_DEFINE_ACCESSORS
27 
28 
29  protected:
30  double k_pose_gain_;
31  std::string tag_string_id_;
32 
34 
35 
36  protected:
37  virtual void setDefaults()
38  {
40  k_pose_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_pose_gain"), k_pose_gain_);
58  logger.log(LogEntryName(subname).add("tag_string_id"), tag_string_id_);
59  }
60 
61 
62  public:
63  TaskTagPose(const std::string& tag_string_id = "",
64  const double gain = 1.0,
65  const double k_pose_gain = 1.0)
66  : TaskAB(std::string("TaskTagPose_") + tag_string_id, gain)
67  {
68  k_pose_gain_ = k_pose_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.getTagCompleteJacobian(A, tag_);
93 
94  b.noalias() = k_pose_gain_ * wb_controller.getTagPoseErrorInGlobal(model, tag_string_id_);
95 
96  if(!isApproximatelyEqual(1.0, getGain()))
97  {
98  A*=getGain();
99  b*=getGain();
100  }
101  }
102  };
103  }//pepper
104 }//humoto
boost::shared_ptr< const TagLink > TagLinkPtr
Definition: tag.h:44
virtual void setDefaults()
Set members to their default values.
Definition: task_tagpose.h:37
Abstract base class (for control problems)
TaskTagPose(const std::string &tag_string_id="", const double gain=1.0, const double k_pose_gain=1.0)
Definition: task_tagpose.h:63
#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.
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
virtual void logTask(humoto::Logger &logger, const LogEntryName &parent=LogEntryName(), const std::string &name="task") const
Log task.
Definition: task_tagpose.h:51
Represents log entry name.
Definition: logger.h:169
void getTagCompleteJacobian(Eigen::MatrixXd &jacobian, const rbdl::TagLinkPtr tag) const
Get tag complete Jacobian.
Definition: model.h:502
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
void form(const humoto::SolutionStructure &sol_structure, const humoto::Model &model_base, const humoto::ControlProblem &control_problem)
Form the task.
Definition: task_tagpose.h:74
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