sot-core  4.10.1
Hierarchical task solver plug-in for dynamic-graph.
robot-utils.hh
Go to the documentation of this file.
1 /*
2  * Copyright 2017, 2019
3  * LAAS-CNRS
4  * A. Del Prete, T. Flayols, O. Stasse, F. Bailly
5  *
6  */
7 
8 #ifndef __sot_torque_control_common_H__
9 #define __sot_torque_control_common_H__
10 
11 /* --------------------------------------------------------------------- */
12 /* --- INCLUDE --------------------------------------------------------- */
13 /* --------------------------------------------------------------------- */
14 
23 #ifdef BOOST_MPL_LIMIT_VECTOR_SIZE
24 #pragma push_macro("BOOST_MPL_LIMIT_VECTOR_SIZE")
25 #define UNDEF_BOOST_MPL_LIMIT_VECTOR_SIZE
26 #undef BOOST_MPL_LIMIT_VECTOR_SIZE
27 #endif
28 
29 #ifdef BOOST_MPL_LIMIT_LIST_SIZE
30 #pragma push_macro("BOOST_MPL_LIMIT_LIST_SIZE")
31 #define UNDEF_BOOST_MPL_LIMIT_LIST_SIZE
32 #undef BOOST_MPL_LIMIT_LIST_SIZE
33 #endif
34 
35 #include <boost/property_tree/ptree.hpp>
36 
37 #ifdef UNDEF_BOOST_MPL_LIMIT_VECTOR_SIZE
38 #pragma pop_macro("BOOST_MPL_LIMIT_VECTOR_SIZE")
39 #undef UNDEF_BOOST_MPL_LIMIT_VECTOR_SIZE
40 #endif
41 
42 #ifdef UNDEF_BOOST_MPL_LIMIT_LIST_SIZE
43 #pragma pop_macro("BOOST_MPL_LIMIT_LIST_SIZE")
44 #undef UNDEF_BOOST_MPL_LIMIT_LIST_SIZE
45 #endif
46 
47 #include "boost/assign.hpp"
48 #include <dynamic-graph/linear-algebra.h>
49 #include <dynamic-graph/logger.h>
50 #include <dynamic-graph/signal-helper.h>
51 #include <map>
53 
54 namespace dynamicgraph {
55 namespace sot {
56 
58  double upper;
59  double lower;
60 
61  JointLimits() : upper(0.0), lower(0.0) {}
62 
63  JointLimits(double l, double u) : upper(u), lower(l) {}
64 };
65 
67 
69 
70 public:
72  ExtractJointMimics(std::string &robot_model);
73 
75  const std::vector<std::string> &get_mimic_joints();
76 
77 private:
78  void go_through(boost::property_tree::ptree &pt, int level, int stage);
79 
80  // Create empty property tree object
81  boost::property_tree::ptree tree_;
82  std::vector<std::string> mimic_joints_;
83  std::string current_joint_name_;
84  void go_through_full();
85 };
86 
88  Eigen::VectorXd upper;
89  Eigen::VectorXd lower;
90 
91  ForceLimits() : upper(Vector6d::Zero()), lower(Vector6d::Zero()) {}
92 
93  ForceLimits(const Eigen::VectorXd &l, const Eigen::VectorXd &u)
94  : upper(u), lower(l) {}
95 
96  void display(std::ostream &os) const;
97 };
98 
100  std::map<Index, ForceLimits> m_force_id_to_limits;
101  std::map<std::string, Index> m_name_to_force_id;
102  std::map<Index, std::string> m_force_id_to_name;
103 
104  Index m_Force_Id_Left_Hand, m_Force_Id_Right_Hand, m_Force_Id_Left_Foot,
105  m_Force_Id_Right_Foot;
106 
107  void set_name_to_force_id(const std::string &name, const Index &force_id);
108 
109  void set_force_id_to_limits(const Index &force_id,
110  const dynamicgraph::Vector &lf,
111  const dynamicgraph::Vector &uf);
112 
113  void create_force_id_to_name_map();
114 
115  Index get_id_from_name(const std::string &name);
116 
117  const std::string &get_name_from_id(Index idx);
118  std::string cp_get_name_from_id(Index idx);
119 
120  const ForceLimits &get_limits_from_id(Index force_id);
121  ForceLimits cp_get_limits_from_id(Index force_id);
122 
123  Index get_force_id_left_hand() { return m_Force_Id_Left_Hand; }
124 
125  void set_force_id_left_hand(Index anId) { m_Force_Id_Left_Hand = anId; }
126 
127  Index get_force_id_right_hand() { return m_Force_Id_Right_Hand; }
128 
129  void set_force_id_right_hand(Index anId) { m_Force_Id_Right_Hand = anId; }
130 
131  Index get_force_id_left_foot() { return m_Force_Id_Left_Foot; }
132 
133  void set_force_id_left_foot(Index anId) { m_Force_Id_Left_Foot = anId; }
134 
135  Index get_force_id_right_foot() { return m_Force_Id_Right_Foot; }
136 
137  void set_force_id_right_foot(Index anId) { m_Force_Id_Right_Foot = anId; }
138 
139  void display(std::ostream &out) const;
140 
141 }; // struct ForceUtil
142 
145  dynamicgraph::Vector m_Right_Foot_Sole_XYZ;
146 
148  dynamicgraph::Vector m_Right_Foot_Force_Sensor_XYZ;
149 
152  void display(std::ostream &os) const;
153 };
154 
158  void display(std::ostream &os) const;
159 };
160 
162 public:
163  RobotUtil();
164 
167 
170 
173 
175  std::vector<Index> m_urdf_to_sot;
176 
178  std::size_t m_nbJoints;
179 
181  std::map<std::string, Index> m_name_to_id;
182 
184  std::map<Index, std::string> m_id_to_name;
185 
187  std::map<Index, JointLimits> m_limits_map;
188 
190  std::string m_imu_joint_name;
191 
195  void create_id_to_name_map();
196 
198  std::string m_urdf_filename;
199 
200  dynamicgraph::Vector m_dgv_urdf_to_sot;
201 
206  const Index &get_id_from_name(const std::string &name);
207 
214  const std::string &get_name_from_id(Index id);
216 
218  void set_name_to_id(const std::string &jointName, const Index &jointId);
219 
221  void set_urdf_to_sot(const std::vector<Index> &urdf_to_sot);
222  void set_urdf_to_sot(const dynamicgraph::Vector &urdf_to_sot);
223 
225  void set_joint_limits_for_id(const Index &idx, const double &lq,
226  const double &uq);
227 
228  bool joints_urdf_to_sot(ConstRefVector q_urdf, RefVector q_sot);
229 
230  bool joints_sot_to_urdf(ConstRefVector q_sot, RefVector q_urdf);
231 
232  bool velocity_urdf_to_sot(ConstRefVector q_urdf, ConstRefVector v_urdf,
233  RefVector v_sot);
234 
235  bool velocity_sot_to_urdf(ConstRefVector q_urdf, ConstRefVector v_sot,
236  RefVector v_urdf);
237 
238  bool config_urdf_to_sot(ConstRefVector q_urdf, RefVector q_sot);
239  bool config_sot_to_urdf(ConstRefVector q_sot, RefVector q_urdf);
240 
241  bool base_urdf_to_sot(ConstRefVector q_urdf, RefVector q_sot);
242  bool base_sot_to_urdf(ConstRefVector q_sot, RefVector q_urdf);
243 
249  const JointLimits &get_joint_limits_from_id(Index id);
250  JointLimits cp_get_joint_limits_from_id(Index id);
251 
254  void sendMsg(const std::string &msg, MsgType t = MSG_TYPE_INFO,
257  const std::string &lineId = "");
258 
260  void setLoggerVerbosityLevel(LoggerVerbosity lv) { logger_.setVerbosity(lv); }
261 
263  LoggerVerbosity getLoggerVerbosityLevel() { return logger_.getVerbosity(); };
264 
265  void display(std::ostream &os) const;
266 
272  template <typename Type>
273  void set_parameter(const std::string &parameter_name,
274  const Type &parameter_value) {
275  try {
276  typedef boost::property_tree::ptree::path_type path;
277  path apath(parameter_name, '/');
278  property_tree_.put<Type>(apath, parameter_value);
279  } catch (const boost::property_tree::ptree_error &e) {
280  std::ostringstream oss;
281  oss << "Robot utils: parameter path is invalid " << '\n'
282  << " for set_parameter(" << parameter_name << ")\n"
283  << e.what() << std::endl;
284  sendMsg(oss.str(), MSG_TYPE_ERROR);
285  return;
286  }
287  }
288 
295  template <typename Type>
296  Type get_parameter(const std::string &parameter_name) {
297  try {
298  boost::property_tree::ptree::path_type apath(parameter_name, '/');
299  const Type &res = property_tree_.get<Type>(apath);
300 
301  return res;
302  } catch (const boost::property_tree::ptree_error &e) {
303  std::ostringstream oss;
304  oss << "Robot utils: parameter path is invalid " << '\n'
305  << " for get_parameter(" << parameter_name << ")\n"
306  << e.what() << std::endl;
307  sendMsg(oss.str(), MSG_TYPE_ERROR);
308  }
309  }
313  boost::property_tree::ptree &get_property_tree();
314 
315 protected:
316  Logger logger_;
317 
319  std::map<std::string, std::string> parameters_strings_;
320 
322  boost::property_tree::ptree property_tree_;
323 }; // struct RobotUtil
324 
326 typedef std::shared_ptr<RobotUtil> RobotUtilShrPtr;
327 
328 RobotUtilShrPtr RefVoidRobotUtil();
329 RobotUtilShrPtr getRobotUtil(std::string &robotName);
330 bool isNameInRobotUtil(std::string &robotName);
331 RobotUtilShrPtr createRobotUtil(std::string &robotName);
332 std::shared_ptr<std::vector<std::string> > getListOfRobots();
333 
335 
336 } // namespace sot
337 } // namespace dynamicgraph
338 
339 #endif // sot_torque_control_common_h_
JointLimits(double l, double u)
Definition: robot-utils.hh:63
Definition: robot-utils.hh:99
std::map< Index, JointLimits > m_limits_map
The joint limits map.
Definition: robot-utils.hh:187
Definition: robot-utils.hh:143
const Eigen::Ref< const Eigen::VectorXd > & ConstRefVector
Definition: matrix-geometry.hh:69
Index m_Force_Id_Right_Hand
Definition: robot-utils.hh:104
Definition: robot-utils.hh:161
ForceUtil m_force_util
Forces data.
Definition: robot-utils.hh:166
void set_force_id_right_hand(Index anId)
Definition: robot-utils.hh:129
bool base_se3_to_sot(ConstRefVector pos, ConstRefMatrix R, RefVector q_sot)
Eigen::VectorXd::Index Index
Definition: robot-utils.hh:66
Definition: robot-utils.hh:155
RobotUtilShrPtr createRobotUtil(std::string &robotName)
double lower
Definition: robot-utils.hh:59
std::string m_Left_Hand_Frame_Name
Definition: robot-utils.hh:156
void set_parameter(const std::string &parameter_name, const Type &parameter_value)
Set a parameter of type string. If parameter_name already exists the value is overwritten. If not it is inserted.
Definition: robot-utils.hh:273
Eigen::VectorXd upper
Definition: robot-utils.hh:88
void set_force_id_right_foot(Index anId)
Definition: robot-utils.hh:137
dynamicgraph::Vector m_Right_Foot_Sole_XYZ
Position of the foot soles w.r.t. the frame of the foot.
Definition: robot-utils.hh:145
std::map< std::string, Index > m_name_to_force_id
Definition: robot-utils.hh:101
dynamicgraph::Vector m_dgv_urdf_to_sot
Definition: robot-utils.hh:200
Index get_force_id_right_foot()
Definition: robot-utils.hh:135
#define SOT_CORE_EXPORT
Definition: api.hh:20
void set_force_id_left_foot(Index anId)
Definition: robot-utils.hh:133
JointLimits()
Definition: robot-utils.hh:61
ForceLimits()
Definition: robot-utils.hh:91
void setLoggerVerbosityLevel(LoggerVerbosity lv)
Specify the verbosity level of the logger.
Definition: robot-utils.hh:260
const Eigen::Ref< const Eigen::MatrixXd > ConstRefMatrix
Definition: matrix-geometry.hh:71
std::string m_imu_joint_name
The name of the joint IMU is attached to.
Definition: robot-utils.hh:190
std::map< Index, ForceLimits > m_force_id_to_limits
Definition: robot-utils.hh:100
std::shared_ptr< std::vector< std::string > > getListOfRobots()
Eigen::VectorXd lower
Definition: robot-utils.hh:89
Definition: robot-utils.hh:87
std::shared_ptr< RobotUtil > RobotUtilShrPtr
Accessors - This should be changed to RobotUtilPtrShared.
Definition: robot-utils.hh:326
ForceLimits(const Eigen::VectorXd &l, const Eigen::VectorXd &u)
Definition: robot-utils.hh:93
double upper
Definition: robot-utils.hh:58
std::size_t m_nbJoints
Nb of Dofs for the robot.
Definition: robot-utils.hh:178
void set_force_id_left_hand(Index anId)
Definition: robot-utils.hh:125
RobotUtilShrPtr RefVoidRobotUtil()
LoggerVerbosity getLoggerVerbosityLevel()
Get the logger&#39;s verbosity level.
Definition: robot-utils.hh:263
Index get_force_id_right_hand()
Definition: robot-utils.hh:127
std::vector< Index > m_urdf_to_sot
Map from the urdf index to the SoT index.
Definition: robot-utils.hh:175
std::string m_Right_Hand_Frame_Name
Definition: robot-utils.hh:157
RobotUtilShrPtr getRobotUtil(std::string &robotName)
FootUtil m_foot_util
Foot information.
Definition: robot-utils.hh:169
boost::property_tree::ptree property_tree_
Property tree.
Definition: robot-utils.hh:322
Index get_force_id_left_hand()
Definition: robot-utils.hh:123
std::string m_Left_Foot_Frame_Name
Definition: robot-utils.hh:150
std::map< std::string, std::string > parameters_strings_
Map of the parameters: map of strings.
Definition: robot-utils.hh:319
std::map< std::string, Index > m_name_to_id
Map from the name to the id.
Definition: robot-utils.hh:181
Type get_parameter(const std::string &parameter_name)
Get a parameter of type string. If parameter_name already exists the value is overwritten. If not it is inserted.
Definition: robot-utils.hh:296
dynamicgraph::Vector m_Right_Foot_Force_Sensor_XYZ
Position of the force/torque sensors w.r.t. the frame of the hosting link.
Definition: robot-utils.hh:148
HandUtil m_hand_util
Hand information.
Definition: robot-utils.hh:172
Definition: robot-utils.hh:68
Eigen::Ref< Eigen::VectorXd > RefVector
Definition: matrix-geometry.hh:68
Logger logger_
Definition: robot-utils.hh:316
std::map< Index, std::string > m_force_id_to_name
Definition: robot-utils.hh:102
Index get_force_id_left_foot()
Definition: robot-utils.hh:131
Definition: robot-utils.hh:57
bool isNameInRobotUtil(std::string &robotName)
std::string m_Right_Foot_Frame_Name
Definition: robot-utils.hh:151
std::map< Index, std::string > m_id_to_name
The map between id and name.
Definition: robot-utils.hh:184
Definition: abstract-sot-external-interface.hh:17
std::string m_urdf_filename
URDF file path.
Definition: robot-utils.hh:198