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 dg = ::dynamicgraph;
22 using namespace dg;
23 
24 namespace dynamicgraph {
25 namespace sot {
26 
28  double upper;
29  double lower;
30 
31  JointLimits() : upper(0.0), lower(0.0) {}
32 
33  JointLimits(double l, double u) : upper(u), lower(l) {}
34 };
35 
37 
39  Eigen::VectorXd upper;
40  Eigen::VectorXd lower;
41 
42  ForceLimits() : upper(Vector6d::Zero()), lower(Vector6d::Zero()) {}
43 
44  ForceLimits(const Eigen::VectorXd &l, const Eigen::VectorXd &u)
45  : upper(u), lower(l) {}
46 
47  void display(std::ostream &os) const;
48 };
49 
51  std::map<Index, ForceLimits> m_force_id_to_limits;
52  std::map<std::string, Index> m_name_to_force_id;
53  std::map<Index, std::string> m_force_id_to_name;
54 
55  Index m_Force_Id_Left_Hand, m_Force_Id_Right_Hand, m_Force_Id_Left_Foot,
56  m_Force_Id_Right_Foot;
57 
58  void set_name_to_force_id(const std::string &name, const Index &force_id);
59 
60  void set_force_id_to_limits(const Index &force_id, const dg::Vector &lf,
61  const dg::Vector &uf);
62 
63  void create_force_id_to_name_map();
64 
65  Index get_id_from_name(const std::string &name);
66 
67  const std::string &get_name_from_id(Index idx);
68  std::string cp_get_name_from_id(Index idx);
69 
70  const ForceLimits &get_limits_from_id(Index force_id);
71  ForceLimits cp_get_limits_from_id(Index force_id);
72 
73  Index get_force_id_left_hand() { return m_Force_Id_Left_Hand; }
74 
75  void set_force_id_left_hand(Index anId) { m_Force_Id_Left_Hand = anId; }
76 
77  Index get_force_id_right_hand() { return m_Force_Id_Right_Hand; }
78 
79  void set_force_id_right_hand(Index anId) { m_Force_Id_Right_Hand = anId; }
80 
81  Index get_force_id_left_foot() { return m_Force_Id_Left_Foot; }
82 
83  void set_force_id_left_foot(Index anId) { m_Force_Id_Left_Foot = anId; }
84 
85  Index get_force_id_right_foot() { return m_Force_Id_Right_Foot; }
86 
87  void set_force_id_right_foot(Index anId) { m_Force_Id_Right_Foot = anId; }
88 
89  void display(std::ostream &out) const;
90 
91 }; // struct ForceUtil
92 
95  dynamicgraph::Vector m_Right_Foot_Sole_XYZ;
96 
98  dynamicgraph::Vector m_Right_Foot_Force_Sensor_XYZ;
99 
102  void display(std::ostream &os) const;
103 };
104 
108  void display(std::ostream &os) const;
109 };
110 
112 public:
113  RobotUtil();
114 
117 
120 
123 
125  std::vector<Index> m_urdf_to_sot;
126 
128  std::size_t m_nbJoints;
129 
131  std::map<std::string, Index> m_name_to_id;
132 
134  std::map<Index, std::string> m_id_to_name;
135 
137  std::map<Index, JointLimits> m_limits_map;
138 
140  std::string m_imu_joint_name;
141 
145  void create_id_to_name_map();
146 
148  std::string m_urdf_filename;
149 
150  dynamicgraph::Vector m_dgv_urdf_to_sot;
151 
156  const Index &get_id_from_name(const std::string &name);
157 
164  const std::string &get_name_from_id(Index id);
166 
168  void set_name_to_id(const std::string &jointName, const Index &jointId);
169 
171  void set_urdf_to_sot(const std::vector<Index> &urdf_to_sot);
172  void set_urdf_to_sot(const dg::Vector &urdf_to_sot);
173 
175  void set_joint_limits_for_id(const Index &idx, const double &lq,
176  const double &uq);
177 
178  bool joints_urdf_to_sot(ConstRefVector q_urdf, RefVector q_sot);
179 
180  bool joints_sot_to_urdf(ConstRefVector q_sot, RefVector q_urdf);
181 
182  bool velocity_urdf_to_sot(ConstRefVector q_urdf, ConstRefVector v_urdf,
183  RefVector v_sot);
184 
185  bool velocity_sot_to_urdf(ConstRefVector q_urdf, ConstRefVector v_sot,
186  RefVector v_urdf);
187 
188  bool config_urdf_to_sot(ConstRefVector q_urdf, RefVector q_sot);
189  bool config_sot_to_urdf(ConstRefVector q_sot, RefVector q_urdf);
190 
191  bool base_urdf_to_sot(ConstRefVector q_urdf, RefVector q_sot);
192  bool base_sot_to_urdf(ConstRefVector q_sot, RefVector q_urdf);
193 
199  const JointLimits &get_joint_limits_from_id(Index id);
200  JointLimits cp_get_joint_limits_from_id(Index id);
201 
204  void sendMsg(const std::string &msg, MsgType t = MSG_TYPE_INFO,
207  const std::string &lineId = "");
208 
210  void setLoggerVerbosityLevel(LoggerVerbosity lv) { logger_.setVerbosity(lv); }
211 
213  LoggerVerbosity getLoggerVerbosityLevel() { return logger_.getVerbosity(); };
214 
215  void display(std::ostream &os) const;
216 
222  void set_parameter(const std::string &parameter_name,
223  const std::string &parameter_value);
224 
231  const std::string &get_parameter(const std::string &parameter_name);
232 
234 protected:
235  Logger logger_;
236 
238  std::map<std::string, std::string> parameters_strings_;
239 
240 }; // struct RobotUtil
241 
243 typedef boost::shared_ptr<RobotUtil> RobotUtilShrPtr;
244 
245 RobotUtilShrPtr RefVoidRobotUtil();
246 RobotUtilShrPtr getRobotUtil(std::string &robotName);
247 bool isNameInRobotUtil(std::string &robotName);
248 RobotUtilShrPtr createRobotUtil(std::string &robotName);
249 
251 
252 } // namespace sot
253 } // namespace dynamicgraph
254 
255 #endif // sot_torque_control_common_h_
JointLimits(double l, double u)
Definition: robot-utils.hh:33
Definition: robot-utils.hh:50
std::map< Index, JointLimits > m_limits_map
The joint limits map.
Definition: robot-utils.hh:137
Definition: robot-utils.hh:93
const Eigen::Ref< const Eigen::VectorXd > & ConstRefVector
Definition: matrix-geometry.hh:69
Index m_Force_Id_Right_Hand
Definition: robot-utils.hh:55
Definition: robot-utils.hh:111
ForceUtil m_force_util
Forces data.
Definition: robot-utils.hh:116
void set_force_id_right_hand(Index anId)
Definition: robot-utils.hh:79
bool base_se3_to_sot(ConstRefVector pos, ConstRefMatrix R, RefVector q_sot)
Eigen::VectorXd::Index Index
Definition: robot-utils.hh:36
Definition: robot-utils.hh:105
RobotUtilShrPtr createRobotUtil(std::string &robotName)
double lower
Definition: robot-utils.hh:29
std::string m_Left_Hand_Frame_Name
Definition: robot-utils.hh:106
boost::shared_ptr< RobotUtil > RobotUtilShrPtr
Accessors - This should be changed to RobotUtilPtrShared.
Definition: robot-utils.hh:243
Eigen::VectorXd upper
Definition: robot-utils.hh:39
void set_force_id_right_foot(Index anId)
Definition: robot-utils.hh:87
dynamicgraph::Vector m_Right_Foot_Sole_XYZ
Position of the foot soles w.r.t. the frame of the foot.
Definition: robot-utils.hh:95
std::map< std::string, Index > m_name_to_force_id
Definition: robot-utils.hh:52
dynamicgraph::Vector m_dgv_urdf_to_sot
Definition: robot-utils.hh:150
Index get_force_id_right_foot()
Definition: robot-utils.hh:85
#define SOT_CORE_EXPORT
Definition: api.hh:20
void set_force_id_left_foot(Index anId)
Definition: robot-utils.hh:83
JointLimits()
Definition: robot-utils.hh:31
ForceLimits()
Definition: robot-utils.hh:42
void setLoggerVerbosityLevel(LoggerVerbosity lv)
Specify the verbosity level of the logger.
Definition: robot-utils.hh:210
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:140
std::map< Index, ForceLimits > m_force_id_to_limits
Definition: robot-utils.hh:51
Eigen::VectorXd lower
Definition: robot-utils.hh:40
Definition: robot-utils.hh:38
ForceLimits(const Eigen::VectorXd &l, const Eigen::VectorXd &u)
Definition: robot-utils.hh:44
double upper
Definition: robot-utils.hh:28
std::size_t m_nbJoints
Nb of Dofs for the robot.
Definition: robot-utils.hh:128
void set_force_id_left_hand(Index anId)
Definition: robot-utils.hh:75
RobotUtilShrPtr RefVoidRobotUtil()
LoggerVerbosity getLoggerVerbosityLevel()
Get the logger&#39;s verbosity level.
Definition: robot-utils.hh:213
Index get_force_id_right_hand()
Definition: robot-utils.hh:77
std::vector< Index > m_urdf_to_sot
Map from the urdf index to the SoT index.
Definition: robot-utils.hh:125
std::string m_Right_Hand_Frame_Name
Definition: robot-utils.hh:107
RobotUtilShrPtr getRobotUtil(std::string &robotName)
FootUtil m_foot_util
Foot information.
Definition: robot-utils.hh:119
Index get_force_id_left_hand()
Definition: robot-utils.hh:73
std::string m_Left_Foot_Frame_Name
Definition: robot-utils.hh:100
std::map< std::string, std::string > parameters_strings_
Map of the parameters: map of strings.
Definition: robot-utils.hh:238
std::map< std::string, Index > m_name_to_id
Map from the name to the id.
Definition: robot-utils.hh:131
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:98
HandUtil m_hand_util
Hand information.
Definition: robot-utils.hh:122
Eigen::Ref< Eigen::VectorXd > RefVector
Definition: matrix-geometry.hh:68
Logger logger_
Definition: robot-utils.hh:235
std::map< Index, std::string > m_force_id_to_name
Definition: robot-utils.hh:53
Index get_force_id_left_foot()
Definition: robot-utils.hh:81
Definition: robot-utils.hh:27
bool isNameInRobotUtil(std::string &robotName)
std::string m_Right_Foot_Frame_Name
Definition: robot-utils.hh:101
std::map< Index, std::string > m_id_to_name
The map between id and name.
Definition: robot-utils.hh:134
Definition: abstract-sot-external-interface.hh:17
std::string m_urdf_filename
URDF file path.
Definition: robot-utils.hh:148