GCC Code Coverage Report

Directory: ./
File: src/tools/robot-utils.cpp
Date: 2024-12-13 12:22:33
Exec Total Coverage
Lines: 237 330 71.8%
Branches: 207 556 37.2%
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 /** pinocchio is forcing the BOOST_MPL_LIMIT_VECTOR_SIZE to a specific value.
9 This happen to be not working when including the boost property_tree
10 library. For this reason if defined, the current value of
11 BOOST_MPL_LIMIT_VECTOR_SIZE is saved in the preprocessor stack and unset.
12 Once the property_tree included the pinocchio value of this variable is
13 restored.
14 */
15 #ifdef BOOST_MPL_LIMIT_VECTOR_SIZE
16 #pragma push_macro("BOOST_MPL_LIMIT_VECTOR_SIZE")
17 #undef BOOST_MPL_LIMIT_VECTOR_SIZE
18 #include <boost/property_tree/ptree.hpp>
19 #include <boost/property_tree/xml_parser.hpp>
20 #pragma pop_macro("BOOST_MPL_LIMIT_VECTOR_SIZE")
21 #else
22 #include <boost/property_tree/ptree.hpp>
23 #include <boost/property_tree/xml_parser.hpp>
24 #endif
25
26 #include <dynamic-graph/factory.h>
27
28 #include <iostream>
29 #include <sot/core/debug.hh>
30 #include <sot/core/robot-utils.hh>
31 #include <sstream>
32
33 namespace dynamicgraph {
34 namespace sot {
35 namespace dg = ::dynamicgraph;
36 namespace pt = boost::property_tree;
37 using namespace dg;
38 using namespace dg::command;
39
40 ForceLimits VoidForceLimits;
41 JointLimits VoidJointLimits;
42 Index VoidIndex(-1);
43
44 1 RobotUtilShrPtr RefVoidRobotUtil() {
45 1 return std::shared_ptr<RobotUtil>(nullptr);
46 }
47
48 ✗ ExtractJointMimics::ExtractJointMimics(std::string &robot_model) {
49 // Parsing the model from a string.
50 ✗ std::istringstream iss(robot_model);
51 /// Read the XML file in the property tree.
52 ✗ boost::property_tree::read_xml(iss, tree_);
53 /// Start the recursive parsing.
54 ✗ go_through_full();
55 }
56
57 ✗ const std::vector<std::string> &ExtractJointMimics::get_mimic_joints() {
58 ✗ return mimic_joints_;
59 }
60
61 ✗ void ExtractJointMimics::go_through_full() {
62 /// Root of the recursive parsing.
63 ✗ current_joint_name_ = "";
64 ✗ go_through(tree_, 0, 0);
65 }
66
67 ✗ void ExtractJointMimics::go_through(pt::ptree &pt, int level, int stage) {
68 /// If pt is empty (i.e. this is a leaf)
69 ✗ if (pt.empty()) {
70 /// and this is a name of a joint (stage == 3) update the
71 /// curret_joint_name_ variable.
72 ✗ if (stage == 3) current_joint_name_ = pt.data();
73 } else {
74 /// This is not a leaf
75 ✗ for (auto pos : pt) {
76 ✗ int new_stage = stage;
77
78 /// But this is joint
79 ✗ if (pos.first == "joint")
80 /// the continue the exploration.
81 ✗ new_stage = 1;
82 ✗ else if (pos.first == "<xmlattr>") {
83 /// we are exploring the xml attributes of a joint
84 /// -> continue the exploration
85 ✗ if (stage == 1) new_stage = 2;
86 }
87 /// The xml attribute of the joint is the name
88 /// next leaf is the name we are possibly looking for
89 ✗ else if (pos.first == "name") {
90 ✗ if (stage == 2) new_stage = 3;
91 }
92 /// The exploration of the tree tracback on the joint
93 /// and find that this is a mimic joint.
94 ✗ else if (pos.first == "mimic") {
95 ✗ if (stage == 1)
96 /// Save the current name of the joint
97 /// in mimic_joints.
98 ✗ mimic_joints_.push_back(current_joint_name_);
99 } else
100 ✗ new_stage = 0;
101
102 /// Explore the subtree of the XML robot description.
103 ✗ go_through(pos.second, level + 1, new_stage);
104 }
105 }
106 }
107
108 1 void ForceLimits::display(std::ostream &os) const {
109 1 os << "Lower limits:" << std::endl;
110 1 os << lower << std::endl;
111 1 os << "Upper Limits:" << std::endl;
112 1 os << upper << std::endl;
113 1 }
114
115 /******************** FootUtil ***************************/
116
117 2 void FootUtil::display(std::ostream &os) const {
118 2 os << "Right Foot Sole XYZ " << std::endl;
119 2 os << m_Right_Foot_Sole_XYZ << std::endl;
120 2 os << "Left Foot Frame Name:" << m_Left_Foot_Frame_Name << std::endl;
121 2 os << "Right Foot Frame Name:" << m_Right_Foot_Frame_Name << std::endl;
122 2 }
123
124 /******************** HandUtil ***************************/
125
126 1 void HandUtil::display(std::ostream &os) const {
127 1 os << "Left Hand Frame Name:" << m_Left_Hand_Frame_Name << std::endl;
128 1 os << "Right Hand Frame Name:" << m_Right_Hand_Frame_Name << std::endl;
129 1 }
130
131 /******************** ForceUtil ***************************/
132
133 4 void ForceUtil::set_name_to_force_id(const std::string &name,
134 const Index &force_id) {
135 4 m_name_to_force_id[name] = (Index)force_id;
136 4 create_force_id_to_name_map();
137
2/2
✓ Branch 1 taken 1 times.
✓ Branch 2 taken 3 times.
 4 if (name == "rf")
138 1 set_force_id_right_foot(m_name_to_force_id[name]);
139
2/2
✓ Branch 1 taken 1 times.
✓ Branch 2 taken 2 times.
 3 else if (name == "lf")
140 1 set_force_id_left_foot(m_name_to_force_id[name]);
141
2/2
✓ Branch 1 taken 1 times.
✓ Branch 2 taken 1 times.
 2 else if (name == "lh")
142 1 set_force_id_left_hand(m_name_to_force_id[name]);
143
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 else if (name == "rh")
144 1 set_force_id_right_hand(m_name_to_force_id[name]);
145 4 }
146
147 1 void ForceUtil::set_force_id_to_limits(const Index &force_id,
148 const dg::Vector &lf,
149 const dg::Vector &uf) {
150
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 1 m_force_id_to_limits[(Index)force_id].lower = lf;
151
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 1 m_force_id_to_limits[(Index)force_id].upper = uf;
152 1 }
153
154 4 Index ForceUtil::get_id_from_name(const std::string &name) {
155 4 std::map<std::string, Index>::const_iterator it;
156
1/2
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
 4 it = m_name_to_force_id.find(name);
157
1/2
✓ Branch 3 taken 4 times.
✗ Branch 4 not taken.
 4 if (it != m_name_to_force_id.end()) return it->second;
158 ✗ return -1;
159 }
160
161 std::string force_default_rtn("Force name not found");
162 std::string joint_default_rtn("Joint name not found");
163
164 4 const std::string &ForceUtil::get_name_from_id(Index idx) {
165 4 std::map<Index, std::string>::const_iterator it;
166
1/2
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
 4 it = m_force_id_to_name.find(idx);
167
1/2
✓ Branch 3 taken 4 times.
✗ Branch 4 not taken.
 4 if (it != m_force_id_to_name.end()) return it->second;
168 ✗ return force_default_rtn;
169 }
170
171 ✗ std::string ForceUtil::cp_get_name_from_id(Index idx) {
172 ✗ const std::string &default_rtn = get_name_from_id(idx);
173 ✗ return default_rtn;
174 }
175 4 void ForceUtil::create_force_id_to_name_map() {
176 4 std::map<std::string, Index>::const_iterator it;
177
2/2
✓ Branch 6 taken 10 times.
✓ Branch 7 taken 4 times.
 14 for (it = m_name_to_force_id.begin(); it != m_name_to_force_id.end(); it++)
178
2/4
✓ Branch 3 taken 10 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 10 times.
✗ Branch 7 not taken.
 10 m_force_id_to_name[it->second] = it->first;
179 4 }
180
181 2 const ForceLimits &ForceUtil::get_limits_from_id(Index force_id) {
182 std::map<Index, ForceLimits>::const_iterator iter =
183
1/2
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
 2 m_force_id_to_limits.find(force_id);
184
1/2
✗ Branch 3 not taken.
✓ Branch 4 taken 2 times.
 2 if (iter == m_force_id_to_limits.end())
185 ✗ return VoidForceLimits; // Returns void instance
186 2 return iter->second;
187 }
188
189 ✗ ForceLimits ForceUtil::cp_get_limits_from_id(Index force_id) {
190 std::map<Index, ForceLimits>::const_iterator iter =
191 ✗ m_force_id_to_limits.find(force_id);
192 ✗ if (iter == m_force_id_to_limits.end())
193 ✗ return VoidForceLimits; // Returns void instance
194 ✗ return iter->second;
195 }
196
197 2 void ForceUtil::display(std::ostream &os) const {
198 2 os << "Force Id to limits " << std::endl;
199 2 for (std::map<Index, ForceLimits>::const_iterator it =
200 2 m_force_id_to_limits.begin();
201
2/2
✓ Branch 3 taken 1 times.
✓ Branch 4 taken 2 times.
 3 it != m_force_id_to_limits.end(); ++it) {
202
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
 1 it->second.display(os);
203 }
204
205 2 os << "Name to force id:" << std::endl;
206 2 for (std::map<std::string, Index>::const_iterator it =
207 2 m_name_to_force_id.begin();
208
2/2
✓ Branch 3 taken 4 times.
✓ Branch 4 taken 2 times.
 6 it != m_name_to_force_id.end(); ++it) {
209
5/10
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 4 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 4 times.
✗ Branch 9 not taken.
✓ Branch 12 taken 4 times.
✗ Branch 13 not taken.
✓ Branch 15 taken 4 times.
✗ Branch 16 not taken.
 4 os << "(" << it->first << "," << it->second << ") ";
210 }
211 2 os << std::endl;
212
213 2 os << "Force id to Name:" << std::endl;
214 2 for (std::map<Index, std::string>::const_iterator it =
215 2 m_force_id_to_name.begin();
216
2/2
✓ Branch 3 taken 4 times.
✓ Branch 4 taken 2 times.
 6 it != m_force_id_to_name.end(); ++it) {
217
5/10
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 4 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 4 times.
✗ Branch 9 not taken.
✓ Branch 12 taken 4 times.
✗ Branch 13 not taken.
✓ Branch 15 taken 4 times.
✗ Branch 16 not taken.
 4 os << "(" << it->first << "," << it->second << ") ";
218 }
219 2 os << std::endl;
220
221 2 os << "Index for force sensors:" << std::endl;
222 2 os << "Left Hand (" << m_Force_Id_Left_Hand << ") ,"
223 2 << "Right Hand (" << m_Force_Id_Right_Hand << ") ,"
224 2 << "Left Foot (" << m_Force_Id_Left_Foot << ") ,"
225 2 << "Right Foot (" << m_Force_Id_Right_Foot << ") " << std::endl;
226 2 }
227
228 /**************** FromURDFToSot *************************/
229
4/8
✓ Branch 2 taken 2 times.
✗ Branch 3 not taken.
✓ Branch 12 taken 2 times.
✗ Branch 13 not taken.
✓ Branch 15 taken 2 times.
✗ Branch 16 not taken.
✓ Branch 19 taken 2 times.
✗ Branch 20 not taken.
 2 RobotUtil::RobotUtil() {}
230
231 1 void RobotUtil::set_joint_limits_for_id(const Index &idx, const double &lq,
232 const double &uq) {
233
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
 1 m_limits_map[(Index)idx] = JointLimits(lq, uq);
234 1 }
235
236 4 const JointLimits &RobotUtil::get_joint_limits_from_id(Index id) {
237
1/2
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
 4 std::map<Index, JointLimits>::const_iterator iter = m_limits_map.find(id);
238
1/2
✗ Branch 3 not taken.
✓ Branch 4 taken 4 times.
 4 if (iter == m_limits_map.end()) return VoidJointLimits;
239 4 return iter->second;
240 }
241 2 JointLimits RobotUtil::cp_get_joint_limits_from_id(Index id) {
242 2 const JointLimits &rtn = get_joint_limits_from_id(id);
243 2 return rtn;
244 }
245
246 1 void RobotUtil::set_name_to_id(const std::string &jointName,
247 const Index &jointId) {
248 1 m_name_to_id[jointName] = (Index)jointId;
249 1 create_id_to_name_map();
250 1 }
251
252 2 void RobotUtil::create_id_to_name_map() {
253 2 std::map<std::string, Index>::const_iterator it;
254
2/2
✓ Branch 6 taken 2 times.
✓ Branch 7 taken 2 times.
 4 for (it = m_name_to_id.begin(); it != m_name_to_id.end(); it++)
255
2/4
✓ Branch 3 taken 2 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 2 times.
✗ Branch 7 not taken.
 2 m_id_to_name[it->second] = it->first;
256 2 }
257
258 1 const Index &RobotUtil::get_id_from_name(const std::string &name) {
259
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 std::map<std::string, Index>::const_iterator it = m_name_to_id.find(name);
260
1/2
✗ Branch 3 not taken.
✓ Branch 4 taken 1 times.
 1 if (it == m_name_to_id.end()) return VoidIndex;
261 1 return it->second;
262 }
263
264 1 const std::string &RobotUtil::get_name_from_id(Index id) {
265
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 std::map<Index, std::string>::const_iterator iter = m_id_to_name.find(id);
266
1/2
✗ Branch 3 not taken.
✓ Branch 4 taken 1 times.
 1 if (iter == m_id_to_name.end()) return joint_default_rtn;
267 1 return iter->second;
268 }
269
270 ✗ void RobotUtil::set_urdf_to_sot(const std::vector<Index> &urdf_to_sot) {
271 ✗ m_nbJoints = urdf_to_sot.size();
272 ✗ m_urdf_to_sot.resize(urdf_to_sot.size());
273 ✗ m_dgv_urdf_to_sot.resize(urdf_to_sot.size());
274 ✗ for (std::size_t idx = 0; idx < urdf_to_sot.size(); idx++) {
275 ✗ m_urdf_to_sot[(Index)idx] = urdf_to_sot[(Index)idx];
276 ✗ m_dgv_urdf_to_sot[(Index)idx] =
277 ✗ static_cast<double>(urdf_to_sot[(Index)idx]);
278 }
279 }
280
281 1 void RobotUtil::set_urdf_to_sot(const dg::Vector &urdf_to_sot) {
282 1 m_nbJoints = urdf_to_sot.size();
283 1 m_urdf_to_sot.resize(urdf_to_sot.size());
284
2/2
✓ Branch 1 taken 3 times.
✓ Branch 2 taken 1 times.
 4 for (unsigned int idx = 0; idx < urdf_to_sot.size(); idx++) {
285 3 m_urdf_to_sot[idx] = (unsigned int)urdf_to_sot[idx];
286 }
287 1 m_dgv_urdf_to_sot = urdf_to_sot;
288 1 }
289
290 3 bool RobotUtil::joints_urdf_to_sot(ConstRefVector q_urdf, RefVector q_sot) {
291
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 3 times.
 3 if (m_nbJoints == 0) {
292 ✗ SEND_MSG("set_urdf_to_sot should be called", MSG_TYPE_ERROR);
293 ✗ return false;
294 }
295
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 3 times.
 3 assert(q_urdf.size() == static_cast<Eigen::VectorXd::Index>(m_nbJoints));
296
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 3 times.
 3 assert(q_sot.size() == static_cast<Eigen::VectorXd::Index>(m_nbJoints));
297
298
2/2
✓ Branch 0 taken 9 times.
✓ Branch 1 taken 3 times.
 12 for (unsigned int idx = 0; idx < m_nbJoints; idx++)
299 9 q_sot[m_urdf_to_sot[idx]] = q_urdf[idx];
300 3 return true;
301 }
302
303 3 bool RobotUtil::joints_sot_to_urdf(ConstRefVector q_sot, RefVector q_urdf) {
304
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 3 times.
 3 assert(q_urdf.size() == static_cast<Eigen::VectorXd::Index>(m_nbJoints));
305
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 3 times.
 3 assert(q_sot.size() == static_cast<Eigen::VectorXd::Index>(m_nbJoints));
306
307
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 3 times.
 3 if (m_nbJoints == 0) {
308 ✗ SEND_MSG("set_urdf_to_sot should be called", MSG_TYPE_ERROR);
309 ✗ return false;
310 }
311
312
2/2
✓ Branch 0 taken 9 times.
✓ Branch 1 taken 3 times.
 12 for (unsigned int idx = 0; idx < m_nbJoints; idx++)
313 9 q_urdf[idx] = q_sot[m_urdf_to_sot[idx]];
314 3 return true;
315 }
316
317 1 bool RobotUtil::velocity_urdf_to_sot(ConstRefVector q_urdf,
318 ConstRefVector v_urdf, RefVector v_sot) {
319
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 1 times.
 1 assert(q_urdf.size() == static_cast<Eigen::VectorXd::Index>(m_nbJoints + 7));
320
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 1 times.
 1 assert(v_urdf.size() == static_cast<Eigen::VectorXd::Index>(m_nbJoints + 6));
321
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 1 times.
 1 assert(v_sot.size() == static_cast<Eigen::VectorXd::Index>(m_nbJoints + 6));
322
323
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 1 times.
 1 if (m_nbJoints == 0) {
324 ✗ SEND_MSG("velocity_urdf_to_sot should be called", MSG_TYPE_ERROR);
325 ✗ return false;
326 }
327
5/10
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 1 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 1 times.
✗ Branch 14 not taken.
 1 const Eigen::Quaterniond q(q_urdf(6), q_urdf(3), q_urdf(4), q_urdf(5));
328
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 Eigen::Matrix3d oRb = q.toRotationMatrix();
329
4/8
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 1 times.
✗ Branch 11 not taken.
 1 v_sot.head<3>() = oRb * v_urdf.head<3>();
330
4/8
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 1 times.
✗ Branch 11 not taken.
 1 v_sot.segment<3>(3) = oRb * v_urdf.segment<3>(3);
331 // v_sot.head<6>() = v_urdf.head<6>();
332
5/10
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 1 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 1 times.
✗ Branch 14 not taken.
 1 joints_urdf_to_sot(v_urdf.tail(m_nbJoints), v_sot.tail(m_nbJoints));
333 1 return true;
334 }
335
336 1 bool RobotUtil::velocity_sot_to_urdf(ConstRefVector q_urdf,
337 ConstRefVector v_sot, RefVector v_urdf) {
338
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 1 times.
 1 assert(q_urdf.size() == static_cast<Eigen::VectorXd::Index>(m_nbJoints + 7));
339
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 1 times.
 1 assert(v_urdf.size() == static_cast<Eigen::VectorXd::Index>(m_nbJoints + 6));
340
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 1 times.
 1 assert(v_sot.size() == static_cast<Eigen::VectorXd::Index>(m_nbJoints + 6));
341
342
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 1 times.
 1 if (m_nbJoints == 0) {
343 ✗ SEND_MSG("velocity_sot_to_urdf should be called", MSG_TYPE_ERROR);
344 ✗ return false;
345 }
346 // compute rotation from world to base frame
347
5/10
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 1 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 1 times.
✗ Branch 14 not taken.
 1 const Eigen::Quaterniond q(q_urdf(6), q_urdf(3), q_urdf(4), q_urdf(5));
348
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 Eigen::Matrix3d oRb = q.toRotationMatrix();
349
5/10
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 1 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 1 times.
✗ Branch 14 not taken.
 1 v_urdf.head<3>() = oRb.transpose() * v_sot.head<3>();
350
5/10
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 1 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 1 times.
✗ Branch 14 not taken.
 1 v_urdf.segment<3>(3) = oRb.transpose() * v_sot.segment<3>(3);
351 // v_urdf.head<6>() = v_sot.head<6>();
352
5/10
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 1 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 1 times.
✗ Branch 14 not taken.
 1 joints_sot_to_urdf(v_sot.tail(m_nbJoints), v_urdf.tail(m_nbJoints));
353 1 return true;
354 }
355
356 2 bool RobotUtil::base_urdf_to_sot(ConstRefVector q_urdf, RefVector q_sot) {
357
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 2 times.
 2 assert(q_urdf.size() == 7);
358
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 2 times.
 2 assert(q_sot.size() == 6);
359
360 // ********* Quat to RPY *********
361
1/2
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
 2 const double W = q_urdf[6];
362
1/2
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
 2 const double X = q_urdf[3];
363
1/2
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
 2 const double Y = q_urdf[4];
364
1/2
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
 2 const double Z = q_urdf[5];
365
2/4
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2 times.
✗ Branch 5 not taken.
 2 const Eigen::Matrix3d R = Eigen::Quaterniond(W, X, Y, Z).toRotationMatrix();
366
5/10
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 2 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 2 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 2 times.
✗ Branch 14 not taken.
 2 return base_se3_to_sot(q_urdf.head<3>(), R, q_sot);
367 }
368
369 2 bool RobotUtil::base_sot_to_urdf(ConstRefVector q_sot, RefVector q_urdf) {
370
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 2 times.
 2 assert(q_urdf.size() == 7);
371
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 2 times.
 2 assert(q_sot.size() == 6);
372 // ********* RPY to Quat *********
373
1/2
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
 2 const double r = q_sot[3];
374
1/2
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
 2 const double p = q_sot[4];
375
1/2
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
 2 const double y = q_sot[5];
376
2/4
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2 times.
✗ Branch 5 not taken.
 2 const Eigen::AngleAxisd rollAngle(r, Eigen::Vector3d::UnitX());
377
2/4
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2 times.
✗ Branch 5 not taken.
 2 const Eigen::AngleAxisd pitchAngle(p, Eigen::Vector3d::UnitY());
378
2/4
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2 times.
✗ Branch 5 not taken.
 2 const Eigen::AngleAxisd yawAngle(y, Eigen::Vector3d::UnitZ());
379
2/4
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2 times.
✗ Branch 5 not taken.
 2 const Eigen::Quaternion<double> quat = yawAngle * pitchAngle * rollAngle;
380
381
2/4
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2 times.
✗ Branch 5 not taken.
 2 q_urdf[0] = q_sot[0]; // BASE
382
2/4
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2 times.
✗ Branch 5 not taken.
 2 q_urdf[1] = q_sot[1];
383
2/4
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2 times.
✗ Branch 5 not taken.
 2 q_urdf[2] = q_sot[2];
384
2/4
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2 times.
✗ Branch 5 not taken.
 2 q_urdf[3] = quat.x();
385
2/4
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2 times.
✗ Branch 5 not taken.
 2 q_urdf[4] = quat.y();
386
2/4
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2 times.
✗ Branch 5 not taken.
 2 q_urdf[5] = quat.z();
387
2/4
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2 times.
✗ Branch 5 not taken.
 2 q_urdf[6] = quat.w();
388
389 2 return true;
390 }
391
392 1 bool RobotUtil::config_urdf_to_sot(ConstRefVector q_urdf, RefVector q_sot) {
393
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 1 times.
 1 assert(q_urdf.size() == static_cast<Eigen::VectorXd::Index>(m_nbJoints + 7));
394
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 1 times.
 1 assert(q_sot.size() == static_cast<Eigen::VectorXd::Index>(m_nbJoints + 6));
395
396
4/8
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 1 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
 1 base_urdf_to_sot(q_urdf.head<7>(), q_sot.head<6>());
397
4/8
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 1 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
 1 joints_urdf_to_sot(q_urdf.tail(m_nbJoints), q_sot.tail(m_nbJoints));
398
399 1 return true;
400 }
401
402 1 bool RobotUtil::config_sot_to_urdf(ConstRefVector q_sot, RefVector q_urdf) {
403
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 1 times.
 1 assert(q_urdf.size() == static_cast<Eigen::VectorXd::Index>(m_nbJoints + 7));
404
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 1 times.
 1 assert(q_sot.size() == static_cast<Eigen::VectorXd::Index>(m_nbJoints + 6));
405
4/8
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 1 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
 1 base_sot_to_urdf(q_sot.head<6>(), q_urdf.head<7>());
406
4/8
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 1 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
 1 joints_sot_to_urdf(q_sot.tail(m_nbJoints), q_urdf.tail(m_nbJoints));
407 1 return true;
408 }
409 1 void RobotUtil::display(std::ostream &os) const {
410 1 m_force_util.display(os);
411 1 m_foot_util.display(os);
412 1 m_hand_util.display(os);
413 1 os << "Nb of joints: " << m_nbJoints << std::endl;
414 1 os << "Urdf file name: " << m_urdf_filename << std::endl;
415
416 // Display m_urdf_to_sot
417 1 os << "Map from urdf index to the Sot Index " << std::endl;
418
2/2
✓ Branch 1 taken 3 times.
✓ Branch 2 taken 1 times.
 4 for (unsigned int i = 0; i < m_urdf_to_sot.size(); i++)
419 3 os << "(" << i << " : " << m_urdf_to_sot[i] << ") ";
420 1 os << std::endl;
421
422 1 os << "Joint name to joint id:" << std::endl;
423 1 for (std::map<std::string, Index>::const_iterator it = m_name_to_id.begin();
424
2/2
✓ Branch 3 taken 1 times.
✓ Branch 4 taken 1 times.
 2 it != m_name_to_id.end(); ++it) {
425
5/10
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 1 times.
✗ Branch 9 not taken.
✓ Branch 12 taken 1 times.
✗ Branch 13 not taken.
✓ Branch 15 taken 1 times.
✗ Branch 16 not taken.
 1 os << "(" << it->first << "," << it->second << ") ";
426 }
427 1 os << std::endl;
428
429 1 os << "Joint id to joint Name:" << std::endl;
430 1 for (std::map<Index, std::string>::const_iterator it = m_id_to_name.begin();
431
2/2
✓ Branch 3 taken 1 times.
✓ Branch 4 taken 1 times.
 2 it != m_id_to_name.end(); ++it) {
432
5/10
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 1 times.
✗ Branch 9 not taken.
✓ Branch 12 taken 1 times.
✗ Branch 13 not taken.
✓ Branch 15 taken 1 times.
✗ Branch 16 not taken.
 1 os << "(" << it->first << "," << it->second << ") ";
433 }
434 1 os << std::endl;
435
436
2/4
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
 1 boost::property_tree::write_xml(os, property_tree_);
437 1 }
438
439 1 void RobotUtil::sendMsg(const std::string &msg, MsgType t,
440 const std::string &lineId) {
441
4/8
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 1 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
 1 logger_.stream(t, lineId) << "[RobotUtil]" << msg << '\n';
442 1 }
443
444 2 bool base_se3_to_sot(ConstRefVector pos, ConstRefMatrix R, RefVector q_sot) {
445
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 2 times.
 2 assert(q_sot.size() == 6);
446
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 2 times.
 2 assert(pos.size() == 3);
447
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 2 times.
 2 assert(R.rows() == 3);
448
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 2 times.
 2 assert(R.cols() == 3);
449 // ********* Quat to RPY *********
450 double r, p, y, m;
451 2 m = sqrt(R(2, 1) * R(2, 1) + R(2, 2) * R(2, 2));
452 2 p = atan2(-R(2, 0), m);
453
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 2 times.
 2 if (fabs(fabs(p) - M_PI / 2) < 0.001) {
454 ✗ r = 0.0;
455 ✗ y = -atan2(R(0, 1), R(1, 1));
456 } else {
457 2 y = atan2(R(1, 0), R(0, 0));
458 2 r = atan2(R(2, 1), R(2, 2));
459 }
460 // *********************************
461 2 q_sot[0] = pos[0];
462 2 q_sot[1] = pos[1];
463 2 q_sot[2] = pos[2];
464 2 q_sot[3] = r;
465 2 q_sot[4] = p;
466 2 q_sot[5] = y;
467 2 return true;
468 }
469
470 ✗ bool base_urdf_to_sot(ConstRefVector q_urdf, RefVector q_sot) {
471 ✗ assert(q_urdf.size() == 7);
472 ✗ assert(q_sot.size() == 6);
473 // ********* Quat to RPY *********
474 ✗ const double W = q_urdf[6];
475 ✗ const double X = q_urdf[3];
476 ✗ const double Y = q_urdf[4];
477 ✗ const double Z = q_urdf[5];
478 ✗ const Eigen::Matrix3d R = Eigen::Quaterniond(W, X, Y, Z).toRotationMatrix();
479 ✗ return base_se3_to_sot(q_urdf.head<3>(), R, q_sot);
480 }
481
482 ✗ bool base_sot_to_urdf(ConstRefVector q_sot, RefVector q_urdf) {
483 ✗ assert(q_urdf.size() == 7);
484 ✗ assert(q_sot.size() == 6);
485 // ********* RPY to Quat *********
486 ✗ const double r = q_sot[3];
487 ✗ const double p = q_sot[4];
488 ✗ const double y = q_sot[5];
489 ✗ const Eigen::AngleAxisd rollAngle(r, Eigen::Vector3d::UnitX());
490 ✗ const Eigen::AngleAxisd pitchAngle(p, Eigen::Vector3d::UnitY());
491 ✗ const Eigen::AngleAxisd yawAngle(y, Eigen::Vector3d::UnitZ());
492 ✗ const Eigen::Quaternion<double> quat = yawAngle * pitchAngle * rollAngle;
493
494 ✗ q_urdf[0] = q_sot[0]; // BASE
495 ✗ q_urdf[1] = q_sot[1];
496 ✗ q_urdf[2] = q_sot[2];
497 ✗ q_urdf[3] = quat.x();
498 ✗ q_urdf[4] = quat.y();
499 ✗ q_urdf[5] = quat.z();
500 ✗ q_urdf[6] = quat.w();
501
502 ✗ return true;
503 }
504
505 static std::map<std::string, RobotUtilShrPtr> sgl_map_name_to_robot_util;
506
507 ✗ std::shared_ptr<std::vector<std::string> > getListOfRobots() {
508 std::shared_ptr<std::vector<std::string> > res =
509 ✗ std::make_shared<std::vector<std::string> >();
510
511 std::map<std::string, RobotUtilShrPtr>::iterator it =
512 ✗ sgl_map_name_to_robot_util.begin();
513 ✗ while (it != sgl_map_name_to_robot_util.end()) {
514 ✗ res->push_back(it->first);
515 ✗ it++;
516 }
517
518 ✗ return res;
519 }
520
521 ✗ RobotUtilShrPtr getRobotUtil(std::string &robotName) {
522 std::map<std::string, RobotUtilShrPtr>::iterator it =
523 ✗ sgl_map_name_to_robot_util.find(robotName);
524 ✗ if (it != sgl_map_name_to_robot_util.end()) return it->second;
525 ✗ return RefVoidRobotUtil();
526 }
527
528 1 bool isNameInRobotUtil(std::string &robotName) {
529 std::map<std::string, RobotUtilShrPtr>::iterator it =
530
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 sgl_map_name_to_robot_util.find(robotName);
531
1/2
✗ Branch 2 not taken.
✓ Branch 3 taken 1 times.
 1 if (it != sgl_map_name_to_robot_util.end()) return true;
532 1 return false;
533 }
534
535 2 RobotUtilShrPtr createRobotUtil(std::string &robotName) {
536 std::map<std::string, RobotUtilShrPtr>::iterator it =
537
1/2
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
 2 sgl_map_name_to_robot_util.find(robotName);
538
1/2
✓ Branch 2 taken 2 times.
✗ Branch 3 not taken.
 2 if (it == sgl_map_name_to_robot_util.end()) {
539
2/4
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2 times.
✗ Branch 5 not taken.
 2 sgl_map_name_to_robot_util[robotName] = std::make_shared<RobotUtil>();
540
1/2
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
 2 it = sgl_map_name_to_robot_util.find(robotName);
541 2 return it->second;
542 }
543 ✗ return RefVoidRobotUtil();
544 }
545 } // namespace sot
546 } // namespace dynamicgraph
547