GCC Code Coverage Report

Directory: ./
File: include/pinocchio/parsers/urdf/model.hxx
Date: 2024-08-27 18:20:05
Exec Total Coverage
Lines: 122 177 68.9%
Branches: 123 318 38.7%
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 42 UrdfVisitorBaseTpl()
112 42 : log(NULL)
113 {
114 42 }
115
116 template<typename T>
117 54091 UrdfVisitorBaseTpl & operator<<(const T & t)
118 {
119 54091 if (log != NULL)
120 ✗ *log << t;
121 54091 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
145 42 UrdfVisitor(Model & model)
146 42 : model(model)
147 {
148 42 }
149
150 34 void setName(const std::string & name)
151 {
152 34 model.name = name;
153 34 }
154
155 25 virtual void addRootJoint(const Inertia & Y, const std::string & body_name)
156 {
157
3/6
✓ Branch 2 taken 25 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 25 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 25 times.
✗ Branch 9 not taken.
 25 addFixedJointAndBody(0, SE3::Identity(), "root_joint", Y, body_name);
158 // appendBodyToJoint(0,Y,SE3::Identity(),body_name); TODO: change for the
159 // correct behavior, see https://github.com/stack-of-tasks/pinocchio/pull/1102
160 // for discussions on the topic
161 25 }
162
163 254 void addJointAndBody(
164 JointType type,
165 const Vector3 & axis,
166 const FrameIndex & parentFrameId,
167 const SE3 & placement,
168 const std::string & joint_name,
169 const Inertia & Y,
170 const std::string & body_name,
171 const VectorConstRef & max_effort,
172 const VectorConstRef & max_velocity,
173 const VectorConstRef & min_config,
174 const VectorConstRef & max_config,
175 const VectorConstRef & friction,
176 const VectorConstRef & damping)
177 {
178
1/2
✓ Branch 2 taken 254 times.
✗ Branch 3 not taken.
 254 addJointAndBody(
179 type, axis, parentFrameId, placement, joint_name, Y, SE3::Identity(), body_name,
180 max_effort, max_velocity, min_config, max_config, friction, damping);
181 254 }
182
183 326 void addJointAndBody(
184 JointType type,
185 const Vector3 & axis,
186 const FrameIndex & parentFrameId,
187 const SE3 & placement,
188 const std::string & joint_name,
189 const Inertia & Y,
190 const SE3 & frame_placement,
191 const std::string & body_name,
192 const VectorConstRef & max_effort,
193 const VectorConstRef & max_velocity,
194 const VectorConstRef & min_config,
195 const VectorConstRef & max_config,
196 const VectorConstRef & friction,
197 const VectorConstRef & damping)
198 {
199 JointIndex joint_id;
200 326 const Frame & frame = model.frames[parentFrameId];
201
202
4/7
✓ Branch 0 taken 2 times.
✓ Branch 1 taken 315 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 6 times.
✗ Branch 4 not taken.
✓ Branch 5 taken 3 times.
✗ Branch 6 not taken.
 326 switch (type)
203 {
204 2 case Base::FLOATING:
205
7/14
✓ 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.
✓ Branch 16 taken 2 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 2 times.
✗ Branch 20 not taken.
 8 joint_id = model.addJoint(
206
2/4
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2 times.
✗ Branch 5 not taken.
 6 frame.parentJoint, typename JointCollection::JointModelFreeFlyer(),
207
1/2
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
 2 frame.placement * placement, joint_name, max_effort, max_velocity, min_config,
208 max_config, friction, damping);
209 2 break;
210 315 case Base::REVOLUTE:
211 315 joint_id = addJoint<
212 typename JointCollection::JointModelRX, typename JointCollection::JointModelRY,
213 typename JointCollection::JointModelRZ,
214
1/2
✓ Branch 1 taken 315 times.
✗ Branch 2 not taken.
 315 typename JointCollection::JointModelRevoluteUnaligned>(
215 axis, frame, placement, joint_name, max_effort, max_velocity, min_config, max_config,
216 friction, damping);
217 315 break;
218 ✗ case Base::CONTINUOUS:
219 ✗ joint_id = addJoint<
220 typename JointCollection::JointModelRUBX, typename JointCollection::JointModelRUBY,
221 typename JointCollection::JointModelRUBZ,
222 ✗ typename JointCollection::JointModelRevoluteUnboundedUnaligned>(
223 axis, frame, placement, joint_name, max_effort, max_velocity, min_config, max_config,
224 friction, damping);
225 ✗ break;
226 6 case Base::PRISMATIC:
227 6 joint_id = addJoint<
228 typename JointCollection::JointModelPX, typename JointCollection::JointModelPY,
229 typename JointCollection::JointModelPZ,
230
1/2
✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
 6 typename JointCollection::JointModelPrismaticUnaligned>(
231 axis, frame, placement, joint_name, max_effort, max_velocity, min_config, max_config,
232 friction, damping);
233 6 break;
234 ✗ case Base::PLANAR:
235 ✗ joint_id = model.addJoint(
236 ✗ frame.parentJoint, typename JointCollection::JointModelPlanar(),
237 ✗ frame.placement * placement, joint_name, max_effort, max_velocity, min_config,
238 max_config, friction, damping);
239 ✗ break;
240 3 case Base::SPHERICAL:
241
7/14
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 3 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 3 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 3 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 3 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 3 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 3 times.
✗ Branch 20 not taken.
 12 joint_id = model.addJoint(
242
2/4
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 3 times.
✗ Branch 5 not taken.
 9 frame.parentJoint, typename JointCollection::JointModelSpherical(),
243
1/2
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
 3 frame.placement * placement, joint_name, max_effort, max_velocity, min_config,
244 max_config, friction, damping);
245 3 break;
246 ✗ default:
247 ✗ PINOCCHIO_CHECK_INPUT_ARGUMENT(false, "The joint type is not correct.");
248 };
249
250
1/2
✓ Branch 1 taken 326 times.
✗ Branch 2 not taken.
 326 FrameIndex jointFrameId = model.addJointFrame(joint_id, (int)parentFrameId);
251
1/2
✓ Branch 1 taken 326 times.
✗ Branch 2 not taken.
 326 appendBodyToJoint(jointFrameId, Y, frame_placement, body_name);
252 326 }
253
254 198 void addFixedJointAndBody(
255 const FrameIndex & parent_frame_id,
256 const SE3 & joint_placement,
257 const std::string & joint_name,
258 const Inertia & Y,
259 const std::string & body_name)
260 {
261 198 const Frame & parent_frame = model.frames[parent_frame_id];
262 198 const JointIndex parent_frame_parent = parent_frame.parentJoint;
263
264
1/2
✓ Branch 1 taken 198 times.
✗ Branch 2 not taken.
 198 const SE3 placement = parent_frame.placement * joint_placement;
265
1/2
✓ Branch 1 taken 198 times.
✗ Branch 2 not taken.
 198 FrameIndex fid = model.addFrame(Frame(
266
1/2
✓ Branch 1 taken 198 times.
✗ Branch 2 not taken.
 198 joint_name, parent_frame.parentJoint, parent_frame_id, placement, FIXED_JOINT, Y));
267
268
1/2
✓ Branch 1 taken 198 times.
✗ Branch 2 not taken.
 198 model.addBodyFrame(body_name, parent_frame_parent, placement, (int)fid);
269 198 }
270
271 341 void appendBodyToJoint(
272 const FrameIndex fid,
273 const Inertia & Y,
274 const SE3 & placement,
275 const std::string & body_name)
276 {
277 341 const Frame & frame = model.frames[fid];
278
1/2
✓ Branch 1 taken 341 times.
✗ Branch 2 not taken.
 341 const SE3 & p = frame.placement * placement;
279 assert(frame.parentJoint >= 0);
280
3/4
✓ Branch 1 taken 341 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 321 times.
✓ Branch 4 taken 20 times.
 341 if (!Y.isZero(Scalar(0)))
281 {
282
1/2
✓ Branch 1 taken 321 times.
✗ Branch 2 not taken.
 321 model.appendBodyToJoint(frame.parentJoint, Y, p);
283 }
284
285
1/2
✓ Branch 1 taken 341 times.
✗ Branch 2 not taken.
 341 model.addBodyFrame(body_name, frame.parentJoint, p, (int)fid);
286 // Reference to model.frames[fid] can has changed because the vector
287 // may have been reallocated.
288 341 assert(model.frames[fid].parentJoint >= 0);
289 {
290
4/8
✓ Branch 4 taken 341 times.
✗ Branch 5 not taken.
✓ Branch 6 taken 341 times.
✗ Branch 7 not taken.
✓ Branch 13 taken 341 times.
✗ Branch 14 not taken.
✓ Branch 15 taken 341 times.
✗ Branch 16 not taken.
 341 assert(
291 !hasNaN(model.inertias[model.frames[fid].parentJoint].lever())
292 && !hasNaN(model.inertias[model.frames[fid].parentJoint].inertia().data()));
293 }
294 341 }
295
296 580 FrameIndex getBodyId(const std::string & frame_name) const
297 {
298
299
2/4
✓ Branch 1 taken 580 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 580 times.
✗ Branch 4 not taken.
 580 if (model.existFrame(frame_name, BODY))
300 {
301
1/2
✓ Branch 1 taken 580 times.
✗ Branch 2 not taken.
 580 FrameIndex fid = model.getFrameId(frame_name, BODY);
302
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 580 times.
 580 assert(model.frames[fid].type == BODY);
303 580 return fid;
304 }
305 else
306 ✗ throw std::invalid_argument("Model does not have any body named " + frame_name);
307 }
308
309 72 FrameIndex getJointId(const std::string & joint_name) const
310 {
311
312
1/2
✓ Branch 1 taken 72 times.
✗ Branch 2 not taken.
 72 if (model.existJointName(joint_name))
313 {
314 72 JointIndex jid = model.getJointId(joint_name);
315 72 return jid;
316 }
317 else
318 ✗ throw std::invalid_argument("Model does not have any joint named " + joint_name);
319 }
320
321 ✗ const std::string & getJointName(const JointIndex jointId) const
322 {
323 ✗ return model.names[jointId];
324 }
325
326 5 Frame getBodyFrame(const std::string & frame_name) const
327 {
328
329
2/4
✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 5 times.
✗ Branch 4 not taken.
 5 if (model.existFrame(frame_name, BODY))
330 {
331
1/2
✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
 5 FrameIndex fid = model.getFrameId(frame_name, BODY);
332
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 5 times.
 5 assert(model.frames[fid].type == BODY);
333 5 return model.frames[fid];
334 }
335 else
336 ✗ throw std::invalid_argument("Model does not have any body named " + frame_name);
337 }
338
339 ✗ JointIndex getParentId(const std::string & frame_name) const
340 {
341
342 ✗ if (model.existFrame(frame_name, BODY))
343 {
344 ✗ FrameIndex fid = model.getFrameId(frame_name, BODY);
345 ✗ assert(model.frames[fid].type == BODY);
346 ✗ return model.frames[fid].parentJoint;
347 }
348 else
349 ✗ throw std::invalid_argument("Model does not have any body named " + frame_name);
350 }
351
352 ✗ bool existFrame(const std::string & frame_name, const FrameType type) const
353 {
354 ✗ return model.existFrame(frame_name, type);
355 }
356
357 template<typename TypeX, typename TypeY, typename TypeZ, typename TypeUnaligned>
358 642 JointIndex addJoint(
359 const Vector3 & axis,
360 const Frame & frame,
361 const SE3 & placement,
362 const std::string & joint_name,
363 const VectorConstRef & max_effort,
364 const VectorConstRef & max_velocity,
365 const VectorConstRef & min_config,
366 const VectorConstRef & max_config,
367 const VectorConstRef & friction,
368 const VectorConstRef & damping)
369 {
370 642 CartesianAxis axisType = extractCartesianAxis(axis);
371
4/5
✓ Branch 0 taken 83 times.
✓ Branch 1 taken 142 times.
✓ Branch 2 taken 95 times.
✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
 642 switch (axisType)
372 {
373 166 case AXIS_X:
374
6/12
✓ Branch 2 taken 83 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 83 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 83 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 83 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 83 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 83 times.
✗ Branch 18 not taken.
 664 return model.addJoint(
375
3/6
✓ Branch 1 taken 83 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 83 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 83 times.
✗ Branch 8 not taken.
 498 frame.parentJoint, TypeX(), frame.placement * placement, joint_name, max_effort,
376 166 max_velocity, min_config, max_config, friction, damping);
377 break;
378
379 284 case AXIS_Y:
380
6/12
✓ Branch 2 taken 142 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 142 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 142 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 142 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 142 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 142 times.
✗ Branch 18 not taken.
 1136 return model.addJoint(
381
3/6
✓ Branch 1 taken 142 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 142 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 142 times.
✗ Branch 8 not taken.
 852 frame.parentJoint, TypeY(), frame.placement * placement, joint_name, max_effort,
382 284 max_velocity, min_config, max_config, friction, damping);
383 break;
384
385 190 case AXIS_Z:
386
6/12
✓ Branch 2 taken 95 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 95 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 95 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 95 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 95 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 95 times.
✗ Branch 18 not taken.
 760 return model.addJoint(
387
3/6
✓ Branch 1 taken 95 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 95 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 95 times.
✗ Branch 8 not taken.
 570 frame.parentJoint, TypeZ(), frame.placement * placement, joint_name, max_effort,
388 190 max_velocity, min_config, max_config, friction, damping);
389 break;
390
391 2 case AXIS_UNALIGNED:
392
6/12
✓ 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.
✓ Branch 14 taken 1 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 1 times.
✗ Branch 18 not taken.
 8 return model.addJoint(
393
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.
 6 frame.parentJoint, TypeUnaligned(axis.normalized()), frame.placement * placement,
394 2 joint_name, max_effort, max_velocity, min_config, max_config, friction, damping);
395 break;
396 ✗ default:
397 ✗ PINOCCHIO_CHECK_INPUT_ARGUMENT(false, "The axis type of the joint is of wrong type.");
398 break;
399 }
400 }
401
402 private:
403 ///
404 /// \brief The four possible cartesian types of an 3D axis.
405 ///
406 enum CartesianAxis
407 {
408 AXIS_X = 0,
409 AXIS_Y = 1,
410 AXIS_Z = 2,
411 AXIS_UNALIGNED
412 };
413
414 ///
415 /// \brief Extract the cartesian property of a particular 3D axis.
416 ///
417 /// \param[in] axis The input URDF axis.
418 ///
419 /// \return The property of the particular axis pinocchio::urdf::CartesianAxis.
420 ///
421 321 static inline CartesianAxis extractCartesianAxis(const Vector3 & axis)
422 {
423
4/6
✓ Branch 2 taken 321 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 321 times.
✗ Branch 6 not taken.
✓ Branch 7 taken 83 times.
✓ Branch 8 taken 238 times.
 321 if (axis.isApprox(Vector3::UnitX()))
424 83 return AXIS_X;
425
4/6
✓ Branch 2 taken 238 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 238 times.
✗ Branch 6 not taken.
✓ Branch 7 taken 142 times.
✓ Branch 8 taken 96 times.
 238 else if (axis.isApprox(Vector3::UnitY()))
426 142 return AXIS_Y;
427
4/6
✓ Branch 2 taken 96 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 96 times.
✗ Branch 6 not taken.
✓ Branch 7 taken 95 times.
✓ Branch 8 taken 1 times.
 96 else if (axis.isApprox(Vector3::UnitZ()))
428 95 return AXIS_Z;
429 else
430 1 return AXIS_UNALIGNED;
431 }
432 };
433
434 template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl>
435 class UrdfVisitorWithRootJoint : public UrdfVisitor<Scalar, Options, JointCollectionTpl>
436 {
437 public:
438 typedef UrdfVisitor<Scalar, Options, JointCollectionTpl> Base;
439 typedef typename Base::Inertia Inertia;
440 using Base::appendBodyToJoint;
441 using Base::model;
442
443 typedef ModelTpl<Scalar, Options, JointCollectionTpl> Model;
444 typedef typename Model::JointCollection JointCollection;
445 typedef typename Model::JointModel JointModel;
446
447 JointModel root_joint;
448
449 9 UrdfVisitorWithRootJoint(Model & model, const JointModelBase<JointModel> & root_joint)
450 : Base(model)
451 9 , root_joint(root_joint.derived())
452 {
453 9 }
454
455 9 void addRootJoint(const Inertia & Y, const std::string & body_name)
456 {
457 9 const Frame & frame = model.frames[0];
458
459
3/8
✓ Branch 2 taken 9 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 9 times.
✗ Branch 6 not taken.
✗ Branch 9 not taken.
✓ Branch 10 taken 9 times.
✗ Branch 13 not taken.
✗ Branch 14 not taken.
 9 PINOCCHIO_THROW(
460 !model.existJointName("root_joint"), std::invalid_argument,
461 "root_joint already exists as a joint in the kinematic tree.");
462
463
2/4
✓ Branch 2 taken 9 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 9 times.
✗ Branch 6 not taken.
 18 JointIndex idx = model.addJoint(
464
1/2
✓ Branch 1 taken 9 times.
✗ Branch 2 not taken.
 9 frame.parentJoint, root_joint, SE3::Identity(), "root_joint"
465 // TODO ,max_effort,max_velocity,min_config,max_config
466 );
467
468
1/2
✓ Branch 1 taken 9 times.
✗ Branch 2 not taken.
 9 FrameIndex jointFrameId = model.addJointFrame(idx, 0);
469
2/4
✓ Branch 1 taken 9 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 9 times.
✗ Branch 5 not taken.
 9 appendBodyToJoint(jointFrameId, Y, SE3::Identity(), body_name);
470 9 }
471 };
472
473 typedef UrdfVisitorBaseTpl<double, 0> UrdfVisitorBase;
474
475 PINOCCHIO_PARSERS_DLLAPI void
476 parseRootTree(const ::urdf::ModelInterface * urdfTree, UrdfVisitorBase & model);
477
478 PINOCCHIO_PARSERS_DLLAPI void
479 parseRootTree(const std::string & filename, UrdfVisitorBase & model);
480
481 PINOCCHIO_PARSERS_DLLAPI void
482 parseRootTreeFromXML(const std::string & xmlString, UrdfVisitorBase & model);
483 } // namespace details
484
485 template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl>
486 8 ModelTpl<Scalar, Options, JointCollectionTpl> & buildModel(
487 const std::string & filename,
488 const typename ModelTpl<Scalar, Options, JointCollectionTpl>::JointModel & root_joint,
489 ModelTpl<Scalar, Options, JointCollectionTpl> & model,
490 const bool verbose)
491 {
492
1/2
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
 8 details::UrdfVisitorWithRootJoint<Scalar, Options, JointCollectionTpl> visitor(
493 model, root_joint);
494
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 8 times.
 8 if (verbose)
495 ✗ visitor.log = &std::cout;
496
1/2
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
 8 details::parseRootTree(filename, visitor);
497 8 return model;
498 8 }
499
500 template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl>
501 8 ModelTpl<Scalar, Options, JointCollectionTpl> & buildModel(
502 const std::string & filename,
503 ModelTpl<Scalar, Options, JointCollectionTpl> & model,
504 const bool verbose)
505 {
506
1/2
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
 8 details::UrdfVisitor<Scalar, Options, JointCollectionTpl> visitor(model);
507
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 8 times.
 8 if (verbose)
508 ✗ visitor.log = &std::cout;
509
1/2
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
 8 details::parseRootTree(filename, visitor);
510 8 return model;
511 }
512
513 template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl>
514 ✗ ModelTpl<Scalar, Options, JointCollectionTpl> & buildModelFromXML(
515 const std::string & xmlStream,
516 const typename ModelTpl<Scalar, Options, JointCollectionTpl>::JointModel & rootJoint,
517 ModelTpl<Scalar, Options, JointCollectionTpl> & model,
518 const bool verbose)
519 {
520 ✗ details::UrdfVisitorWithRootJoint<Scalar, Options, JointCollectionTpl> visitor(
521 model, rootJoint);
522 ✗ if (verbose)
523 ✗ visitor.log = &std::cout;
524 ✗ details::parseRootTreeFromXML(xmlStream, visitor);
525 ✗ return model;
526 }
527
528 template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl>
529 ✗ ModelTpl<Scalar, Options, JointCollectionTpl> & buildModelFromXML(
530 const std::string & xmlStream,
531 ModelTpl<Scalar, Options, JointCollectionTpl> & model,
532 const bool verbose)
533 {
534 ✗ details::UrdfVisitor<Scalar, Options, JointCollectionTpl> visitor(model);
535 ✗ if (verbose)
536 ✗ visitor.log = &std::cout;
537 ✗ details::parseRootTreeFromXML(xmlStream, visitor);
538 ✗ return model;
539 }
540
541 template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl>
542 ✗ ModelTpl<Scalar, Options, JointCollectionTpl> & buildModel(
543 const std::shared_ptr<::urdf::ModelInterface> urdfTree,
544 const typename ModelTpl<Scalar, Options, JointCollectionTpl>::JointModel & rootJoint,
545 ModelTpl<Scalar, Options, JointCollectionTpl> & model,
546 const bool verbose)
547 {
548 ✗ PINOCCHIO_CHECK_INPUT_ARGUMENT(urdfTree != NULL);
549 ✗ details::UrdfVisitorWithRootJoint<Scalar, Options, JointCollectionTpl> visitor(
550 model, rootJoint);
551 ✗ if (verbose)
552 ✗ visitor.log = &std::cout;
553 ✗ details::parseRootTree(urdfTree.get(), visitor);
554 ✗ return model;
555 }
556
557 template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl>
558 ✗ ModelTpl<Scalar, Options, JointCollectionTpl> & buildModel(
559 const std::shared_ptr<::urdf::ModelInterface> urdfTree,
560 ModelTpl<Scalar, Options, JointCollectionTpl> & model,
561 const bool verbose)
562 {
563 ✗ PINOCCHIO_CHECK_INPUT_ARGUMENT(urdfTree != NULL);
564 ✗ details::UrdfVisitor<Scalar, Options, JointCollectionTpl> visitor(model);
565 ✗ if (verbose)
566 ✗ visitor.log = &std::cout;
567 ✗ details::parseRootTree(urdfTree.get(), visitor);
568 ✗ return model;
569 }
570
571 } // namespace urdf
572 } // namespace pinocchio
573
574 #endif // ifndef __pinocchio_multibody_parsers_urdf_model_hxx__
575