GCC Code Coverage Report

Directory: ./
File: src/parsers/urdf/model.cpp
Date: 2025-04-30 16:14:33
Exec Total Coverage
Lines: 117 181 64.6%
Branches: 191 575 33.2%
Line Branch Exec Source
1 //
2 // Copyright (c) 2015-2020 CNRS
3 // Copyright (c) 2015 Wandercraft, 86 rue de Paris 91400 Orsay, France.
4 //
5
6 #include "pinocchio/parsers/urdf.hpp"
7 #include "pinocchio/parsers/urdf/utils.hpp"
8 #include "pinocchio/parsers/urdf/model.hxx"
9
10 #include <urdf_model/model.h>
11 #include <urdf_parser/urdf_parser.h>
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 ///
24 /// \brief Convert URDF Inertial quantity to Spatial Inertia.
25 ///
26 /// \param[in] Y The input URDF Inertia.
27 ///
28 /// \return The converted Spatial Inertia pinocchio::Inertia.
29 ///
30 2709 static Inertia convertFromUrdf(const ::urdf::Inertial & Y)
31 {
32 2709 const ::urdf::Vector3 & p = Y.origin.position;
33 2709 const ::urdf::Rotation & q = Y.origin.rotation;
34
35
1/2
✓ Branch 1 taken 2709 times.
✗ Branch 2 not taken.
 2709 const Inertia::Vector3 com(p.x, p.y, p.z);
36 const Inertia::Matrix3 & R =
37
3/6
✓ Branch 1 taken 2709 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2709 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 2709 times.
✗ Branch 8 not taken.
 2709 Eigen::Quaterniond(q.w, q.x, q.y, q.z).cast<Inertia::Scalar>().matrix();
38
39
1/2
✓ Branch 1 taken 2709 times.
✗ Branch 2 not taken.
 2709 Inertia::Matrix3 I;
40
9/18
✓ Branch 1 taken 2709 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2709 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 2709 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 2709 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 2709 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 2709 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 2709 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 2709 times.
✗ Branch 23 not taken.
✓ Branch 25 taken 2709 times.
✗ Branch 26 not taken.
 2709 I << Y.ixx, Y.ixy, Y.ixz, Y.ixy, Y.iyy, Y.iyz, Y.ixz, Y.iyz, Y.izz;
41
5/10
✓ Branch 1 taken 2709 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2709 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 2709 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 2709 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 2709 times.
✗ Branch 14 not taken.
 2709 return Inertia(Y.mass, com, R * I * R.transpose());
42 }
43
44 3281 static Inertia convertFromUrdf(const ::urdf::InertialSharedPtr & Y)
45 {
46
2/2
✓ Branch 1 taken 2709 times.
✓ Branch 2 taken 572 times.
 3281 if (Y)
47 2709 return convertFromUrdf(*Y);
48 572 return Inertia::Zero();
49 }
50
51 static FrameIndex
52 3196 getParentLinkFrame(const ::urdf::LinkConstSharedPtr link, UrdfVisitorBase & model)
53 {
54
4/10
✓ Branch 1 taken 3196 times.
✗ Branch 2 not taken.
✗ Branch 6 not taken.
✓ Branch 7 taken 3196 times.
✓ Branch 8 taken 3196 times.
✗ Branch 9 not taken.
✗ Branch 11 not taken.
✓ Branch 12 taken 3196 times.
✗ Branch 15 not taken.
✗ Branch 16 not taken.
 3196 PINOCCHIO_CHECK_INPUT_ARGUMENT(link && link->getParent());
55
1/2
✓ Branch 4 taken 3196 times.
✗ Branch 5 not taken.
 3196 FrameIndex id = model.getBodyId(link->getParent()->name);
56 3196 return id;
57 }
58
59 ///
60 /// \brief Recursive procedure for reading the URDF tree.
61 /// The function returns an exception as soon as a necessary Inertia or Joint
62 /// information are missing.
63 ///
64 /// \param[in] link The current URDF link.
65 /// \param[in] model The model where the link must be added.
66 ///
67 3196 void parseTree(::urdf::LinkConstSharedPtr link, UrdfVisitorBase & model, const bool mimic)
68 {
69 typedef UrdfVisitorBase::Scalar Scalar;
70 typedef UrdfVisitorBase::SE3 SE3;
71 typedef UrdfVisitorBase::Vector Vector;
72 typedef UrdfVisitorBase::Vector3 Vector3;
73 typedef Model::FrameIndex FrameIndex;
74
75 // Parent joint of the current body
76 const ::urdf::JointConstSharedPtr joint =
77 3196 ::urdf::const_pointer_cast<::urdf::Joint>(link->parent_joint);
78
79
1/2
✓ Branch 1 taken 3196 times.
✗ Branch 2 not taken.
 3196 if (joint) // if the link is not the root of the tree
80 {
81
1/4
✗ Branch 4 not taken.
✓ Branch 5 taken 3196 times.
✗ Branch 8 not taken.
✗ Branch 9 not taken.
 3196 PINOCCHIO_CHECK_INPUT_ARGUMENT(link->getParent());
82
83 3196 const std::string & joint_name = joint->name;
84 3196 const std::string & link_name = link->name;
85 3196 const std::string & parent_link_name = link->getParent()->name;
86
1/2
✓ Branch 1 taken 3196 times.
✗ Branch 2 not taken.
 3196 std::ostringstream joint_info;
87
88
1/2
✓ Branch 2 taken 3196 times.
✗ Branch 3 not taken.
 3196 FrameIndex parentFrameId = getParentLinkFrame(link, model);
89
90 // Transformation from the parent link to the joint origin
91 const SE3 jointPlacement =
92
2/4
✓ Branch 2 taken 3196 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 3196 times.
✗ Branch 6 not taken.
 3196 convertFromUrdf(joint->parent_to_joint_origin_transform).cast<Scalar>();
93
94
2/4
✓ Branch 2 taken 3196 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 3196 times.
✗ Branch 6 not taken.
 3196 const Inertia Y = convertFromUrdf(link->inertial).cast<Scalar>();
95
96
4/8
✓ Branch 1 taken 3196 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 3196 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 3196 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 3196 times.
✗ Branch 11 not taken.
 3196 Vector max_effort(1), max_velocity(1), min_config(1), max_config(1);
97
4/8
✓ Branch 1 taken 3196 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 3196 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 3196 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 3196 times.
✗ Branch 11 not taken.
 3196 Vector friction(Vector::Constant(1, 0.)), damping(Vector::Constant(1, 0.));
98
1/2
✓ Branch 4 taken 3196 times.
✗ Branch 5 not taken.
 3196 Vector3 axis(joint->axis.x, joint->axis.y, joint->axis.z);
99
100 3196 const Scalar infty = std::numeric_limits<Scalar>::infinity();
101
102
4/7
✗ Branch 1 not taken.
✓ Branch 2 taken 2075 times.
✓ Branch 3 taken 1 times.
✓ Branch 4 taken 28 times.
✗ Branch 5 not taken.
✓ Branch 6 taken 1092 times.
✗ Branch 7 not taken.
 3196 switch (joint->type)
103 {
104 ✗ case ::urdf::Joint::FLOATING:
105 ✗ joint_info << "joint FreeFlyer";
106
107 ✗ max_effort = Vector::Constant(6, infty);
108 ✗ max_velocity = Vector::Constant(6, infty);
109 ✗ min_config = Vector::Constant(7, -infty);
110 ✗ max_config = Vector::Constant(7, infty);
111 ✗ min_config.tail<4>().setConstant(-1.01);
112 ✗ max_config.tail<4>().setConstant(1.01);
113
114 ✗ friction = Vector::Constant(6, 0.);
115 ✗ damping = Vector::Constant(6, 0.);
116
117 ✗ model.addJointAndBody(
118 ✗ UrdfVisitorBase::FLOATING, axis, parentFrameId, jointPlacement, joint->name, Y,
119 ✗ link->name, max_effort, max_velocity, min_config, max_config, friction, damping);
120 ✗ break;
121
122 2075 case ::urdf::Joint::REVOLUTE:
123
1/2
✓ Branch 1 taken 2075 times.
✗ Branch 2 not taken.
 2075 joint_info << "joint REVOLUTE with axis";
124
125 // TODO I think the URDF standard forbids REVOLUTE with no limits.
126
1/2
✗ Branch 2 not taken.
✓ Branch 3 taken 2075 times.
 2075 assert(joint->limits);
127
1/2
✓ Branch 2 taken 2075 times.
✗ Branch 3 not taken.
 2075 if (joint->limits)
128 {
129
1/2
✓ Branch 3 taken 2075 times.
✗ Branch 4 not taken.
 2075 max_effort << joint->limits->effort;
130
1/2
✓ Branch 3 taken 2075 times.
✗ Branch 4 not taken.
 2075 max_velocity << joint->limits->velocity;
131
1/2
✓ Branch 3 taken 2075 times.
✗ Branch 4 not taken.
 2075 min_config << joint->limits->lower;
132
1/2
✓ Branch 3 taken 2075 times.
✗ Branch 4 not taken.
 2075 max_config << joint->limits->upper;
133 }
134
135
2/2
✓ Branch 2 taken 274 times.
✓ Branch 3 taken 1801 times.
 2075 if (joint->dynamics)
136 {
137
1/2
✓ Branch 3 taken 274 times.
✗ Branch 4 not taken.
 274 friction << joint->dynamics->friction;
138
1/2
✓ Branch 3 taken 274 times.
✗ Branch 4 not taken.
 274 damping << joint->dynamics->damping;
139 }
140
4/6
✓ Branch 2 taken 286 times.
✓ Branch 3 taken 1789 times.
✗ Branch 4 not taken.
✓ Branch 5 taken 286 times.
✗ Branch 6 not taken.
✓ Branch 7 taken 2075 times.
 2075 if (joint->mimic && mimic)
141 {
142 ✗ max_effort = Vector::Constant(0, infty);
143 ✗ max_velocity = Vector::Constant(0, infty);
144 ✗ min_config = Vector::Constant(0, -infty);
145 ✗ max_config = Vector::Constant(0, infty);
146
147 ✗ friction = Vector::Constant(0, 0.);
148 ✗ damping = Vector::Constant(0, 0.);
149
150 MimicInfo_ mimic_info(
151 ✗ joint->mimic->joint_name, joint->mimic->multiplier, joint->mimic->offset, axis,
152 ✗ UrdfVisitorBase::REVOLUTE);
153
154 ✗ model.addJointAndBody(
155 ✗ UrdfVisitorBase::MIMIC, axis, parentFrameId, jointPlacement, joint->name, Y,
156 ✗ link->name, max_effort, max_velocity, min_config, max_config, friction, damping,
157 mimic_info);
158 }
159 else
160
6/12
✓ Branch 2 taken 2075 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 2075 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 2075 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 2075 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 2075 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 2075 times.
✗ Branch 18 not taken.
 4150 model.addJointAndBody(
161
1/2
✓ Branch 1 taken 2075 times.
✗ Branch 2 not taken.
 2075 UrdfVisitorBase::REVOLUTE, axis, parentFrameId, jointPlacement, joint->name, Y,
162 2075 link->name, max_effort, max_velocity, min_config, max_config, friction, damping);
163 2075 break;
164
165 1 case ::urdf::Joint::CONTINUOUS: // Revolute joint with no joint limits
166
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 joint_info << "joint CONTINUOUS with axis";
167
168
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 min_config.resize(2);
169
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 max_config.resize(2);
170
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 1 min_config << -1.01, -1.01;
171
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 1 max_config << 1.01, 1.01;
172
173
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
 1 if (joint->limits)
174 {
175
1/2
✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
 1 max_effort << joint->limits->effort;
176
1/2
✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
 1 max_velocity << joint->limits->velocity;
177 }
178 else
179 {
180 ✗ max_effort << infty;
181 ✗ max_velocity << infty;
182 }
183
184
1/2
✗ Branch 2 not taken.
✓ Branch 3 taken 1 times.
 1 if (joint->dynamics)
185 {
186 ✗ friction << joint->dynamics->friction;
187 ✗ damping << joint->dynamics->damping;
188 }
189
190
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.
 2 model.addJointAndBody(
191
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 UrdfVisitorBase::CONTINUOUS, axis, parentFrameId, jointPlacement, joint->name, Y,
192 1 link->name, max_effort, max_velocity, min_config, max_config, friction, damping);
193 1 break;
194
195 28 case ::urdf::Joint::PRISMATIC:
196
1/2
✓ Branch 1 taken 28 times.
✗ Branch 2 not taken.
 28 joint_info << "joint PRISMATIC with axis";
197
198 // TODO I think the URDF standard forbids REVOLUTE with no limits.
199
1/2
✗ Branch 2 not taken.
✓ Branch 3 taken 28 times.
 28 assert(joint->limits);
200
1/2
✓ Branch 2 taken 28 times.
✗ Branch 3 not taken.
 28 if (joint->limits)
201 {
202
1/2
✓ Branch 3 taken 28 times.
✗ Branch 4 not taken.
 28 max_effort << joint->limits->effort;
203
1/2
✓ Branch 3 taken 28 times.
✗ Branch 4 not taken.
 28 max_velocity << joint->limits->velocity;
204
1/2
✓ Branch 3 taken 28 times.
✗ Branch 4 not taken.
 28 min_config << joint->limits->lower;
205
1/2
✓ Branch 3 taken 28 times.
✗ Branch 4 not taken.
 28 max_config << joint->limits->upper;
206 }
207
208
2/2
✓ Branch 2 taken 6 times.
✓ Branch 3 taken 22 times.
 28 if (joint->dynamics)
209 {
210
1/2
✓ Branch 3 taken 6 times.
✗ Branch 4 not taken.
 6 friction << joint->dynamics->friction;
211
1/2
✓ Branch 3 taken 6 times.
✗ Branch 4 not taken.
 6 damping << joint->dynamics->damping;
212 }
213
4/6
✓ Branch 2 taken 3 times.
✓ Branch 3 taken 25 times.
✗ Branch 4 not taken.
✓ Branch 5 taken 3 times.
✗ Branch 6 not taken.
✓ Branch 7 taken 28 times.
 28 if (joint->mimic && mimic)
214 {
215 ✗ max_effort = Vector::Constant(0, infty);
216 ✗ max_velocity = Vector::Constant(0, infty);
217 ✗ min_config = Vector::Constant(0, -infty);
218 ✗ max_config = Vector::Constant(0, infty);
219
220 ✗ friction = Vector::Constant(0, 0.);
221 ✗ damping = Vector::Constant(0, 0.);
222
223 MimicInfo_ mimic_info(
224 ✗ joint->mimic->joint_name, joint->mimic->multiplier, joint->mimic->offset, axis,
225 ✗ UrdfVisitorBase::PRISMATIC);
226
227 ✗ model.addJointAndBody(
228 ✗ UrdfVisitorBase::MIMIC, axis, parentFrameId, jointPlacement, joint->name, Y,
229 ✗ link->name, max_effort, max_velocity, min_config, max_config, friction, damping,
230 mimic_info);
231 }
232 else
233
6/12
✓ Branch 2 taken 28 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 28 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 28 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 28 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 28 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 28 times.
✗ Branch 18 not taken.
 56 model.addJointAndBody(
234
1/2
✓ Branch 1 taken 28 times.
✗ Branch 2 not taken.
 28 UrdfVisitorBase::PRISMATIC, axis, parentFrameId, jointPlacement, joint->name, Y,
235 28 link->name, max_effort, max_velocity, min_config, max_config, friction, damping);
236 28 break;
237
238 ✗ case ::urdf::Joint::PLANAR:
239 ✗ joint_info << "joint PLANAR with normal axis along Z";
240
241 ✗ max_effort = Vector::Constant(3, infty);
242 ✗ max_velocity = Vector::Constant(3, infty);
243 ✗ min_config = Vector::Constant(4, -infty);
244 ✗ max_config = Vector::Constant(4, infty);
245 ✗ min_config.tail<2>().setConstant(-1.01);
246 ✗ max_config.tail<2>().setConstant(1.01);
247
248 ✗ friction = Vector::Constant(3, 0.);
249 ✗ damping = Vector::Constant(3, 0.);
250
251 ✗ model.addJointAndBody(
252 ✗ UrdfVisitorBase::PLANAR, axis, parentFrameId, jointPlacement, joint->name, Y,
253 ✗ link->name, max_effort, max_velocity, min_config, max_config, friction, damping);
254 ✗ break;
255
256 1092 case ::urdf::Joint::FIXED:
257 // In case of fixed joint, if link has inertial tag:
258 // -add the inertia of the link to his parent in the model
259 // Otherwise do nothing.
260 // In all cases:
261 // -let all the children become children of parent
262 // -inform the parser of the offset to apply
263 // -add fixed body in model to display it in gepetto-viewer
264
265
1/2
✓ Branch 1 taken 1092 times.
✗ Branch 2 not taken.
 1092 joint_info << "fixed joint";
266
1/2
✓ Branch 1 taken 1092 times.
✗ Branch 2 not taken.
 1092 model.addFixedJointAndBody(parentFrameId, jointPlacement, joint_name, Y, link_name);
267 1092 break;
268
269 ✗ default:
270 ✗ throw std::invalid_argument("The type of joint " + joint_name + " is not supported.");
271 break;
272 }
273
274
2/4
✓ Branch 1 taken 3196 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 3196 times.
✗ Branch 5 not taken.
 3196 model << "Adding Body" << '\n'
275
4/8
✓ Branch 1 taken 3196 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 3196 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 3196 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 3196 times.
✗ Branch 11 not taken.
 3196 << '\"' << link_name << "\" connected to \"" << parent_link_name
276
3/6
✓ Branch 1 taken 3196 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 3196 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 3196 times.
✗ Branch 8 not taken.
 3196 << "\" through joint \"" << joint_name << "\"\n"
277
4/8
✓ Branch 1 taken 3196 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 3196 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 3196 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 3196 times.
✗ Branch 11 not taken.
 3196 << "joint type: " << joint_info.str() << '\n'
278
1/2
✓ Branch 1 taken 3196 times.
✗ Branch 2 not taken.
 3196 << "joint placement:\n"
279
2/4
✓ Branch 1 taken 3196 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 3196 times.
✗ Branch 5 not taken.
 3196 << jointPlacement << '\n'
280
2/4
✓ Branch 1 taken 3196 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 3196 times.
✗ Branch 5 not taken.
 3196 << "body info: " << '\n'
281
3/6
✓ Branch 1 taken 3196 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 3196 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 3196 times.
✗ Branch 9 not taken.
 3196 << " mass: " << Y.mass() << '\n'
282
4/8
✓ Branch 1 taken 3196 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 3196 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 3196 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 3196 times.
✗ Branch 12 not taken.
 3196 << " lever: " << Y.lever().transpose() << '\n'
283
1/2
✓ Branch 1 taken 3196 times.
✗ Branch 2 not taken.
 3196 << " inertia elements (Ixx,Iyx,Iyy,Izx,Izy,Izz): "
284
3/6
✓ Branch 3 taken 3196 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 3196 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 3196 times.
✗ Branch 10 not taken.
 3196 << Y.inertia().data().transpose() << '\n'
285
1/2
✓ Branch 1 taken 3196 times.
✗ Branch 2 not taken.
 3196 << '\n';
286 3196 }
287 ✗ else if (link->getParent())
288 ✗ throw std::invalid_argument(link->name + " - joint information missing.");
289
290
18/30
✓ Branch 2 taken 3196 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 3196 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 3196 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 3196 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 3196 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 3196 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 3064 times.
✗ Branch 21 not taken.
✓ Branch 23 taken 3064 times.
✗ Branch 24 not taken.
✓ Branch 26 taken 3064 times.
✓ Branch 27 taken 3064 times.
✓ Branch 28 taken 3064 times.
✗ Branch 29 not taken.
✓ Branch 31 taken 3064 times.
✗ Branch 32 not taken.
✓ Branch 33 taken 6260 times.
✗ Branch 34 not taken.
✓ Branch 36 taken 6260 times.
✗ Branch 37 not taken.
✓ Branch 38 taken 3064 times.
✓ Branch 39 taken 3196 times.
✓ Branch 40 taken 3064 times.
✓ Branch 41 taken 3196 times.
 9324 BOOST_FOREACH (::urdf::LinkConstSharedPtr child, link->child_links)
291 {
292
1/2
✓ Branch 2 taken 3064 times.
✗ Branch 3 not taken.
 3064 parseTree(child, model, mimic);
293 3064 }
294 3196 }
295
296 ///
297 /// \brief Parse a tree with a specific root joint linking the model to the environment.
298 /// The function returns an exception as soon as a necessary Inertia or Joint
299 /// information are missing.
300 ///
301 /// \param[in] link The current URDF link.
302 /// \param[in] model The model where the link must be added.
303 ///
304 85 void parseRootTree(
305 const ::urdf::ModelInterface * urdfTree, UrdfVisitorBase & model, const bool mimic)
306 {
307
1/2
✓ Branch 2 taken 85 times.
✗ Branch 3 not taken.
 85 model.setName(urdfTree->getName());
308
309 85 ::urdf::LinkConstSharedPtr root_link = urdfTree->getRoot();
310
4/6
✓ Branch 3 taken 85 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 85 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 84 times.
✓ Branch 10 taken 1 times.
 85 model.addRootJoint(convertFromUrdf(root_link->inertial).cast<double>(), root_link->name);
311
312
18/30
✓ Branch 2 taken 84 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 84 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 84 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 84 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 84 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 84 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 132 times.
✗ Branch 21 not taken.
✓ Branch 23 taken 132 times.
✗ Branch 24 not taken.
✓ Branch 26 taken 132 times.
✓ Branch 27 taken 132 times.
✓ Branch 28 taken 132 times.
✗ Branch 29 not taken.
✓ Branch 31 taken 132 times.
✗ Branch 32 not taken.
✓ Branch 33 taken 216 times.
✗ Branch 34 not taken.
✓ Branch 36 taken 216 times.
✗ Branch 37 not taken.
✓ Branch 38 taken 132 times.
✓ Branch 39 taken 84 times.
✓ Branch 40 taken 132 times.
✓ Branch 41 taken 84 times.
 348 BOOST_FOREACH (::urdf::LinkConstSharedPtr child, root_link->child_links)
313 {
314
1/2
✓ Branch 2 taken 132 times.
✗ Branch 3 not taken.
 132 parseTree(child, model, mimic);
315 132 }
316 85 }
317
318 74 void parseRootTree(const std::string & filename, UrdfVisitorBase & model, const bool mimic)
319 {
320
1/2
✓ Branch 1 taken 74 times.
✗ Branch 2 not taken.
 74 ::urdf::ModelInterfaceSharedPtr urdfTree = ::urdf::parseURDFFile(filename);
321
1/2
✓ Branch 1 taken 74 times.
✗ Branch 2 not taken.
 74 if (urdfTree)
322
1/2
✓ Branch 2 taken 74 times.
✗ Branch 3 not taken.
 148 return parseRootTree(urdfTree.get(), model, mimic);
323 else
324 ✗ throw std::invalid_argument(
325 ✗ "The file " + filename
326 ✗ + " does not "
327 ✗ "contain a valid URDF model.");
328 74 }
329
330 void
331 9 parseRootTreeFromXML(const std::string & xmlString, UrdfVisitorBase & model, const bool mimic)
332 {
333
1/2
✓ Branch 1 taken 9 times.
✗ Branch 2 not taken.
 9 ::urdf::ModelInterfaceSharedPtr urdfTree = ::urdf::parseURDF(xmlString);
334
1/2
✓ Branch 1 taken 9 times.
✗ Branch 2 not taken.
 9 if (urdfTree)
335
2/2
✓ Branch 2 taken 8 times.
✓ Branch 3 taken 1 times.
 17 return parseRootTree(urdfTree.get(), model, mimic);
336 else
337 ✗ throw std::invalid_argument("The XML stream does not contain a valid "
338 ✗ "URDF model.");
339 9 }
340 } // namespace details
341 } // namespace urdf
342 } // namespace pinocchio
343