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_core_robot_utils_H__
9 #define __sot_core_robot_utils_H__
10 
11 /* --------------------------------------------------------------------- */
12 /* --- INCLUDE --------------------------------------------------------- */
13 /* --------------------------------------------------------------------- */
14 
15 #include <map>
16 #include <pinocchio/fwd.hpp>
17 // keep pinocchio before boost
18 
20 #include <dynamic-graph/logger.h>
22 
23 #include <boost/assign.hpp>
24 #include <boost/property_tree/ptree.hpp>
26 
27 namespace dynamicgraph {
28 namespace sot {
29 
31  double upper;
32  double lower;
33 
34  JointLimits() : upper(0.0), lower(0.0) {}
35 
36  JointLimits(double l, double u) : upper(u), lower(l) {}
37 };
38 
40 
42  public:
44  ExtractJointMimics(std::string &robot_model);
45 
47  const std::vector<std::string> &get_mimic_joints();
48 
49  private:
50  void go_through(boost::property_tree::ptree &pt, size_type level,
51  size_type stage);
52 
53  // Create empty property tree object
54  boost::property_tree::ptree tree_;
55  std::vector<std::string> mimic_joints_;
56  std::string current_joint_name_;
57  void go_through_full();
58 };
59 
61  Eigen::VectorXd upper;
62  Eigen::VectorXd lower;
63 
64  ForceLimits() : upper(Vector6d::Zero()), lower(Vector6d::Zero()) {}
65 
66  ForceLimits(const Eigen::VectorXd &l, const Eigen::VectorXd &u)
67  : upper(u), lower(l) {}
68 
69  void display(std::ostream &os) const;
70 };
71 
73  std::map<Index, ForceLimits> m_force_id_to_limits;
74  std::map<std::string, Index> m_name_to_force_id;
75  std::map<Index, std::string> m_force_id_to_name;
76 
77  Index m_Force_Id_Left_Hand, m_Force_Id_Right_Hand, m_Force_Id_Left_Foot,
79 
80  void set_name_to_force_id(const std::string &name, const Index &force_id);
81 
82  void set_force_id_to_limits(const Index &force_id,
83  const dynamicgraph::Vector &lf,
84  const dynamicgraph::Vector &uf);
85 
86  void create_force_id_to_name_map();
87 
88  Index get_id_from_name(const std::string &name);
89 
90  const std::string &get_name_from_id(Index idx);
91  std::string cp_get_name_from_id(Index idx);
92 
93  const ForceLimits &get_limits_from_id(Index force_id);
94  ForceLimits cp_get_limits_from_id(Index force_id);
95 
96  Index get_force_id_left_hand() { return m_Force_Id_Left_Hand; }
97 
98  void set_force_id_left_hand(Index anId) { m_Force_Id_Left_Hand = anId; }
99 
100  Index get_force_id_right_hand() { return m_Force_Id_Right_Hand; }
101 
102  void set_force_id_right_hand(Index anId) { m_Force_Id_Right_Hand = anId; }
103 
104  Index get_force_id_left_foot() { return m_Force_Id_Left_Foot; }
105 
106  void set_force_id_left_foot(Index anId) { m_Force_Id_Left_Foot = anId; }
107 
108  Index get_force_id_right_foot() { return m_Force_Id_Right_Foot; }
109 
110  void set_force_id_right_foot(Index anId) { m_Force_Id_Right_Foot = anId; }
111 
112  void display(std::ostream &out) const;
113 
114 }; // struct ForceUtil
115 
119 
122 
125  void display(std::ostream &os) const;
126 };
127 
131  void display(std::ostream &os) const;
132 };
133 
135  public:
136  RobotUtil();
137 
140 
143 
146 
148  std::vector<Index> m_urdf_to_sot;
149 
151  std::size_t m_nbJoints;
152 
154  std::map<std::string, Index> m_name_to_id;
155 
157  std::map<Index, std::string> m_id_to_name;
158 
160  std::map<Index, JointLimits> m_limits_map;
161 
163  std::string m_imu_joint_name;
164 
168  void create_id_to_name_map();
169 
171  std::string m_urdf_filename;
172 
174 
179  const Index &get_id_from_name(const std::string &name);
180 
187  const std::string &get_name_from_id(Index id);
189 
191  void set_name_to_id(const std::string &jointName, const Index &jointId);
192 
194  void set_urdf_to_sot(const std::vector<Index> &urdf_to_sot);
195  void set_urdf_to_sot(const dynamicgraph::Vector &urdf_to_sot);
196 
198  void set_joint_limits_for_id(const Index &idx, const double &lq,
199  const double &uq);
200 
201  bool joints_urdf_to_sot(ConstRefVector q_urdf, RefVector q_sot);
202 
203  bool joints_sot_to_urdf(ConstRefVector q_sot, RefVector q_urdf);
204 
205  bool velocity_urdf_to_sot(ConstRefVector q_urdf, ConstRefVector v_urdf,
206  RefVector v_sot);
207 
208  bool velocity_sot_to_urdf(ConstRefVector q_urdf, ConstRefVector v_sot,
209  RefVector v_urdf);
210 
211  bool config_urdf_to_sot(ConstRefVector q_urdf, RefVector q_sot);
212  bool config_sot_to_urdf(ConstRefVector q_sot, RefVector q_urdf);
213 
214  bool base_urdf_to_sot(ConstRefVector q_urdf, RefVector q_sot);
215  bool base_sot_to_urdf(ConstRefVector q_sot, RefVector q_urdf);
216 
222  const JointLimits &get_joint_limits_from_id(Index id);
223  JointLimits cp_get_joint_limits_from_id(Index id);
224 
227  void sendMsg(const std::string &msg, MsgType t = MSG_TYPE_INFO,
230  const std::string &lineId = "");
231 
233  void setLoggerVerbosityLevel(LoggerVerbosity lv) { logger_.setVerbosity(lv); }
234 
236  LoggerVerbosity getLoggerVerbosityLevel() { return logger_.getVerbosity(); };
237 
238  void display(std::ostream &os) const;
239 
245  template <typename Type>
246  void set_parameter(const std::string &parameter_name,
247  const Type &parameter_value) {
248  try {
249  typedef boost::property_tree::ptree::path_type path;
250  path apath(parameter_name, '/');
251  property_tree_.put<Type>(apath, parameter_value);
252  } catch (const boost::property_tree::ptree_error &e) {
253  std::ostringstream oss;
254  oss << "Robot utils: parameter path is invalid " << '\n'
255  << " for set_parameter(" << parameter_name << ")\n"
256  << e.what() << std::endl;
257  sendMsg(oss.str(), MSG_TYPE_ERROR);
258  return;
259  }
260  }
261 
268  template <typename Type>
269  Type get_parameter(const std::string &parameter_name) {
270  try {
271  boost::property_tree::ptree::path_type apath(parameter_name, '/');
272  const Type &res = property_tree_.get<Type>(apath);
273 
274  return res;
275  } catch (const boost::property_tree::ptree_error &e) {
276  std::ostringstream oss;
277  oss << "Robot utils: parameter path is invalid " << '\n'
278  << " for get_parameter(" << parameter_name << ")\n"
279  << e.what() << std::endl;
280  sendMsg(oss.str(), MSG_TYPE_ERROR);
281  throw;
282  }
283  }
287  boost::property_tree::ptree &get_property_tree();
288 
289  protected:
291 
293  std::map<std::string, std::string> parameters_strings_;
294 
296  boost::property_tree::ptree property_tree_;
297 }; // struct RobotUtil
298 
300 typedef std::shared_ptr<RobotUtil> RobotUtilShrPtr;
301 
303 RobotUtilShrPtr getRobotUtil(std::string &robotName);
304 bool isNameInRobotUtil(std::string &robotName);
305 RobotUtilShrPtr createRobotUtil(std::string &robotName);
306 std::shared_ptr<std::vector<std::string> > getListOfRobots();
307 
309 
310 } // namespace sot
311 } // namespace dynamicgraph
312 
313 #endif // sot_torque_control_common_h_
dynamicgraph::sot::ForceUtil::get_force_id_left_hand
Index get_force_id_left_hand()
Definition: robot-utils.hh:96
dynamicgraph::sot::RobotUtil::m_id_to_name
std::map< Index, std::string > m_id_to_name
The map between id and name.
Definition: robot-utils.hh:157
dynamicgraph::Logger
dynamicgraph::sot::RobotUtil::m_urdf_to_sot
std::vector< Index > m_urdf_to_sot
Map from the urdf index to the SoT index.
Definition: robot-utils.hh:148
dynamicgraph::sot::base_sot_to_urdf
bool base_sot_to_urdf(ConstRefVector q_sot, RefVector q_urdf)
Definition: robot-utils.cpp:483
SOT_CORE_EXPORT
#define SOT_CORE_EXPORT
Definition: api.hh:20
dynamicgraph::sot::ForceLimits::upper
Eigen::VectorXd upper
Definition: robot-utils.hh:61
dynamicgraph::LoggerVerbosity
LoggerVerbosity
dynamicgraph::sot::createRobotUtil
RobotUtilShrPtr createRobotUtil(std::string &robotName)
Definition: robot-utils.cpp:536
dynamicgraph::sot::RobotUtil::m_nbJoints
std::size_t m_nbJoints
Nb of Dofs for the robot.
Definition: robot-utils.hh:151
dynamicgraph::sot::base_se3_to_sot
bool base_se3_to_sot(ConstRefVector pos, ConstRefMatrix R, RefVector q_sot)
Definition: robot-utils.cpp:445
dynamicgraph
dynamicgraph::sot::ForceUtil::get_force_id_right_foot
Index get_force_id_right_foot()
Definition: robot-utils.hh:108
dynamicgraph::sot::RobotUtil::property_tree_
boost::property_tree::ptree property_tree_
Property tree.
Definition: robot-utils.hh:296
dynamicgraph::sot::RobotUtil::get_parameter
Type get_parameter(const std::string &parameter_name)
Get a parameter of type string. If parameter_name already exists the value is overwritten....
Definition: robot-utils.hh:269
dynamicgraph::sot::Index
Eigen::VectorXd::Index Index
Definition: robot-utils.hh:39
dynamicgraph::sot::ForceUtil::m_Force_Id_Right_Foot
Index m_Force_Id_Right_Foot
Definition: robot-utils.hh:78
dynamicgraph::sot::FootUtil::m_Right_Foot_Frame_Name
std::string m_Right_Foot_Frame_Name
Definition: robot-utils.hh:124
dynamicgraph::sot::ConstRefMatrix
const typedef Eigen::Ref< const Eigen::MatrixXd > ConstRefMatrix
Definition: matrix-geometry.hh:72
dynamicgraph::sot::ForceUtil
Definition: robot-utils.hh:72
dynamicgraph::sot::RobotUtil::setLoggerVerbosityLevel
void setLoggerVerbosityLevel(LoggerVerbosity lv)
Specify the verbosity level of the logger.
Definition: robot-utils.hh:233
dynamicgraph::sot::ExtractJointMimics::mimic_joints_
std::vector< std::string > mimic_joints_
Definition: robot-utils.hh:55
dynamicgraph::sot::ForceUtil::get_force_id_right_hand
Index get_force_id_right_hand()
Definition: robot-utils.hh:100
logger.h
dynamicgraph::sot::RobotUtil::parameters_strings_
std::map< std::string, std::string > parameters_strings_
Map of the parameters: map of strings.
Definition: robot-utils.hh:293
dynamicgraph::sot::ExtractJointMimics::tree_
boost::property_tree::ptree tree_
Definition: robot-utils.hh:54
dynamicgraph::sot::JointLimits::upper
double upper
Definition: robot-utils.hh:31
dynamicgraph::sot::RobotUtil::m_dgv_urdf_to_sot
dynamicgraph::Vector m_dgv_urdf_to_sot
Definition: robot-utils.hh:173
dynamicgraph::MSG_TYPE_INFO
MSG_TYPE_INFO
dynamicgraph::sot::getListOfRobots
std::shared_ptr< std::vector< std::string > > getListOfRobots()
Definition: robot-utils.cpp:508
dynamicgraph::MSG_TYPE_ERROR
MSG_TYPE_ERROR
dynamicgraph::sot::RobotUtil::set_parameter
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....
Definition: robot-utils.hh:246
dynamicgraph::sot::ConstRefVector
const typedef Eigen::Ref< const Eigen::VectorXd > & ConstRefVector
Definition: matrix-geometry.hh:70
dynamicgraph::sot::ForceUtil::get_force_id_left_foot
Index get_force_id_left_foot()
Definition: robot-utils.hh:104
res
res
dynamicgraph::sot::RobotUtilShrPtr
std::shared_ptr< RobotUtil > RobotUtilShrPtr
Accessors - This should be changed to RobotUtilPtrShared.
Definition: robot-utils.hh:300
dynamicgraph::sot::RobotUtil::getLoggerVerbosityLevel
LoggerVerbosity getLoggerVerbosityLevel()
Get the logger's verbosity level.
Definition: robot-utils.hh:236
dynamicgraph::sot::ForceUtil::set_force_id_left_hand
void set_force_id_left_hand(Index anId)
Definition: robot-utils.hh:98
dynamicgraph::sot::RobotUtil::m_limits_map
std::map< Index, JointLimits > m_limits_map
The joint limits map.
Definition: robot-utils.hh:160
dynamicgraph::sot::FootUtil
Definition: robot-utils.hh:116
dynamicgraph::sot::RobotUtil::logger_
Logger logger_
Definition: robot-utils.hh:290
dynamicgraph::sot::ForceLimits::ForceLimits
ForceLimits(const Eigen::VectorXd &l, const Eigen::VectorXd &u)
Definition: robot-utils.hh:66
dynamicgraph::sot::RobotUtil::m_force_util
ForceUtil m_force_util
Forces data.
Definition: robot-utils.hh:139
dynamicgraph::sot::ForceUtil::set_force_id_left_foot
void set_force_id_left_foot(Index anId)
Definition: robot-utils.hh:106
dynamicgraph::sot::ForceLimits::ForceLimits
ForceLimits()
Definition: robot-utils.hh:64
u
u
dynamicgraph::sot::RefVoidRobotUtil
RobotUtilShrPtr RefVoidRobotUtil()
Definition: robot-utils.cpp:44
dynamicgraph::sot::JointLimits::JointLimits
JointLimits()
Definition: robot-utils.hh:34
dynamicgraph::sot::ForceUtil::m_force_id_to_limits
std::map< Index, ForceLimits > m_force_id_to_limits
Definition: robot-utils.hh:73
dynamicgraph::sot::JointLimits
Definition: robot-utils.hh:30
dynamicgraph::sot::HandUtil
Definition: robot-utils.hh:128
dynamicgraph::size_type
Matrix::Index size_type
display
path
path
dynamicgraph::Vector
Eigen::VectorXd Vector
dynamicgraph::sot::ExtractJointMimics
Definition: robot-utils.hh:41
dynamicgraph::sot::HandUtil::m_Right_Hand_Frame_Name
std::string m_Right_Hand_Frame_Name
Definition: robot-utils.hh:130
dynamicgraph::MsgType
MsgType
dynamicgraph::sot::JointLimits::JointLimits
JointLimits(double l, double u)
Definition: robot-utils.hh:36
linear-algebra.h
dynamicgraph::sot::ForceUtil::m_force_id_to_name
std::map< Index, std::string > m_force_id_to_name
Definition: robot-utils.hh:75
dynamicgraph::sot::FootUtil::m_Right_Foot_Force_Sensor_XYZ
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:121
signal-helper.h
matrix-geometry.hh
dynamicgraph::sot::RefVector
Eigen::Ref< Eigen::VectorXd > RefVector
Definition: matrix-geometry.hh:69
dynamicgraph::sot::RobotUtil::m_hand_util
HandUtil m_hand_util
Hand information.
Definition: robot-utils.hh:145
dynamicgraph::sot::ForceUtil::m_Force_Id_Right_Hand
Index m_Force_Id_Right_Hand
Definition: robot-utils.hh:77
dynamicgraph::sot::getRobotUtil
RobotUtilShrPtr getRobotUtil(std::string &robotName)
Definition: robot-utils.cpp:522
dynamicgraph::sot::RobotUtil
Definition: robot-utils.hh:134
dynamicgraph::sot::FootUtil::m_Left_Foot_Frame_Name
std::string m_Left_Foot_Frame_Name
Definition: robot-utils.hh:123
dynamicgraph::sot::JointLimits::lower
double lower
Definition: robot-utils.hh:32
dynamicgraph::sot::ForceUtil::m_name_to_force_id
std::map< std::string, Index > m_name_to_force_id
Definition: robot-utils.hh:74
dynamicgraph::sot::ForceUtil::set_force_id_right_hand
void set_force_id_right_hand(Index anId)
Definition: robot-utils.hh:102
dynamicgraph::sot::base_urdf_to_sot
bool base_urdf_to_sot(ConstRefVector q_urdf, RefVector q_sot)
Definition: robot-utils.cpp:471
dynamicgraph::sot::RobotUtil::m_imu_joint_name
std::string m_imu_joint_name
The name of the joint IMU is attached to.
Definition: robot-utils.hh:163
dynamicgraph::sot::RobotUtil::m_urdf_filename
std::string m_urdf_filename
URDF file path.
Definition: robot-utils.hh:171
t
Transform3f t
dynamicgraph::sot::RobotUtil::m_name_to_id
std::map< std::string, Index > m_name_to_id
Map from the name to the id.
Definition: robot-utils.hh:154
dynamicgraph::sot::HandUtil::m_Left_Hand_Frame_Name
std::string m_Left_Hand_Frame_Name
Definition: robot-utils.hh:129
Type
Eigen::Block< Mat > Type
dynamicgraph::sot::ForceUtil::set_force_id_right_foot
void set_force_id_right_foot(Index anId)
Definition: robot-utils.hh:110
dynamicgraph::sot::ForceLimits::lower
Eigen::VectorXd lower
Definition: robot-utils.hh:62
dynamicgraph::sot::RobotUtil::m_foot_util
FootUtil m_foot_util
Foot information.
Definition: robot-utils.hh:142
dynamicgraph::sot::ForceLimits
Definition: robot-utils.hh:60
dynamicgraph::sot::ExtractJointMimics::current_joint_name_
std::string current_joint_name_
Definition: robot-utils.hh:56
compile.name
name
Definition: compile.py:23
dynamicgraph::sot::FootUtil::m_Right_Foot_Sole_XYZ
dynamicgraph::Vector m_Right_Foot_Sole_XYZ
Position of the foot soles w.r.t. the frame of the foot.
Definition: robot-utils.hh:118
dynamicgraph::sot::isNameInRobotUtil
bool isNameInRobotUtil(std::string &robotName)
Definition: robot-utils.cpp:529


sot-core
Author(s): Olivier Stasse, ostasse@laas.fr
autogenerated on Tue Oct 24 2023 02:26:31