| GCC Code Coverage Report | |||||||||||||||||||||
|
|||||||||||||||||||||
| 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/urdf.hpp" |
||
11 |
#include "pinocchio/multibody/model.hpp" |
||
12 |
|||
13 |
#include <sstream> |
||
14 |
#include <boost/foreach.hpp> |
||
15 |
#include <limits> |
||
16 |
|||
17 |
namespace pinocchio |
||
18 |
{ |
||
19 |
namespace urdf |
||
20 |
{ |
||
21 |
namespace details |
||
22 |
{ |
||
23 |
typedef double urdf_scalar_type; |
||
24 |
|||
25 |
template<typename _Scalar, int Options> |
||
26 |
class UrdfVisitorBaseTpl { |
||
27 |
public: |
||
28 |
enum JointType { |
||
29 |
REVOLUTE, CONTINUOUS, PRISMATIC, FLOATING, PLANAR |
||
30 |
}; |
||
31 |
|||
32 |
typedef _Scalar Scalar; |
||
33 |
typedef SE3Tpl<Scalar,Options> SE3; |
||
34 |
typedef InertiaTpl<Scalar,Options> Inertia; |
||
35 |
|||
36 |
typedef Eigen::Matrix<Scalar, 3, 1> Vector3; |
||
37 |
typedef Eigen::Matrix<Scalar, Eigen::Dynamic, 1> Vector; |
||
38 |
typedef Eigen::Ref<Vector> VectorRef; |
||
39 |
typedef Eigen::Ref<const Vector> VectorConstRef; |
||
40 |
|||
41 |
virtual void setName (const std::string& name) = 0; |
||
42 |
|||
43 |
virtual void addRootJoint (const Inertia& Y, const std::string & body_name) = 0; |
||
44 |
|||
45 |
virtual void addJointAndBody( |
||
46 |
JointType type, |
||
47 |
const Vector3& axis, |
||
48 |
const FrameIndex & parentFrameId, |
||
49 |
const SE3 & placement, |
||
50 |
const std::string & joint_name, |
||
51 |
const Inertia& Y, |
||
52 |
const std::string & body_name, |
||
53 |
const VectorConstRef& max_effort, |
||
54 |
const VectorConstRef& max_velocity, |
||
55 |
const VectorConstRef& min_config, |
||
56 |
const VectorConstRef& max_config, |
||
57 |
const VectorConstRef& friction, |
||
58 |
const VectorConstRef& damping |
||
59 |
) = 0; |
||
60 |
|||
61 |
virtual void addFixedJointAndBody( |
||
62 |
const FrameIndex & parentFrameId, |
||
63 |
const SE3 & joint_placement, |
||
64 |
const std::string & joint_name, |
||
65 |
const Inertia& Y, |
||
66 |
const std::string & body_name) = 0; |
||
67 |
|||
68 |
virtual void appendBodyToJoint( |
||
69 |
const FrameIndex fid, |
||
70 |
const Inertia& Y, |
||
71 |
const SE3 & placement, |
||
72 |
const std::string & body_name) = 0; |
||
73 |
|||
74 |
virtual FrameIndex getBodyId ( |
||
75 |
const std::string& frame_name) const = 0; |
||
76 |
|||
77 |
52 |
UrdfVisitorBaseTpl () : log (NULL) {} |
|
78 |
|||
79 |
template <typename T> |
||
80 |
43713 |
UrdfVisitorBaseTpl& operator<< (const T& t) |
|
81 |
{ |
||
82 |
43713 |
if (log != NULL) *log << t; |
|
83 |
43713 |
return *this; |
|
84 |
} |
||
85 |
|||
86 |
std::ostream* log; |
||
87 |
}; |
||
88 |
|||
89 |
template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl> |
||
90 |
class UrdfVisitor : public UrdfVisitorBaseTpl<Scalar, Options> |
||
91 |
{ |
||
92 |
public: |
||
93 |
typedef UrdfVisitorBaseTpl<Scalar, Options> Base; |
||
94 |
typedef typename Base::JointType JointType; |
||
95 |
typedef typename Base::Vector3 Vector3; |
||
96 |
typedef typename Base::VectorConstRef VectorConstRef; |
||
97 |
typedef typename Base::SE3 SE3; |
||
98 |
typedef typename Base::Inertia Inertia; |
||
99 |
|||
100 |
typedef ModelTpl<Scalar,Options,JointCollectionTpl> Model; |
||
101 |
typedef typename Model::JointCollection JointCollection; |
||
102 |
typedef typename Model::JointModel JointModel; |
||
103 |
typedef typename Model::Frame Frame; |
||
104 |
|||
105 |
Model& model; |
||
106 |
|||
107 |
52 |
UrdfVisitor (Model& model) : model(model) {} |
|
108 |
|||
109 |
52 |
void setName (const std::string& name) |
|
110 |
{ |
||
111 |
52 |
model.name = name; |
|
112 |
52 |
} |
|
113 |
|||
114 |
29 |
virtual void addRootJoint(const Inertia& Y, const std::string & body_name) |
|
115 |
{ |
||
116 |
✓✗✓✗ ✓✗ |
29 |
addFixedJointAndBody(0, SE3::Identity(), "root_joint", Y, body_name); |
117 |
// appendBodyToJoint(0,Y,SE3::Identity(),body_name); TODO: change for the correct behavior, see https://github.com/stack-of-tasks/pinocchio/pull/1102 for discussions on the topic |
||
118 |
|||
119 |
|||
120 |
29 |
} |
|
121 |
|||
122 |
1054 |
void addJointAndBody( |
|
123 |
JointType type, |
||
124 |
const Vector3& axis, |
||
125 |
const FrameIndex & parentFrameId, |
||
126 |
const SE3 & placement, |
||
127 |
const std::string & joint_name, |
||
128 |
const Inertia& Y, |
||
129 |
const std::string & body_name, |
||
130 |
const VectorConstRef& max_effort, |
||
131 |
const VectorConstRef& max_velocity, |
||
132 |
const VectorConstRef& min_config, |
||
133 |
const VectorConstRef& max_config, |
||
134 |
const VectorConstRef& friction, |
||
135 |
const VectorConstRef& damping) |
||
136 |
{ |
||
137 |
JointIndex joint_id; |
||
138 |
1054 |
const Frame & frame = model.frames[parentFrameId]; |
|
139 |
|||
140 |
✗✓✓✓ ✗✗ |
1054 |
switch (type) { |
141 |
case Base::FLOATING: |
||
142 |
joint_id = model.addJoint(frame.parent, |
||
143 |
typename JointCollection::JointModelFreeFlyer(), |
||
144 |
frame.placement * placement, |
||
145 |
joint_name, |
||
146 |
max_effort,max_velocity,min_config,max_config, |
||
147 |
friction,damping |
||
148 |
); |
||
149 |
break; |
||
150 |
1047 |
case Base::REVOLUTE: |
|
151 |
✓✗ | 1047 |
joint_id = addJoint< |
152 |
typename JointCollection::JointModelRX, |
||
153 |
typename JointCollection::JointModelRY, |
||
154 |
typename JointCollection::JointModelRZ, |
||
155 |
typename JointCollection::JointModelRevoluteUnaligned> ( |
||
156 |
axis, frame, placement, joint_name, |
||
157 |
max_effort, max_velocity, min_config, max_config, |
||
158 |
friction, damping); |
||
159 |
1047 |
break; |
|
160 |
1 |
case Base::CONTINUOUS: |
|
161 |
✓✗ | 1 |
joint_id = addJoint< |
162 |
typename JointCollection::JointModelRUBX, |
||
163 |
typename JointCollection::JointModelRUBY, |
||
164 |
typename JointCollection::JointModelRUBZ, |
||
165 |
typename JointCollection::JointModelRevoluteUnboundedUnaligned>( |
||
166 |
axis, frame, placement, joint_name, |
||
167 |
max_effort, max_velocity, min_config, max_config, |
||
168 |
friction, damping); |
||
169 |
1 |
break; |
|
170 |
6 |
case Base::PRISMATIC: |
|
171 |
✓✗ | 6 |
joint_id = addJoint< |
172 |
typename JointCollection::JointModelPX, |
||
173 |
typename JointCollection::JointModelPY, |
||
174 |
typename JointCollection::JointModelPZ, |
||
175 |
typename JointCollection::JointModelPrismaticUnaligned> ( |
||
176 |
axis, frame, placement, joint_name, |
||
177 |
max_effort, max_velocity, min_config, max_config, |
||
178 |
friction, damping); |
||
179 |
6 |
break; |
|
180 |
case Base::PLANAR: |
||
181 |
joint_id = model.addJoint(frame.parent, |
||
182 |
typename JointCollection::JointModelPlanar(), |
||
183 |
frame.placement * placement, |
||
184 |
joint_name, |
||
185 |
max_effort,max_velocity,min_config,max_config, |
||
186 |
friction, damping |
||
187 |
); |
||
188 |
break; |
||
189 |
default: |
||
190 |
PINOCCHIO_CHECK_INPUT_ARGUMENT(false, "The joint type is not correct."); |
||
191 |
}; |
||
192 |
|||
193 |
✓✗ | 1054 |
FrameIndex jointFrameId = model.addJointFrame(joint_id, (int)parentFrameId); |
194 |
✓✗✓✗ |
1054 |
appendBodyToJoint(jointFrameId, Y, SE3::Identity(), body_name); |
195 |
1054 |
} |
|
196 |
|||
197 |
594 |
void addFixedJointAndBody( |
|
198 |
const FrameIndex & parent_frame_id, |
||
199 |
const SE3 & joint_placement, |
||
200 |
const std::string & joint_name, |
||
201 |
const Inertia & Y, |
||
202 |
const std::string & body_name) |
||
203 |
{ |
||
204 |
594 |
const Frame & parent_frame = model.frames[parent_frame_id]; |
|
205 |
594 |
const JointIndex parent_frame_parent = parent_frame.parent; |
|
206 |
|||
207 |
✓✗ | 594 |
const SE3 placement = parent_frame.placement * joint_placement; |
208 |
✓✗✓✗ |
594 |
FrameIndex fid = model.addFrame(Frame(joint_name, parent_frame.parent, parent_frame_id, |
209 |
placement, FIXED_JOINT, Y)); |
||
210 |
|||
211 |
✓✗ | 594 |
model.addBodyFrame(body_name, parent_frame_parent, placement, (int)fid); |
212 |
594 |
} |
|
213 |
|||
214 |
1076 |
void appendBodyToJoint( |
|
215 |
const FrameIndex fid, |
||
216 |
const Inertia& Y, |
||
217 |
const SE3 & placement, |
||
218 |
const std::string & body_name) |
||
219 |
{ |
||
220 |
1076 |
const Frame & frame = model.frames[fid]; |
|
221 |
✓✗ | 1076 |
const SE3 & p = frame.placement * placement; |
222 |
assert(frame.parent >= 0); |
||
223 |
✓✗✓✓ |
1076 |
if(!Y.isZero(Scalar(0))) |
224 |
{ |
||
225 |
✓✗ | 1054 |
model.appendBodyToJoint(frame.parent, Y, p); |
226 |
} |
||
227 |
|||
228 |
✓✗ | 1076 |
model.addBodyFrame(body_name, frame.parent, p, (int)fid); |
229 |
// Reference to model.frames[fid] can has changed because the vector |
||
230 |
// may have been reallocated. |
||
231 |
1076 |
assert (model.frames[fid].parent >= 0); |
|
232 |
{ |
||
233 |
✓✗✓✗ ✓✗✓✗ |
1076 |
assert ( !hasNaN(model.inertias[model.frames[fid].parent].lever()) |
234 |
&& !hasNaN(model.inertias[model.frames[fid].parent].inertia().data())); |
||
235 |
} |
||
236 |
1076 |
} |
|
237 |
|||
238 |
1619 |
FrameIndex getBodyId (const std::string& frame_name) const |
|
239 |
{ |
||
240 |
✓✗✓✗ |
1619 |
if (model.existFrame(frame_name, BODY)) { |
241 |
✓✗ | 1619 |
FrameIndex fid = model.getFrameId (frame_name, BODY); |
242 |
✗✓ | 1619 |
assert(model.frames[fid].type == BODY); |
243 |
1619 |
return fid; |
|
244 |
} else |
||
245 |
throw std::invalid_argument("Model does not have any body named " |
||
246 |
+ frame_name); |
||
247 |
} |
||
248 |
|||
249 |
private: |
||
250 |
/// |
||
251 |
/// \brief The four possible cartesian types of an 3D axis. |
||
252 |
/// |
||
253 |
enum CartesianAxis { AXIS_X=0, AXIS_Y=1, AXIS_Z=2, AXIS_UNALIGNED }; |
||
254 |
|||
255 |
/// |
||
256 |
/// \brief Extract the cartesian property of a particular 3D axis. |
||
257 |
/// |
||
258 |
/// \param[in] axis The input URDF axis. |
||
259 |
/// |
||
260 |
/// \return The property of the particular axis pinocchio::urdf::CartesianAxis. |
||
261 |
/// |
||
262 |
1054 |
static inline CartesianAxis extractCartesianAxis (const Vector3 & axis) |
|
263 |
{ |
||
264 |
✓✗✓✗ ✓✓ |
1054 |
if( axis == Vector3(1., 0., 0.)) |
265 |
288 |
return AXIS_X; |
|
266 |
✓✗✓✗ ✓✓ |
766 |
else if( axis == Vector3(0., 1., 0.)) |
267 |
450 |
return AXIS_Y; |
|
268 |
✓✗✓✗ ✓✓ |
316 |
else if( axis == Vector3(0., 0., 1.)) |
269 |
315 |
return AXIS_Z; |
|
270 |
else |
||
271 |
1 |
return AXIS_UNALIGNED; |
|
272 |
} |
||
273 |
|||
274 |
template <typename TypeX, typename TypeY, typename TypeZ, |
||
275 |
typename TypeUnaligned> |
||
276 |
2108 |
JointIndex addJoint( |
|
277 |
const Vector3& axis, |
||
278 |
const Frame & frame, |
||
279 |
const SE3 & placement, |
||
280 |
const std::string & joint_name, |
||
281 |
const VectorConstRef& max_effort, |
||
282 |
const VectorConstRef& max_velocity, |
||
283 |
const VectorConstRef& min_config, |
||
284 |
const VectorConstRef& max_config, |
||
285 |
const VectorConstRef& friction, |
||
286 |
const VectorConstRef& damping) |
||
287 |
{ |
||
288 |
2108 |
CartesianAxis axisType = extractCartesianAxis(axis); |
|
289 |
✓✓✓✓ ✗ |
2108 |
switch (axisType) |
290 |
{ |
||
291 |
576 |
case AXIS_X: |
|
292 |
1152 |
return model.addJoint(frame.parent, TypeX(), |
|
293 |
576 |
frame.placement * placement, joint_name, |
|
294 |
max_effort,max_velocity,min_config,max_config, |
||
295 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗ |
576 |
friction, damping); |
296 |
break; |
||
297 |
|||
298 |
900 |
case AXIS_Y: |
|
299 |
1800 |
return model.addJoint(frame.parent, TypeY(), |
|
300 |
900 |
frame.placement * placement, joint_name, |
|
301 |
max_effort,max_velocity,min_config,max_config, |
||
302 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗ |
900 |
friction, damping); |
303 |
break; |
||
304 |
|||
305 |
630 |
case AXIS_Z: |
|
306 |
1260 |
return model.addJoint(frame.parent, TypeZ(), |
|
307 |
630 |
frame.placement * placement, joint_name, |
|
308 |
max_effort,max_velocity,min_config,max_config, |
||
309 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗ |
630 |
friction, damping); |
310 |
break; |
||
311 |
|||
312 |
2 |
case AXIS_UNALIGNED: |
|
313 |
4 |
return model.addJoint(frame.parent, TypeUnaligned (axis.normalized()), |
|
314 |
2 |
frame.placement * placement, joint_name, |
|
315 |
max_effort,max_velocity,min_config,max_config, |
||
316 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ |
2 |
friction, damping); |
317 |
break; |
||
318 |
default: |
||
319 |
PINOCCHIO_CHECK_INPUT_ARGUMENT(false, "The axis type of the joint is of wrong type."); |
||
320 |
break; |
||
321 |
} |
||
322 |
} |
||
323 |
}; |
||
324 |
|||
325 |
template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl> |
||
326 |
class UrdfVisitorWithRootJoint : public UrdfVisitor<Scalar, Options, JointCollectionTpl> |
||
327 |
{ |
||
328 |
public: |
||
329 |
typedef UrdfVisitor<Scalar, Options, JointCollectionTpl> Base; |
||
330 |
typedef typename Base::Inertia Inertia; |
||
331 |
using Base::model; |
||
332 |
using Base::appendBodyToJoint; |
||
333 |
|||
334 |
typedef ModelTpl<Scalar,Options,JointCollectionTpl> Model; |
||
335 |
typedef typename Model::JointCollection JointCollection; |
||
336 |
typedef typename Model::JointModel JointModel; |
||
337 |
|||
338 |
JointModel root_joint; |
||
339 |
|||
340 |
23 |
UrdfVisitorWithRootJoint (Model& model, const JointModelBase<JointModel> & root_joint) |
|
341 |
23 |
: Base (model), root_joint (root_joint.derived()) {} |
|
342 |
|||
343 |
23 |
void addRootJoint(const Inertia & Y, const std::string & body_name) |
|
344 |
{ |
||
345 |
23 |
const Frame & frame = model.frames[0]; |
|
346 |
|||
347 |
✓✗✓✗ ✓✓✓✗ |
23 |
PINOCCHIO_THROW(!model.existJointName("root_joint"), |
348 |
std::invalid_argument, |
||
349 |
"root_joint already exists as a joint in the kinematic tree."); |
||
350 |
|||
351 |
✓✗✓✗ ✓✗ |
22 |
JointIndex idx = model.addJoint(frame.parent, root_joint, |
352 |
SE3::Identity(), "root_joint" |
||
353 |
//TODO ,max_effort,max_velocity,min_config,max_config |
||
354 |
); |
||
355 |
|||
356 |
✓✗ | 22 |
FrameIndex jointFrameId = model.addJointFrame(idx, 0); |
357 |
✓✗✓✗ |
22 |
appendBodyToJoint(jointFrameId, Y, SE3::Identity(), body_name); |
358 |
22 |
} |
|
359 |
}; |
||
360 |
|||
361 |
typedef UrdfVisitorBaseTpl<double, 0> UrdfVisitorBase; |
||
362 |
|||
363 |
void PINOCCHIO_DLLAPI parseRootTree(const ::urdf::ModelInterface * urdfTree, |
||
364 |
UrdfVisitorBase & model); |
||
365 |
|||
366 |
void PINOCCHIO_DLLAPI parseRootTree(const std::string & filename, |
||
367 |
UrdfVisitorBase & model); |
||
368 |
|||
369 |
void PINOCCHIO_DLLAPI parseRootTreeFromXML(const std::string & xmlString, |
||
370 |
UrdfVisitorBase & model); |
||
371 |
} |
||
372 |
|||
373 |
template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl> |
||
374 |
ModelTpl<Scalar,Options,JointCollectionTpl> & |
||
375 |
20 |
buildModel(const std::string & filename, |
|
376 |
const typename ModelTpl<Scalar,Options,JointCollectionTpl>::JointModel & root_joint, |
||
377 |
ModelTpl<Scalar,Options,JointCollectionTpl> & model, |
||
378 |
const bool verbose) |
||
379 |
{ |
||
380 |
✓✗ | 20 |
details::UrdfVisitorWithRootJoint<Scalar, Options, JointCollectionTpl> visitor (model, root_joint); |
381 |
✗✓ | 20 |
if (verbose) visitor.log = &std::cout; |
382 |
✓✗ | 20 |
details::parseRootTree(filename, visitor); |
383 |
40 |
return model; |
|
384 |
} |
||
385 |
|||
386 |
template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl> |
||
387 |
ModelTpl<Scalar,Options,JointCollectionTpl> & |
||
388 |
23 |
buildModel(const std::string & filename, |
|
389 |
ModelTpl<Scalar,Options,JointCollectionTpl> & model, |
||
390 |
const bool verbose) |
||
391 |
{ |
||
392 |
✓✗ | 23 |
details::UrdfVisitor<Scalar, Options, JointCollectionTpl> visitor (model); |
393 |
✗✓ | 23 |
if (verbose) visitor.log = &std::cout; |
394 |
✓✗ | 23 |
details::parseRootTree(filename, visitor); |
395 |
23 |
return model; |
|
396 |
} |
||
397 |
|||
398 |
template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl> |
||
399 |
ModelTpl<Scalar,Options,JointCollectionTpl> & |
||
400 |
2 |
buildModelFromXML(const std::string & xmlStream, |
|
401 |
const typename ModelTpl<Scalar,Options,JointCollectionTpl>::JointModel & rootJoint, |
||
402 |
ModelTpl<Scalar,Options,JointCollectionTpl> & model, |
||
403 |
const bool verbose) |
||
404 |
{ |
||
405 |
✓✗ | 3 |
details::UrdfVisitorWithRootJoint<Scalar, Options, JointCollectionTpl> visitor (model, rootJoint); |
406 |
✗✓ | 2 |
if (verbose) visitor.log = &std::cout; |
407 |
✓✓ | 2 |
details::parseRootTreeFromXML(xmlStream, visitor); |
408 |
2 |
return model; |
|
409 |
} |
||
410 |
|||
411 |
template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl> |
||
412 |
ModelTpl<Scalar,Options,JointCollectionTpl> & |
||
413 |
5 |
buildModelFromXML(const std::string & xmlStream, |
|
414 |
ModelTpl<Scalar,Options,JointCollectionTpl> & model, |
||
415 |
const bool verbose) |
||
416 |
{ |
||
417 |
✓✗ | 5 |
details::UrdfVisitor<Scalar, Options, JointCollectionTpl> visitor (model); |
418 |
✗✓ | 5 |
if (verbose) visitor.log = &std::cout; |
419 |
✓✗ | 5 |
details::parseRootTreeFromXML(xmlStream, visitor); |
420 |
5 |
return model; |
|
421 |
} |
||
422 |
|||
423 |
template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl> |
||
424 |
ModelTpl<Scalar,Options,JointCollectionTpl> & |
||
425 |
buildModel(const boost::shared_ptr< ::urdf::ModelInterface> urdfTree, |
||
426 |
const typename ModelTpl<Scalar,Options,JointCollectionTpl>::JointModel & rootJoint, |
||
427 |
ModelTpl<Scalar,Options,JointCollectionTpl> & model, |
||
428 |
const bool verbose) |
||
429 |
{ |
||
430 |
PINOCCHIO_CHECK_INPUT_ARGUMENT(urdfTree != NULL); |
||
431 |
details::UrdfVisitorWithRootJoint<Scalar, Options, JointCollectionTpl> visitor (model, rootJoint); |
||
432 |
if (verbose) visitor.log = &std::cout; |
||
433 |
details::parseRootTree(urdfTree.get(), visitor); |
||
434 |
return model; |
||
435 |
} |
||
436 |
|||
437 |
template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl> |
||
438 |
ModelTpl<Scalar,Options,JointCollectionTpl> & |
||
439 |
buildModel(const boost::shared_ptr< ::urdf::ModelInterface> urdfTree, |
||
440 |
ModelTpl<Scalar,Options,JointCollectionTpl> & model, |
||
441 |
const bool verbose) |
||
442 |
{ |
||
443 |
PINOCCHIO_CHECK_INPUT_ARGUMENT(urdfTree != NULL); |
||
444 |
details::UrdfVisitor<Scalar, Options, JointCollectionTpl> visitor (model); |
||
445 |
if (verbose) visitor.log = &std::cout; |
||
446 |
details::parseRootTree(urdfTree.get(), visitor); |
||
447 |
return model; |
||
448 |
} |
||
449 |
|||
450 |
#ifdef PINOCCHIO_WITH_CXX11_SUPPORT |
||
451 |
template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl> |
||
452 |
ModelTpl<Scalar,Options,JointCollectionTpl> & |
||
453 |
1 |
buildModel(const std::shared_ptr< ::urdf::ModelInterface> urdfTree, |
|
454 |
const typename ModelTpl<Scalar,Options,JointCollectionTpl>::JointModel & rootJoint, |
||
455 |
ModelTpl<Scalar,Options,JointCollectionTpl> & model, |
||
456 |
const bool verbose) |
||
457 |
{ |
||
458 |
✗✓✗✗ |
1 |
PINOCCHIO_CHECK_INPUT_ARGUMENT(urdfTree != NULL); |
459 |
✓✗ | 1 |
details::UrdfVisitorWithRootJoint<Scalar, Options, JointCollectionTpl> visitor (model, rootJoint); |
460 |
✗✓ | 1 |
if (verbose) visitor.log = &std::cout; |
461 |
✓✗ | 1 |
details::parseRootTree(urdfTree.get(), visitor); |
462 |
2 |
return model; |
|
463 |
} |
||
464 |
|||
465 |
template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl> |
||
466 |
ModelTpl<Scalar,Options,JointCollectionTpl> & |
||
467 |
1 |
buildModel(const std::shared_ptr< ::urdf::ModelInterface> urdfTree, |
|
468 |
ModelTpl<Scalar,Options,JointCollectionTpl> & model, |
||
469 |
const bool verbose) |
||
470 |
{ |
||
471 |
✗✓✗✗ |
1 |
PINOCCHIO_CHECK_INPUT_ARGUMENT(urdfTree != NULL); |
472 |
✓✗ | 1 |
details::UrdfVisitor<Scalar, Options, JointCollectionTpl> visitor (model); |
473 |
✗✓ | 1 |
if (verbose) visitor.log = &std::cout; |
474 |
✓✗ | 1 |
details::parseRootTree(urdfTree.get(), visitor); |
475 |
1 |
return model; |
|
476 |
} |
||
477 |
#endif |
||
478 |
|||
479 |
} // namespace urdf |
||
480 |
} // namespace pinocchio |
||
481 |
|||
482 |
#endif // ifndef __pinocchio_multibody_parsers_urdf_model_hxx__ |
| Generated by: GCOVR (Version 4.2) |