sot-core  4.10.0
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 #include "boost/assign.hpp"
15 #include <dynamic-graph/linear-algebra.h>
16 #include <dynamic-graph/logger.h>
17 #include <dynamic-graph/signal-helper.h>
18 #include <map>
20 
21 namespace dynamicgraph {
22 namespace sot {
23 
25  double upper;
26  double lower;
27 
28  JointLimits() : upper(0.0), lower(0.0) {}
29 
30  JointLimits(double l, double u) : upper(u), lower(l) {}
31 };
32 
34 
36  Eigen::VectorXd upper;
37  Eigen::VectorXd lower;
38 
39  ForceLimits() : upper(Vector6d::Zero()), lower(Vector6d::Zero()) {}
40 
41  ForceLimits(const Eigen::VectorXd &l, const Eigen::VectorXd &u)
42  : upper(u), lower(l) {}
43 
44  void display(std::ostream &os) const;
45 };
46 
48  std::map<Index, ForceLimits> m_force_id_to_limits;
49  std::map<std::string, Index> m_name_to_force_id;
50  std::map<Index, std::string> m_force_id_to_name;
51 
52  Index m_Force_Id_Left_Hand, m_Force_Id_Right_Hand, m_Force_Id_Left_Foot,
54 
55  void set_name_to_force_id(const std::string &name, const Index &force_id);
56 
57  void set_force_id_to_limits(const Index &force_id,
58  const dynamicgraph::Vector &lf,
59  const dynamicgraph::Vector &uf);
60 
61  void create_force_id_to_name_map();
62 
63  Index get_id_from_name(const std::string &name);
64 
65  const std::string &get_name_from_id(Index idx);
66  std::string cp_get_name_from_id(Index idx);
67 
68  const ForceLimits &get_limits_from_id(Index force_id);
69  ForceLimits cp_get_limits_from_id(Index force_id);
70 
71  Index get_force_id_left_hand() { return m_Force_Id_Left_Hand; }
72 
73  void set_force_id_left_hand(Index anId) { m_Force_Id_Left_Hand = anId; }
74 
75  Index get_force_id_right_hand() { return m_Force_Id_Right_Hand; }
76 
77  void set_force_id_right_hand(Index anId) { m_Force_Id_Right_Hand = anId; }
78 
79  Index get_force_id_left_foot() { return m_Force_Id_Left_Foot; }
80 
81  void set_force_id_left_foot(Index anId) { m_Force_Id_Left_Foot = anId; }
82 
83  Index get_force_id_right_foot() { return m_Force_Id_Right_Foot; }
84 
85  void set_force_id_right_foot(Index anId) { m_Force_Id_Right_Foot = anId; }
86 
87  void display(std::ostream &out) const;
88 
89 }; // struct ForceUtil
90 
93  dynamicgraph::Vector m_Right_Foot_Sole_XYZ;
94 
96  dynamicgraph::Vector m_Right_Foot_Force_Sensor_XYZ;
97 
100  void display(std::ostream &os) const;
101 };
102 
106  void display(std::ostream &os) const;
107 };
108 
110 public:
111  RobotUtil();
112 
115 
118 
121 
123  std::vector<Index> m_urdf_to_sot;
124 
126  std::size_t m_nbJoints;
127 
129  std::map<std::string, Index> m_name_to_id;
130 
132  std::map<Index, std::string> m_id_to_name;
133 
135  std::map<Index, JointLimits> m_limits_map;
136 
138  std::string m_imu_joint_name;
139 
143  void create_id_to_name_map();
144 
146  std::string m_urdf_filename;
147 
148  dynamicgraph::Vector m_dgv_urdf_to_sot;
149 
154  const Index &get_id_from_name(const std::string &name);
155 
162  const std::string &get_name_from_id(Index id);
164 
166  void set_name_to_id(const std::string &jointName, const Index &jointId);
167 
169  void set_urdf_to_sot(const std::vector<Index> &urdf_to_sot);
170  void set_urdf_to_sot(const dynamicgraph::Vector &urdf_to_sot);
171 
173  void set_joint_limits_for_id(const Index &idx, const double &lq,
174  const double &uq);
175 
176  bool joints_urdf_to_sot(ConstRefVector q_urdf, RefVector q_sot);
177 
178  bool joints_sot_to_urdf(ConstRefVector q_sot, RefVector q_urdf);
179 
180  bool velocity_urdf_to_sot(ConstRefVector q_urdf, ConstRefVector v_urdf,
181  RefVector v_sot);
182 
183  bool velocity_sot_to_urdf(ConstRefVector q_urdf, ConstRefVector v_sot,
184  RefVector v_urdf);
185 
186  bool config_urdf_to_sot(ConstRefVector q_urdf, RefVector q_sot);
187  bool config_sot_to_urdf(ConstRefVector q_sot, RefVector q_urdf);
188 
189  bool base_urdf_to_sot(ConstRefVector q_urdf, RefVector q_sot);
190  bool base_sot_to_urdf(ConstRefVector q_sot, RefVector q_urdf);
191 
197  const JointLimits &get_joint_limits_from_id(Index id);
198  JointLimits cp_get_joint_limits_from_id(Index id);
199 
202  void sendMsg(const std::string &msg, MsgType t = MSG_TYPE_INFO,
205  const std::string &lineId = "");
206 
208  void setLoggerVerbosityLevel(LoggerVerbosity lv) { logger_.setVerbosity(lv); }
209 
211  LoggerVerbosity getLoggerVerbosityLevel() { return logger_.getVerbosity(); };
212 
213  void display(std::ostream &os) const;
214 
220  void set_parameter(const std::string &parameter_name,
221  const std::string &parameter_value);
222 
229  const std::string &get_parameter(const std::string &parameter_name);
230 
232 protected:
233  Logger logger_;
234 
236  std::map<std::string, std::string> parameters_strings_;
237 
238 }; // struct RobotUtil
239 
241 typedef boost::shared_ptr<RobotUtil> RobotUtilShrPtr;
242 
244 RobotUtilShrPtr getRobotUtil(std::string &robotName);
245 bool isNameInRobotUtil(std::string &robotName);
246 RobotUtilShrPtr createRobotUtil(std::string &robotName);
247 std::shared_ptr<std::vector<std::string> > getListOfRobots();
248 
250 
251 } // namespace sot
252 } // namespace dynamicgraph
253 
254 #endif // sot_torque_control_common_h_
dynamicgraph::sot::ForceUtil::get_force_id_left_hand
Index get_force_id_left_hand()
Definition: robot-utils.hh:71
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:132
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:123
SOT_CORE_EXPORT
#define SOT_CORE_EXPORT
Definition: api.hh:20
dynamicgraph::sot::ForceLimits::upper
Eigen::VectorXd upper
Definition: robot-utils.hh:36
dynamicgraph::sot::RobotUtilShrPtr
boost::shared_ptr< RobotUtil > RobotUtilShrPtr
Accessors - This should be changed to RobotUtilPtrShared.
Definition: robot-utils.hh:241
dynamicgraph::sot::createRobotUtil
RobotUtilShrPtr createRobotUtil(std::string &robotName)
dynamicgraph::sot::RobotUtil::m_nbJoints
std::size_t m_nbJoints
Nb of Dofs for the robot.
Definition: robot-utils.hh:126
dynamicgraph::sot::base_se3_to_sot
bool base_se3_to_sot(ConstRefVector pos, ConstRefMatrix R, RefVector q_sot)
dynamicgraph
Definition: abstract-sot-external-interface.hh:17
dynamicgraph::sot::ForceUtil::get_force_id_right_foot
Index get_force_id_right_foot()
Definition: robot-utils.hh:83
dynamicgraph::sot::Index
Eigen::VectorXd::Index Index
Definition: robot-utils.hh:33
dynamicgraph::sot::ForceUtil::m_Force_Id_Right_Foot
Index m_Force_Id_Right_Foot
Definition: robot-utils.hh:53
dynamicgraph::sot::FootUtil::m_Right_Foot_Frame_Name
std::string m_Right_Foot_Frame_Name
Definition: robot-utils.hh:99
dynamicgraph::sot::ConstRefMatrix
const typedef Eigen::Ref< const Eigen::MatrixXd > ConstRefMatrix
Definition: matrix-geometry.hh:71
dynamicgraph::sot::ForceUtil
Definition: robot-utils.hh:47
dynamicgraph::sot::RobotUtil::setLoggerVerbosityLevel
void setLoggerVerbosityLevel(LoggerVerbosity lv)
Specify the verbosity level of the logger.
Definition: robot-utils.hh:208
dynamicgraph::sot::ForceUtil::get_force_id_right_hand
Index get_force_id_right_hand()
Definition: robot-utils.hh:75
dynamicgraph::sot::RobotUtil::parameters_strings_
std::map< std::string, std::string > parameters_strings_
Map of the parameters: map of strings.
Definition: robot-utils.hh:236
dynamicgraph::sot::JointLimits::upper
double upper
Definition: robot-utils.hh:25
dynamicgraph::sot::RobotUtil::m_dgv_urdf_to_sot
dynamicgraph::Vector m_dgv_urdf_to_sot
Definition: robot-utils.hh:148
dynamicgraph::sot::ConstRefVector
const typedef Eigen::Ref< const Eigen::VectorXd > & ConstRefVector
Definition: matrix-geometry.hh:69
dynamicgraph::sot::getListOfRobots
std::shared_ptr< std::vector< std::string > > getListOfRobots()
dynamicgraph::sot::ForceUtil::get_force_id_left_foot
Index get_force_id_left_foot()
Definition: robot-utils.hh:79
dynamicgraph::sot::RobotUtil::getLoggerVerbosityLevel
LoggerVerbosity getLoggerVerbosityLevel()
Get the logger's verbosity level.
Definition: robot-utils.hh:211
dynamicgraph::sot::ForceUtil::set_force_id_left_hand
void set_force_id_left_hand(Index anId)
Definition: robot-utils.hh:73
dynamicgraph::sot::RobotUtil::m_limits_map
std::map< Index, JointLimits > m_limits_map
The joint limits map.
Definition: robot-utils.hh:135
dynamicgraph::sot::FootUtil
Definition: robot-utils.hh:91
dynamicgraph::sot::RobotUtil::logger_
Logger logger_
Definition: robot-utils.hh:233
dynamicgraph::sot::ForceLimits::ForceLimits
ForceLimits(const Eigen::VectorXd &l, const Eigen::VectorXd &u)
Definition: robot-utils.hh:41
dynamicgraph::sot::RobotUtil::m_force_util
ForceUtil m_force_util
Forces data.
Definition: robot-utils.hh:114
dynamicgraph::sot::ForceUtil::set_force_id_left_foot
void set_force_id_left_foot(Index anId)
Definition: robot-utils.hh:81
dynamicgraph::sot::ForceLimits::ForceLimits
ForceLimits()
Definition: robot-utils.hh:39
dynamicgraph::sot::RefVoidRobotUtil
RobotUtilShrPtr RefVoidRobotUtil()
dynamicgraph::sot::JointLimits::JointLimits
JointLimits()
Definition: robot-utils.hh:28
dynamicgraph::sot::ForceUtil::m_force_id_to_limits
std::map< Index, ForceLimits > m_force_id_to_limits
Definition: robot-utils.hh:48
dynamicgraph::sot::JointLimits
Definition: robot-utils.hh:24
dynamicgraph::sot::HandUtil
Definition: robot-utils.hh:103
dynamicgraph::sot::HandUtil::m_Right_Hand_Frame_Name
std::string m_Right_Hand_Frame_Name
Definition: robot-utils.hh:105
dynamicgraph::sot::JointLimits::JointLimits
JointLimits(double l, double u)
Definition: robot-utils.hh:30
dynamicgraph::sot::ForceUtil::m_force_id_to_name
std::map< Index, std::string > m_force_id_to_name
Definition: robot-utils.hh:50
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:96
matrix-geometry.hh
dynamicgraph::sot::RefVector
Eigen::Ref< Eigen::VectorXd > RefVector
Definition: matrix-geometry.hh:68
dynamicgraph::sot::RobotUtil::m_hand_util
HandUtil m_hand_util
Hand information.
Definition: robot-utils.hh:120
dynamicgraph::sot::ForceUtil::m_Force_Id_Right_Hand
Index m_Force_Id_Right_Hand
Definition: robot-utils.hh:52
dynamicgraph::sot::getRobotUtil
RobotUtilShrPtr getRobotUtil(std::string &robotName)
dynamicgraph::sot::RobotUtil
Definition: robot-utils.hh:109
dynamicgraph::sot::FootUtil::m_Left_Foot_Frame_Name
std::string m_Left_Foot_Frame_Name
Definition: robot-utils.hh:98
dynamicgraph::sot::JointLimits::lower
double lower
Definition: robot-utils.hh:26
dynamicgraph::sot::ForceUtil::m_name_to_force_id
std::map< std::string, Index > m_name_to_force_id
Definition: robot-utils.hh:49
dynamicgraph::sot::ForceUtil::set_force_id_right_hand
void set_force_id_right_hand(Index anId)
Definition: robot-utils.hh:77
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:138
dynamicgraph::sot::RobotUtil::m_urdf_filename
std::string m_urdf_filename
URDF file path.
Definition: robot-utils.hh:146
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:129
dynamicgraph::sot::HandUtil::m_Left_Hand_Frame_Name
std::string m_Left_Hand_Frame_Name
Definition: robot-utils.hh:104
dynamicgraph::sot::ForceUtil::set_force_id_right_foot
void set_force_id_right_foot(Index anId)
Definition: robot-utils.hh:85
dynamicgraph::sot::ForceLimits::lower
Eigen::VectorXd lower
Definition: robot-utils.hh:37
dynamicgraph::sot::RobotUtil::m_foot_util
FootUtil m_foot_util
Foot information.
Definition: robot-utils.hh:117
dynamicgraph::sot::ForceLimits
Definition: robot-utils.hh:35
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:93
dynamicgraph::sot::isNameInRobotUtil
bool isNameInRobotUtil(std::string &robotName)