GCC Code Coverage Report

Directory: ./
File: include/pinocchio/parsers/urdf/model.hxx
Date: 2025-02-12 21:03:38
Exec Total Coverage
Lines: 172 200 86.0%
Branches: 153 336 45.5%
Line Branch Exec Source
1 //
2 // Copyright (c) 2015-2021 CNRS INRIA
3 // Copyright (c) 2015 Wandercraft, 86 rue de Paris 91400 Orsay, France.
4 //
5
6 #ifndef __pinocchio_multibody_parsers_urdf_model_hxx__
7 #define __pinocchio_multibody_parsers_urdf_model_hxx__
8
9 #include "pinocchio/math/matrix.hpp"
10 #include "pinocchio/parsers/config.hpp"
11 #include "pinocchio/parsers/urdf.hpp"
12 #include "pinocchio/multibody/model.hpp"
13
14 #include <sstream>
15 #include <boost/foreach.hpp>
16 #include <limits>
17 #include <iostream>
18
19 namespace pinocchio
20 {
21 namespace urdf
22 {
23 namespace details
24 {
25 typedef double urdf_scalar_type;
26
27 template<typename _Scalar, int Options>
28 class UrdfVisitorBaseTpl
29 {
30 public:
31 enum JointType
32 {
33 REVOLUTE,
34 CONTINUOUS,
35 PRISMATIC,
36 FLOATING,
37 PLANAR,
38 SPHERICAL
39 };
40
41 typedef enum ::pinocchio::FrameType FrameType;
42 typedef _Scalar Scalar;
43 typedef SE3Tpl<Scalar, Options> SE3;
44 typedef InertiaTpl<Scalar, Options> Inertia;
45
46 typedef Eigen::Matrix<Scalar, 3, 1> Vector3;
47 typedef Eigen::Matrix<Scalar, Eigen::Dynamic, 1> Vector;
48 typedef Eigen::Ref<Vector> VectorRef;
49 typedef Eigen::Ref<const Vector> VectorConstRef;
50
51 virtual void setName(const std::string & name) = 0;
52
53 virtual void addRootJoint(const Inertia & Y, const std::string & body_name) = 0;
54
55 virtual void addJointAndBody(
56 JointType type,
57 const Vector3 & axis,
58 const FrameIndex & parentFrameId,
59 const SE3 & placement,
60 const std::string & joint_name,
61 const Inertia & Y,
62 const std::string & body_name,
63 const VectorConstRef & max_effort,
64 const VectorConstRef & max_velocity,
65 const VectorConstRef & min_config,
66 const VectorConstRef & max_config,
67 const VectorConstRef & friction,
68 const VectorConstRef & damping) = 0;
69
70 virtual void addJointAndBody(
71 JointType type,
72 const Vector3 & axis,
73 const FrameIndex & parentFrameId,
74 const SE3 & placement,
75 const std::string & joint_name,
76 const Inertia & Y,
77 const SE3 & frame_placement,
78 const std::string & body_name,
79 const VectorConstRef & max_effort,
80 const VectorConstRef & max_velocity,
81 const VectorConstRef & min_config,
82 const VectorConstRef & max_config,
83 const VectorConstRef & friction,
84 const VectorConstRef & damping) = 0;
85
86 virtual void addFixedJointAndBody(
87 const FrameIndex & parentFrameId,
88 const SE3 & joint_placement,
89 const std::string & joint_name,
90 const Inertia & Y,
91 const std::string & body_name) = 0;
92
93 virtual void appendBodyToJoint(
94 const FrameIndex fid,
95 const Inertia & Y,
96 const SE3 & placement,
97 const std::string & body_name) = 0;
98
99 virtual FrameIndex getBodyId(const std::string & frame_name) const = 0;
100
101 virtual JointIndex getJointId(const std::string & joint_name) const = 0;
102
103 virtual const std::string & getJointName(const JointIndex jointId) const = 0;
104
105 virtual Frame getBodyFrame(const std::string & frame_name) const = 0;
106
107 virtual JointIndex getParentId(const std::string & frame_name) const = 0;
108
109 virtual bool existFrame(const std::string & frame_name, const FrameType type) const = 0;
110
111 127 UrdfVisitorBaseTpl()
112 127 : log(NULL)
113 {
114 127 }
115
116 template<typename T>
117 86795 UrdfVisitorBaseTpl & operator<<(const T & t)
118 {
119 86795 if (log != NULL)
120 ✗ *log << t;
121 86795 return *this;
122 }
123
124 std::ostream * log;
125 };
126
127 template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl>
128 class UrdfVisitor : public UrdfVisitorBaseTpl<Scalar, Options>
129 {
130 public:
131 typedef UrdfVisitorBaseTpl<Scalar, Options> Base;
132 typedef typename Base::JointType JointType;
133 typedef typename Base::Vector3 Vector3;
134 typedef typename Base::VectorConstRef VectorConstRef;
135 typedef typename Base::SE3 SE3;
136 typedef typename Base::Inertia Inertia;
137
138 typedef ModelTpl<Scalar, Options, JointCollectionTpl> Model;
139 typedef typename Model::JointCollection JointCollection;
140 typedef typename Model::JointModel JointModel;
141 typedef typename Model::Frame Frame;
142
143 Model & model;
144 std::string root_joint_name;
145
146 69 UrdfVisitor(Model & model)
147 69 : model(model)
148 {
149 69 }
150
151 58 UrdfVisitor(Model & model, const std::string & rjn)
152 58 : model(model)
153 58 , root_joint_name(rjn)
154 {
155 58 }
156
157 112 void setName(const std::string & name)
158 {
159 112 model.name = name;
160 112 }
161
162 51 virtual void addRootJoint(const Inertia & Y, const std::string & body_name)
163 {
164 51 const Frame & parent_frame = model.frames[0];
165
166
1/2
✓ Branch 1 taken 51 times.
✗ Branch 2 not taken.
 51 model.addFrame(
167 102 Frame(body_name, parent_frame.parentJoint, 0, parent_frame.placement, BODY, Y));
168 51 }
169
170 2104 void addJointAndBody(
171 JointType type,
172 const Vector3 & axis,
173 const FrameIndex & parentFrameId,
174 const SE3 & placement,
175 const std::string & joint_name,
176 const Inertia & Y,
177 const std::string & body_name,
178 const VectorConstRef & max_effort,
179 const VectorConstRef & max_velocity,
180 const VectorConstRef & min_config,
181 const VectorConstRef & max_config,
182 const VectorConstRef & friction,
183 const VectorConstRef & damping)
184 {
185
1/2
✓ Branch 2 taken 2104 times.
✗ Branch 3 not taken.
 2104 addJointAndBody(
186 type, axis, parentFrameId, placement, joint_name, Y, SE3::Identity(), body_name,
187 max_effort, max_velocity, min_config, max_config, friction, damping);
188 2104 }
189
190 2268 void addJointAndBody(
191 JointType type,
192 const Vector3 & axis,
193 const FrameIndex & parentFrameId,
194 const SE3 & placement,
195 const std::string & joint_name,
196 const Inertia & Y,
197 const SE3 & frame_placement,
198 const std::string & body_name,
199 const VectorConstRef & max_effort,
200 const VectorConstRef & max_velocity,
201 const VectorConstRef & min_config,
202 const VectorConstRef & max_config,
203 const VectorConstRef & friction,
204 const VectorConstRef & damping)
205 {
206 JointIndex joint_id;
207 2268 const Frame & frame = model.frames[parentFrameId];
208
209
5/7
✓ Branch 0 taken 4 times.
✓ Branch 1 taken 2220 times.
✓ Branch 2 taken 1 times.
✓ Branch 3 taken 34 times.
✗ Branch 4 not taken.
✓ Branch 5 taken 9 times.
✗ Branch 6 not taken.
 2268 switch (type)
210 {
211 4 case Base::FLOATING:
212
7/14
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 4 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 4 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 4 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 4 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 4 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 4 times.
✗ Branch 20 not taken.
 16 joint_id = model.addJoint(
213
2/4
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 4 times.
✗ Branch 5 not taken.
 12 frame.parentJoint, typename JointCollection::JointModelFreeFlyer(),
214
1/2
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
 4 frame.placement * placement, joint_name, max_effort, max_velocity, min_config,
215 max_config, friction, damping);
216 4 break;
217 2220 case Base::REVOLUTE:
218 2220 joint_id = addJoint<
219 typename JointCollection::JointModelRX, typename JointCollection::JointModelRY,
220 typename JointCollection::JointModelRZ,
221
1/2
✓ Branch 1 taken 2220 times.
✗ Branch 2 not taken.
 2220 typename JointCollection::JointModelRevoluteUnaligned>(
222 axis, frame, placement, joint_name, max_effort, max_velocity, min_config, max_config,
223 friction, damping);
224 2220 break;
225 1 case Base::CONTINUOUS:
226 1 joint_id = addJoint<
227 typename JointCollection::JointModelRUBX, typename JointCollection::JointModelRUBY,
228 typename JointCollection::JointModelRUBZ,
229
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 typename JointCollection::JointModelRevoluteUnboundedUnaligned>(
230 axis, frame, placement, joint_name, max_effort, max_velocity, min_config, max_config,
231 friction, damping);
232 1 break;
233 34 case Base::PRISMATIC:
234 34 joint_id = addJoint<
235 typename JointCollection::JointModelPX, typename JointCollection::JointModelPY,
236 typename JointCollection::JointModelPZ,
237
1/2
✓ Branch 1 taken 34 times.
✗ Branch 2 not taken.
 34 typename JointCollection::JointModelPrismaticUnaligned>(
238 axis, frame, placement, joint_name, max_effort, max_velocity, min_config, max_config,
239 friction, damping);
240 34 break;
241 ✗ case Base::PLANAR:
242 ✗ joint_id = model.addJoint(
243 ✗ frame.parentJoint, typename JointCollection::JointModelPlanar(),
244 ✗ frame.placement * placement, joint_name, max_effort, max_velocity, min_config,
245 max_config, friction, damping);
246 ✗ break;
247 9 case Base::SPHERICAL:
248
7/14
✓ Branch 1 taken 9 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 9 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 9 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 9 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 9 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 9 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 9 times.
✗ Branch 20 not taken.
 36 joint_id = model.addJoint(
249
2/4
✓ Branch 1 taken 9 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 9 times.
✗ Branch 5 not taken.
 27 frame.parentJoint, typename JointCollection::JointModelSpherical(),
250
1/2
✓ Branch 1 taken 9 times.
✗ Branch 2 not taken.
 9 frame.placement * placement, joint_name, max_effort, max_velocity, min_config,
251 max_config, friction, damping);
252 9 break;
253 ✗ default:
254 ✗ PINOCCHIO_CHECK_INPUT_ARGUMENT(false, "The joint type is not correct.");
255 };
256
257
1/2
✓ Branch 1 taken 2268 times.
✗ Branch 2 not taken.
 2268 FrameIndex jointFrameId = model.addJointFrame(joint_id, (int)parentFrameId);
258
1/2
✓ Branch 1 taken 2268 times.
✗ Branch 2 not taken.
 2268 appendBodyToJoint(jointFrameId, Y, frame_placement, body_name);
259 2268 }
260
261 1100 void addFixedJointAndBody(
262 const FrameIndex & parent_frame_id,
263 const SE3 & joint_placement,
264 const std::string & joint_name,
265 const Inertia & Y,
266 const std::string & body_name)
267 {
268 1100 const Frame & parent_frame = model.frames[parent_frame_id];
269 1100 const JointIndex parent_frame_parent = parent_frame.parentJoint;
270
271
1/2
✓ Branch 1 taken 1100 times.
✗ Branch 2 not taken.
 1100 const SE3 placement = parent_frame.placement * joint_placement;
272
1/2
✓ Branch 1 taken 1100 times.
✗ Branch 2 not taken.
 1100 FrameIndex fid = model.addFrame(Frame(
273
1/2
✓ Branch 1 taken 1100 times.
✗ Branch 2 not taken.
 1100 joint_name, parent_frame.parentJoint, parent_frame_id, placement, FIXED_JOINT, Y));
274
275
1/2
✓ Branch 1 taken 1100 times.
✗ Branch 2 not taken.
 1100 model.addBodyFrame(body_name, parent_frame_parent, placement, (int)fid);
276 1100 }
277
278 2332 void appendBodyToJoint(
279 const FrameIndex fid,
280 const Inertia & Y,
281 const SE3 & placement,
282 const std::string & body_name)
283 {
284 2332 const Frame & frame = model.frames[fid];
285
1/2
✓ Branch 1 taken 2332 times.
✗ Branch 2 not taken.
 2332 const SE3 & p = frame.placement * placement;
286 assert(frame.parentJoint >= 0);
287
3/4
✓ Branch 1 taken 2332 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 1941 times.
✓ Branch 4 taken 391 times.
 2332 if (!Y.isZero(Scalar(0)))
288 {
289
1/2
✓ Branch 1 taken 1941 times.
✗ Branch 2 not taken.
 1941 model.appendBodyToJoint(frame.parentJoint, Y, p);
290 }
291
292
1/2
✓ Branch 1 taken 2332 times.
✗ Branch 2 not taken.
 2332 model.addBodyFrame(body_name, frame.parentJoint, p, (int)fid);
293 // Reference to model.frames[fid] can has changed because the vector
294 // may have been reallocated.
295 2332 assert(model.frames[fid].parentJoint >= 0);
296 {
297
4/8
✓ Branch 4 taken 2332 times.
✗ Branch 5 not taken.
✓ Branch 6 taken 2332 times.
✗ Branch 7 not taken.
✓ Branch 13 taken 2332 times.
✗ Branch 14 not taken.
✓ Branch 15 taken 2332 times.
✗ Branch 16 not taken.
 2332 assert(
298 !hasNaN(model.inertias[model.frames[fid].parentJoint].lever())
299 && !hasNaN(model.inertias[model.frames[fid].parentJoint].inertia().data()));
300 }
301 2332 }
302
303 3562 FrameIndex getBodyId(const std::string & frame_name) const
304 {
305
306
2/4
✓ Branch 1 taken 3562 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 3562 times.
✗ Branch 4 not taken.
 3562 if (model.existFrame(frame_name, BODY))
307 {
308
1/2
✓ Branch 1 taken 3562 times.
✗ Branch 2 not taken.
 3562 FrameIndex fid = model.getFrameId(frame_name, BODY);
309
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 3562 times.
 3562 assert(model.frames[fid].type == BODY);
310 3562 return fid;
311 }
312 else
313 ✗ throw std::invalid_argument("Model does not have any body named " + frame_name);
314 }
315
316 164 FrameIndex getJointId(const std::string & joint_name) const
317 {
318
319
1/2
✓ Branch 1 taken 164 times.
✗ Branch 2 not taken.
 164 if (model.existJointName(joint_name))
320 {
321 164 JointIndex jid = model.getJointId(joint_name);
322 164 return jid;
323 }
324 else
325 ✗ throw std::invalid_argument("Model does not have any joint named " + joint_name);
326 }
327
328 ✗ const std::string & getJointName(const JointIndex jointId) const
329 {
330 ✗ return model.names[jointId];
331 }
332
333 29 Frame getBodyFrame(const std::string & frame_name) const
334 {
335
336
2/4
✓ Branch 1 taken 29 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 29 times.
✗ Branch 4 not taken.
 29 if (model.existFrame(frame_name, BODY))
337 {
338
1/2
✓ Branch 1 taken 29 times.
✗ Branch 2 not taken.
 29 FrameIndex fid = model.getFrameId(frame_name, BODY);
339
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 29 times.
 29 assert(model.frames[fid].type == BODY);
340 29 return model.frames[fid];
341 }
342 else
343 ✗ throw std::invalid_argument("Model does not have any body named " + frame_name);
344 }
345
346 16 JointIndex getParentId(const std::string & frame_name) const
347 {
348
349
2/4
✓ Branch 1 taken 16 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 16 times.
✗ Branch 4 not taken.
 16 if (model.existFrame(frame_name, BODY))
350 {
351
1/2
✓ Branch 1 taken 16 times.
✗ Branch 2 not taken.
 16 FrameIndex fid = model.getFrameId(frame_name, BODY);
352
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 16 times.
 16 assert(model.frames[fid].type == BODY);
353 16 return model.frames[fid].parentJoint;
354 }
355 else
356 ✗ throw std::invalid_argument("Model does not have any body named " + frame_name);
357 }
358
359 ✗ bool existFrame(const std::string & frame_name, const FrameType type) const
360 {
361 ✗ return model.existFrame(frame_name, type);
362 }
363
364 template<typename TypeX, typename TypeY, typename TypeZ, typename TypeUnaligned>
365 4510 JointIndex addJoint(
366 const Vector3 & axis,
367 const Frame & frame,
368 const SE3 & placement,
369 const std::string & joint_name,
370 const VectorConstRef & max_effort,
371 const VectorConstRef & max_velocity,
372 const VectorConstRef & min_config,
373 const VectorConstRef & max_config,
374 const VectorConstRef & friction,
375 const VectorConstRef & damping)
376 {
377 4510 CartesianAxis axisType = extractCartesianAxis(axis);
378
4/5
✓ Branch 0 taken 547 times.
✓ Branch 1 taken 797 times.
✓ Branch 2 taken 598 times.
✓ Branch 3 taken 313 times.
✗ Branch 4 not taken.
 4510 switch (axisType)
379 {
380 1094 case AXIS_X:
381
6/12
✓ Branch 2 taken 547 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 547 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 547 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 547 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 547 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 547 times.
✗ Branch 18 not taken.
 4376 return model.addJoint(
382
3/6
✓ Branch 1 taken 547 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 547 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 547 times.
✗ Branch 8 not taken.
 3282 frame.parentJoint, TypeX(), frame.placement * placement, joint_name, max_effort,
383 1094 max_velocity, min_config, max_config, friction, damping);
384 break;
385
386 1594 case AXIS_Y:
387
6/12
✓ Branch 2 taken 797 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 797 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 797 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 797 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 797 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 797 times.
✗ Branch 18 not taken.
 6376 return model.addJoint(
388
3/6
✓ Branch 1 taken 797 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 797 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 797 times.
✗ Branch 8 not taken.
 4782 frame.parentJoint, TypeY(), frame.placement * placement, joint_name, max_effort,
389 1594 max_velocity, min_config, max_config, friction, damping);
390 break;
391
392 1196 case AXIS_Z:
393
6/12
✓ Branch 2 taken 598 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 598 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 598 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 598 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 598 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 598 times.
✗ Branch 18 not taken.
 4784 return model.addJoint(
394
3/6
✓ Branch 1 taken 598 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 598 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 598 times.
✗ Branch 8 not taken.
 3588 frame.parentJoint, TypeZ(), frame.placement * placement, joint_name, max_effort,
395 1196 max_velocity, min_config, max_config, friction, damping);
396 break;
397
398 626 case AXIS_UNALIGNED:
399
6/12
✓ Branch 2 taken 313 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 313 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 313 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 313 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 313 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 313 times.
✗ Branch 18 not taken.
 2504 return model.addJoint(
400
4/8
✓ Branch 1 taken 313 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 313 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 313 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 313 times.
✗ Branch 11 not taken.
 1878 frame.parentJoint, TypeUnaligned(axis.normalized()), frame.placement * placement,
401 626 joint_name, max_effort, max_velocity, min_config, max_config, friction, damping);
402 break;
403 ✗ default:
404 ✗ PINOCCHIO_CHECK_INPUT_ARGUMENT(false, "The axis type of the joint is of wrong type.");
405 break;
406 }
407 }
408
409 private:
410 ///
411 /// \brief The four possible cartesian types of an 3D axis.
412 ///
413 enum CartesianAxis
414 {
415 AXIS_X = 0,
416 AXIS_Y = 1,
417 AXIS_Z = 2,
418 AXIS_UNALIGNED
419 };
420
421 ///
422 /// \brief Extract the cartesian property of a particular 3D axis.
423 ///
424 /// \param[in] axis The input URDF axis.
425 ///
426 /// \return The property of the particular axis pinocchio::urdf::CartesianAxis.
427 ///
428 2255 static inline CartesianAxis extractCartesianAxis(const Vector3 & axis)
429 {
430
4/6
✓ Branch 2 taken 2255 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 2255 times.
✗ Branch 6 not taken.
✓ Branch 7 taken 547 times.
✓ Branch 8 taken 1708 times.
 2255 if (axis.isApprox(Vector3::UnitX()))
431 547 return AXIS_X;
432
4/6
✓ Branch 2 taken 1708 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1708 times.
✗ Branch 6 not taken.
✓ Branch 7 taken 797 times.
✓ Branch 8 taken 911 times.
 1708 else if (axis.isApprox(Vector3::UnitY()))
433 797 return AXIS_Y;
434
4/6
✓ Branch 2 taken 911 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 911 times.
✗ Branch 6 not taken.
✓ Branch 7 taken 598 times.
✓ Branch 8 taken 313 times.
 911 else if (axis.isApprox(Vector3::UnitZ()))
435 598 return AXIS_Z;
436 else
437 313 return AXIS_UNALIGNED;
438 }
439 };
440
441 template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl>
442 class UrdfVisitorWithRootJoint : public UrdfVisitor<Scalar, Options, JointCollectionTpl>
443 {
444 public:
445 typedef UrdfVisitor<Scalar, Options, JointCollectionTpl> Base;
446 typedef typename Base::Inertia Inertia;
447 using Base::appendBodyToJoint;
448 using Base::model;
449
450 typedef ModelTpl<Scalar, Options, JointCollectionTpl> Model;
451 typedef typename Model::JointCollection JointCollection;
452 typedef typename Model::JointModel JointModel;
453
454 JointModel root_joint;
455
456 58 UrdfVisitorWithRootJoint(
457 Model & model,
458 const JointModelBase<JointModel> & root_joint,
459 const std::string & rjn = "root_joint")
460 : Base(model, rjn)
461
1/2
✓ Branch 3 taken 58 times.
✗ Branch 4 not taken.
 58 , root_joint(root_joint.derived())
462 {
463 58 }
464
465 58 void addRootJoint(const Inertia & Y, const std::string & body_name)
466 {
467 58 const Frame & frame = model.frames[0];
468
469
4/6
✓ Branch 1 taken 58 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 1 times.
✓ Branch 4 taken 57 times.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
 58 PINOCCHIO_THROW(
470 !model.existJointName(this->root_joint_name), std::invalid_argument,
471 "root_joint already exists as a joint in the kinematic tree.");
472
473 ✗ JointIndex idx = model.addJoint(
474
2/4
✓ Branch 1 taken 57 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 57 times.
✗ Branch 5 not taken.
 57 frame.parentJoint, root_joint, SE3::Identity(), this->root_joint_name
475 // TODO ,max_effort,max_velocity,min_config,max_config
476 );
477
478
1/2
✓ Branch 1 taken 57 times.
✗ Branch 2 not taken.
 57 FrameIndex jointFrameId = model.addJointFrame(idx, 0);
479
2/4
✓ Branch 1 taken 57 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 57 times.
✗ Branch 5 not taken.
 57 appendBodyToJoint(jointFrameId, Y, SE3::Identity(), body_name);
480 57 }
481 };
482
483 typedef UrdfVisitorBaseTpl<double, 0> UrdfVisitorBase;
484
485 PINOCCHIO_PARSERS_DLLAPI void
486 parseRootTree(const ::urdf::ModelInterface * urdfTree, UrdfVisitorBase & model);
487
488 PINOCCHIO_PARSERS_DLLAPI void
489 parseRootTree(const std::string & filename, UrdfVisitorBase & model);
490
491 PINOCCHIO_PARSERS_DLLAPI void
492 parseRootTreeFromXML(const std::string & xmlString, UrdfVisitorBase & model);
493 } // namespace details
494
495 template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl>
496 48 ModelTpl<Scalar, Options, JointCollectionTpl> & buildModel(
497 const std::string & filename,
498 const typename ModelTpl<Scalar, Options, JointCollectionTpl>::JointModel & rootJoint,
499 const std::string & rootJointName,
500 ModelTpl<Scalar, Options, JointCollectionTpl> & model,
501 const bool verbose)
502 {
503
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 48 times.
 48 if (rootJointName.empty())
504 ✗ throw std::invalid_argument(
505 "rootJoint was given without a name. Please fill the argument root_joint_name");
506
507
1/2
✓ Branch 1 taken 48 times.
✗ Branch 2 not taken.
 48 details::UrdfVisitorWithRootJoint<Scalar, Options, JointCollectionTpl> visitor(
508 model, rootJoint, rootJointName);
509
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 48 times.
 48 if (verbose)
510 ✗ visitor.log = &std::cout;
511
1/2
✓ Branch 1 taken 48 times.
✗ Branch 2 not taken.
 48 details::parseRootTree(filename, visitor);
512 48 return model;
513 48 }
514
515 template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl>
516 46 ModelTpl<Scalar, Options, JointCollectionTpl> & buildModel(
517 const std::string & filename,
518 const typename ModelTpl<Scalar, Options, JointCollectionTpl>::JointModel & rootJoint,
519 ModelTpl<Scalar, Options, JointCollectionTpl> & model,
520 const bool verbose)
521 {
522
2/4
✓ Branch 2 taken 46 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 46 times.
✗ Branch 6 not taken.
 46 return buildModel(filename, rootJoint, "root_joint", model, verbose);
523 }
524
525 template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl>
526 26 ModelTpl<Scalar, Options, JointCollectionTpl> & buildModel(
527 const std::string & filename,
528 ModelTpl<Scalar, Options, JointCollectionTpl> & model,
529 const bool verbose)
530 {
531
1/2
✓ Branch 1 taken 26 times.
✗ Branch 2 not taken.
 26 details::UrdfVisitor<Scalar, Options, JointCollectionTpl> visitor(model);
532
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 26 times.
 26 if (verbose)
533 ✗ visitor.log = &std::cout;
534
1/2
✓ Branch 1 taken 26 times.
✗ Branch 2 not taken.
 26 details::parseRootTree(filename, visitor);
535 26 return model;
536 26 }
537
538 template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl>
539 4 ModelTpl<Scalar, Options, JointCollectionTpl> & buildModelFromXML(
540 const std::string & xmlStream,
541 const typename ModelTpl<Scalar, Options, JointCollectionTpl>::JointModel & rootJoint,
542 const std::string & rootJointName,
543 ModelTpl<Scalar, Options, JointCollectionTpl> & model,
544 const bool verbose)
545 {
546
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 4 times.
 4 if (rootJointName.empty())
547 ✗ throw std::invalid_argument(
548 "rootJoint was given without a name. Please fill the argument rootJointName");
549
550
1/2
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
 4 details::UrdfVisitorWithRootJoint<Scalar, Options, JointCollectionTpl> visitor(
551 model, rootJoint, rootJointName);
552
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 4 times.
 4 if (verbose)
553 ✗ visitor.log = &std::cout;
554
2/2
✓ Branch 1 taken 3 times.
✓ Branch 2 taken 1 times.
 4 details::parseRootTreeFromXML(xmlStream, visitor);
555 3 return model;
556 4 }
557
558 template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl>
559 4 ModelTpl<Scalar, Options, JointCollectionTpl> & buildModelFromXML(
560 const std::string & xmlStream,
561 const typename ModelTpl<Scalar, Options, JointCollectionTpl>::JointModel & rootJoint,
562 ModelTpl<Scalar, Options, JointCollectionTpl> & model,
563 const bool verbose)
564 {
565
3/4
✓ Branch 2 taken 4 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 3 times.
✓ Branch 6 taken 1 times.
 5 return buildModelFromXML(xmlStream, rootJoint, "root_joint", model, verbose);
566 }
567
568 template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl>
569 5 ModelTpl<Scalar, Options, JointCollectionTpl> & buildModelFromXML(
570 const std::string & xmlStream,
571 ModelTpl<Scalar, Options, JointCollectionTpl> & model,
572 const bool verbose)
573 {
574
1/2
✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
 5 details::UrdfVisitor<Scalar, Options, JointCollectionTpl> visitor(model);
575
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 5 times.
 5 if (verbose)
576 ✗ visitor.log = &std::cout;
577
1/2
✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
 5 details::parseRootTreeFromXML(xmlStream, visitor);
578 5 return model;
579 5 }
580
581 template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl>
582 1 ModelTpl<Scalar, Options, JointCollectionTpl> & buildModel(
583 const std::shared_ptr<::urdf::ModelInterface> urdfTree,
584 const typename ModelTpl<Scalar, Options, JointCollectionTpl>::JointModel & rootJoint,
585 const std::string & rootJointName,
586 ModelTpl<Scalar, Options, JointCollectionTpl> & model,
587 const bool verbose)
588 {
589
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 1 times.
 1 if (rootJointName.empty())
590 ✗ throw std::invalid_argument(
591 "rootJoint was given without a name. Please fill the argument rootJointName");
592
593
1/4
✗ Branch 1 not taken.
✓ Branch 2 taken 1 times.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
 1 PINOCCHIO_CHECK_INPUT_ARGUMENT(urdfTree != NULL);
594
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 details::UrdfVisitorWithRootJoint<Scalar, Options, JointCollectionTpl> visitor(
595 model, rootJoint, rootJointName);
596
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 1 times.
 1 if (verbose)
597 ✗ visitor.log = &std::cout;
598
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
 1 details::parseRootTree(urdfTree.get(), visitor);
599 1 return model;
600 1 }
601
602 template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl>
603 1 ModelTpl<Scalar, Options, JointCollectionTpl> & buildModel(
604 const std::shared_ptr<::urdf::ModelInterface> urdfTree,
605 const typename ModelTpl<Scalar, Options, JointCollectionTpl>::JointModel & rootJoint,
606 ModelTpl<Scalar, Options, JointCollectionTpl> & model,
607 const bool verbose)
608 {
609
2/4
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 1 times.
✗ Branch 7 not taken.
 1 return buildModel(urdfTree, rootJoint, "root_joint", model, verbose);
610 }
611
612 template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl>
613 1 ModelTpl<Scalar, Options, JointCollectionTpl> & buildModel(
614 const std::shared_ptr<::urdf::ModelInterface> urdfTree,
615 ModelTpl<Scalar, Options, JointCollectionTpl> & model,
616 const bool verbose)
617 {
618
1/4
✗ Branch 1 not taken.
✓ Branch 2 taken 1 times.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
 1 PINOCCHIO_CHECK_INPUT_ARGUMENT(urdfTree != NULL);
619
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 details::UrdfVisitor<Scalar, Options, JointCollectionTpl> visitor(model);
620
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 1 times.
 1 if (verbose)
621 ✗ visitor.log = &std::cout;
622
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
 1 details::parseRootTree(urdfTree.get(), visitor);
623 1 return model;
624 1 }
625
626 } // namespace urdf
627 } // namespace pinocchio
628
629 #endif // ifndef __pinocchio_multibody_parsers_urdf_model_hxx__
630