Directory: | ./ |
---|---|
File: | src/tools/robot-utils.cpp |
Date: | 2024-08-13 12:13:25 |
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 |