Directory: | ./ |
---|---|
File: | include/sot/core/robot-utils.hh |
Date: | 2024-08-13 12:13:25 |
Exec | Total | Coverage | |
---|---|---|---|
Lines: | 16 | 30 | 53.3% |
Branches: | 5 | 36 | 13.9% |
Line | Branch | Exec | Source |
---|---|---|---|
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 | |||
19 | #include <dynamic-graph/linear-algebra.h> | ||
20 | #include <dynamic-graph/logger.h> | ||
21 | #include <dynamic-graph/signal-helper.h> | ||
22 | |||
23 | #include <boost/assign.hpp> | ||
24 | #include <boost/property_tree/ptree.hpp> | ||
25 | #include <sot/core/matrix-geometry.hh> | ||
26 | |||
27 | namespace dynamicgraph { | ||
28 | namespace sot { | ||
29 | |||
30 | struct SOT_CORE_EXPORT JointLimits { | ||
31 | double upper; | ||
32 | double lower; | ||
33 | |||
34 | 24 | JointLimits() : upper(0.0), lower(0.0) {} | |
35 | |||
36 | 1 | JointLimits(double l, double u) : upper(u), lower(l) {} | |
37 | }; | ||
38 | |||
39 | typedef Eigen::VectorXd::Index Index; | ||
40 | |||
41 | class SOT_CORE_EXPORT ExtractJointMimics { | ||
42 | public: | ||
43 | /// Constructor | ||
44 | ExtractJointMimics(std::string &robot_model); | ||
45 | |||
46 | /// Get mimic joints. | ||
47 | const std::vector<std::string> &get_mimic_joints(); | ||
48 | |||
49 | private: | ||
50 | void go_through(boost::property_tree::ptree &pt, int level, int stage); | ||
51 | |||
52 | // Create empty property tree object | ||
53 | boost::property_tree::ptree tree_; | ||
54 | std::vector<std::string> mimic_joints_; | ||
55 | std::string current_joint_name_; | ||
56 | void go_through_full(); | ||
57 | }; | ||
58 | |||
59 | struct SOT_CORE_EXPORT ForceLimits { | ||
60 | Eigen::VectorXd upper; | ||
61 | Eigen::VectorXd lower; | ||
62 | |||
63 |
3/6✓ Branch 2 taken 24 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 24 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 24 times.
✗ Branch 9 not taken.
|
24 | ForceLimits() : upper(Vector6d::Zero()), lower(Vector6d::Zero()) {} |
64 | |||
65 | ✗ | ForceLimits(const Eigen::VectorXd &l, const Eigen::VectorXd &u) | |
66 | ✗ | : upper(u), lower(l) {} | |
67 | |||
68 | void display(std::ostream &os) const; | ||
69 | }; | ||
70 | |||
71 | struct SOT_CORE_EXPORT ForceUtil { | ||
72 | std::map<Index, ForceLimits> m_force_id_to_limits; | ||
73 | std::map<std::string, Index> m_name_to_force_id; | ||
74 | std::map<Index, std::string> m_force_id_to_name; | ||
75 | |||
76 | Index m_Force_Id_Left_Hand, m_Force_Id_Right_Hand, m_Force_Id_Left_Foot, | ||
77 | m_Force_Id_Right_Foot; | ||
78 | |||
79 | void set_name_to_force_id(const std::string &name, const Index &force_id); | ||
80 | |||
81 | void set_force_id_to_limits(const Index &force_id, | ||
82 | const dynamicgraph::Vector &lf, | ||
83 | const dynamicgraph::Vector &uf); | ||
84 | |||
85 | void create_force_id_to_name_map(); | ||
86 | |||
87 | Index get_id_from_name(const std::string &name); | ||
88 | |||
89 | const std::string &get_name_from_id(Index idx); | ||
90 | std::string cp_get_name_from_id(Index idx); | ||
91 | |||
92 | const ForceLimits &get_limits_from_id(Index force_id); | ||
93 | ForceLimits cp_get_limits_from_id(Index force_id); | ||
94 | |||
95 | ✗ | Index get_force_id_left_hand() { return m_Force_Id_Left_Hand; } | |
96 | |||
97 | 1 | void set_force_id_left_hand(Index anId) { m_Force_Id_Left_Hand = anId; } | |
98 | |||
99 | ✗ | Index get_force_id_right_hand() { return m_Force_Id_Right_Hand; } | |
100 | |||
101 | 1 | void set_force_id_right_hand(Index anId) { m_Force_Id_Right_Hand = anId; } | |
102 | |||
103 | ✗ | Index get_force_id_left_foot() { return m_Force_Id_Left_Foot; } | |
104 | |||
105 | 1 | void set_force_id_left_foot(Index anId) { m_Force_Id_Left_Foot = anId; } | |
106 | |||
107 | ✗ | Index get_force_id_right_foot() { return m_Force_Id_Right_Foot; } | |
108 | |||
109 | 1 | void set_force_id_right_foot(Index anId) { m_Force_Id_Right_Foot = anId; } | |
110 | |||
111 | void display(std::ostream &out) const; | ||
112 | |||
113 | }; // struct ForceUtil | ||
114 | |||
115 | struct SOT_CORE_EXPORT FootUtil { | ||
116 | /// Position of the foot soles w.r.t. the frame of the foot | ||
117 | dynamicgraph::Vector m_Right_Foot_Sole_XYZ; | ||
118 | |||
119 | /// Position of the force/torque sensors w.r.t. the frame of the hosting link | ||
120 | dynamicgraph::Vector m_Right_Foot_Force_Sensor_XYZ; | ||
121 | |||
122 | std::string m_Left_Foot_Frame_Name; | ||
123 | std::string m_Right_Foot_Frame_Name; | ||
124 | void display(std::ostream &os) const; | ||
125 | }; | ||
126 | |||
127 | struct SOT_CORE_EXPORT HandUtil { | ||
128 | std::string m_Left_Hand_Frame_Name; | ||
129 | std::string m_Right_Hand_Frame_Name; | ||
130 | void display(std::ostream &os) const; | ||
131 | }; | ||
132 | |||
133 | struct SOT_CORE_EXPORT RobotUtil { | ||
134 | public: | ||
135 | RobotUtil(); | ||
136 | |||
137 | /// Forces data | ||
138 | ForceUtil m_force_util; | ||
139 | |||
140 | /// Foot information | ||
141 | FootUtil m_foot_util; | ||
142 | |||
143 | /// Hand information | ||
144 | HandUtil m_hand_util; | ||
145 | |||
146 | /// Map from the urdf index to the SoT index. | ||
147 | std::vector<Index> m_urdf_to_sot; | ||
148 | |||
149 | /// Nb of Dofs for the robot. | ||
150 | std::size_t m_nbJoints; | ||
151 | |||
152 | /// Map from the name to the id. | ||
153 | std::map<std::string, Index> m_name_to_id; | ||
154 | |||
155 | /// The map between id and name | ||
156 | std::map<Index, std::string> m_id_to_name; | ||
157 | |||
158 | /// The joint limits map. | ||
159 | std::map<Index, JointLimits> m_limits_map; | ||
160 | |||
161 | /// The name of the joint IMU is attached to | ||
162 | std::string m_imu_joint_name; | ||
163 | |||
164 | /// This method creates the map between id and name. | ||
165 | /// It is called each time a new link between id and name is inserted | ||
166 | /// (i.e. when set_name_to_id is called). | ||
167 | void create_id_to_name_map(); | ||
168 | |||
169 | /// URDF file path | ||
170 | std::string m_urdf_filename; | ||
171 | |||
172 | dynamicgraph::Vector m_dgv_urdf_to_sot; | ||
173 | |||
174 | /** Given a joint name it finds the associated joint id. | ||
175 | * If the specified joint name is not found it returns -1; | ||
176 | * @param name Name of the joint to find. | ||
177 | * @return The id of the specified joint, -1 if not found. */ | ||
178 | const Index &get_id_from_name(const std::string &name); | ||
179 | |||
180 | /** Given a joint id it finds the associated joint name. | ||
181 | * If the specified joint is not found it returns "Joint name not found"; | ||
182 | * @param id Id of the joint to find. | ||
183 | * @return The name of the specified joint, "Joint name not found" if not | ||
184 | * found. */ | ||
185 | |||
186 | /// Get the joint name from its index | ||
187 | const std::string &get_name_from_id(Index id); | ||
188 | |||
189 | /// Set relation between the name and the SoT id | ||
190 | void set_name_to_id(const std::string &jointName, const Index &jointId); | ||
191 | |||
192 | /// Set the map between urdf index and sot index | ||
193 | void set_urdf_to_sot(const std::vector<Index> &urdf_to_sot); | ||
194 | void set_urdf_to_sot(const dynamicgraph::Vector &urdf_to_sot); | ||
195 | |||
196 | /// Set the limits (lq,uq) for joint idx | ||
197 | void set_joint_limits_for_id(const Index &idx, const double &lq, | ||
198 | const double &uq); | ||
199 | |||
200 | bool joints_urdf_to_sot(ConstRefVector q_urdf, RefVector q_sot); | ||
201 | |||
202 | bool joints_sot_to_urdf(ConstRefVector q_sot, RefVector q_urdf); | ||
203 | |||
204 | bool velocity_urdf_to_sot(ConstRefVector q_urdf, ConstRefVector v_urdf, | ||
205 | RefVector v_sot); | ||
206 | |||
207 | bool velocity_sot_to_urdf(ConstRefVector q_urdf, ConstRefVector v_sot, | ||
208 | RefVector v_urdf); | ||
209 | |||
210 | bool config_urdf_to_sot(ConstRefVector q_urdf, RefVector q_sot); | ||
211 | bool config_sot_to_urdf(ConstRefVector q_sot, RefVector q_urdf); | ||
212 | |||
213 | bool base_urdf_to_sot(ConstRefVector q_urdf, RefVector q_sot); | ||
214 | bool base_sot_to_urdf(ConstRefVector q_sot, RefVector q_urdf); | ||
215 | |||
216 | /** Given a joint id it finds the associated joint limits. | ||
217 | * If the specified joint is not found it returns JointLimits(0,0). | ||
218 | * @param id Id of the joint to find. | ||
219 | * @return The limits of the specified joint, JointLimits(0,0) if not found. | ||
220 | */ | ||
221 | const JointLimits &get_joint_limits_from_id(Index id); | ||
222 | JointLimits cp_get_joint_limits_from_id(Index id); | ||
223 | |||
224 | /** \name Logger related methods */ | ||
225 | /** \{*/ | ||
226 | /// \brief Send messages \c msg with level \c t. Add string \c file and \c | ||
227 | /// line to message. | ||
228 | void sendMsg(const std::string &msg, MsgType t = MSG_TYPE_INFO, | ||
229 | const std::string &lineId = ""); | ||
230 | |||
231 | /// \brief Specify the verbosity level of the logger. | ||
232 | void setLoggerVerbosityLevel(LoggerVerbosity lv) { logger_.setVerbosity(lv); } | ||
233 | |||
234 | /// \brief Get the logger's verbosity level. | ||
235 | LoggerVerbosity getLoggerVerbosityLevel() { return logger_.getVerbosity(); }; | ||
236 | |||
237 | void display(std::ostream &os) const; | ||
238 | |||
239 | /**{ \name Handling general parameters */ | ||
240 | /** \brief Set a parameter of type string. | ||
241 | If parameter_name already exists the value is overwritten. | ||
242 | If not it is inserted. | ||
243 | */ | ||
244 | template <typename Type> | ||
245 | 4 | void set_parameter(const std::string ¶meter_name, | |
246 | const Type ¶meter_value) { | ||
247 | try { | ||
248 | typedef boost::property_tree::ptree::path_type path; | ||
249 |
1/2✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
|
4 | path apath(parameter_name, '/'); |
250 |
1/2✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
|
4 | property_tree_.put<Type>(apath, parameter_value); |
251 |
0/2✗ Branch 2 not taken.
✗ Branch 3 not taken.
|
4 | } catch (const boost::property_tree::ptree_error &e) { |
252 | ✗ | std::ostringstream oss; | |
253 | oss << "Robot utils: parameter path is invalid " << '\n' | ||
254 | << " for set_parameter(" << parameter_name << ")\n" | ||
255 | ✗ | << e.what() << std::endl; | |
256 | ✗ | sendMsg(oss.str(), MSG_TYPE_ERROR); | |
257 | ✗ | return; | |
258 | } | ||
259 | } | ||
260 | |||
261 | /** \brief Get a parameter of type string. | ||
262 | If parameter_name already exists the value is overwritten. | ||
263 | If not it is inserted. | ||
264 | @param parameter_name: Name of the parameter | ||
265 | Return false if the parameter is not found. | ||
266 | */ | ||
267 | template <typename Type> | ||
268 | 2 | Type get_parameter(const std::string ¶meter_name) { | |
269 | try { | ||
270 | 2 | boost::property_tree::ptree::path_type apath(parameter_name, '/'); | |
271 | 2 | const Type &res = property_tree_.get<Type>(apath); | |
272 | |||
273 | 3 | return res; | |
274 | 2 | } catch (const boost::property_tree::ptree_error &e) { | |
275 | ✗ | std::ostringstream oss; | |
276 | oss << "Robot utils: parameter path is invalid " << '\n' | ||
277 | << " for get_parameter(" << parameter_name << ")\n" | ||
278 | ✗ | << e.what() << std::endl; | |
279 | ✗ | sendMsg(oss.str(), MSG_TYPE_ERROR); | |
280 | ✗ | throw; | |
281 | } | ||
282 | } | ||
283 | /** @} */ | ||
284 | |||
285 | /** Access to property tree directly */ | ||
286 | boost::property_tree::ptree &get_property_tree(); | ||
287 | |||
288 | protected: | ||
289 | Logger logger_; | ||
290 | |||
291 | /** \brief Map of the parameters: map of strings. */ | ||
292 | std::map<std::string, std::string> parameters_strings_; | ||
293 | |||
294 | /** \brief Property tree */ | ||
295 | boost::property_tree::ptree property_tree_; | ||
296 | }; // struct RobotUtil | ||
297 | |||
298 | /// Accessors - This should be changed to RobotUtilPtrShared | ||
299 | typedef std::shared_ptr<RobotUtil> RobotUtilShrPtr; | ||
300 | |||
301 | RobotUtilShrPtr RefVoidRobotUtil(); | ||
302 | RobotUtilShrPtr getRobotUtil(std::string &robotName); | ||
303 | bool isNameInRobotUtil(std::string &robotName); | ||
304 | RobotUtilShrPtr createRobotUtil(std::string &robotName); | ||
305 | std::shared_ptr<std::vector<std::string> > getListOfRobots(); | ||
306 | |||
307 | bool base_se3_to_sot(ConstRefVector pos, ConstRefMatrix R, RefVector q_sot); | ||
308 | |||
309 | } // namespace sot | ||
310 | } // namespace dynamicgraph | ||
311 | |||
312 | #endif // sot_torque_control_common_h_ | ||
313 |