Directory: | ./ |
---|---|
File: | src/parsers/urdf/model.cpp |
Date: | 2024-08-27 18:20:05 |
Exec | Total | Coverage | |
---|---|---|---|
Lines: | 115 | 157 | 73.2% |
Branches: | 183 | 471 | 38.9% |
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 | 1512 | static Inertia convertFromUrdf(const ::urdf::Inertial & Y) | |
31 | { | ||
32 | 1512 | const ::urdf::Vector3 & p = Y.origin.position; | |
33 | 1512 | const ::urdf::Rotation & q = Y.origin.rotation; | |
34 | |||
35 |
1/2✓ Branch 1 taken 1512 times.
✗ Branch 2 not taken.
|
1512 | const Inertia::Vector3 com(p.x, p.y, p.z); |
36 | const Inertia::Matrix3 & R = | ||
37 |
3/6✓ Branch 1 taken 1512 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1512 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1512 times.
✗ Branch 8 not taken.
|
1512 | Eigen::Quaterniond(q.w, q.x, q.y, q.z).cast<Inertia::Scalar>().matrix(); |
38 | |||
39 |
1/2✓ Branch 1 taken 1512 times.
✗ Branch 2 not taken.
|
1512 | Inertia::Matrix3 I; |
40 |
9/18✓ Branch 1 taken 1512 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1512 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1512 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 1512 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 1512 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 1512 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 1512 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 1512 times.
✗ Branch 23 not taken.
✓ Branch 25 taken 1512 times.
✗ Branch 26 not taken.
|
1512 | 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 1512 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1512 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1512 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 1512 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 1512 times.
✗ Branch 14 not taken.
|
1512 | return Inertia(Y.mass, com, R * I * R.transpose()); |
42 | } | ||
43 | |||
44 | 2055 | static Inertia convertFromUrdf(const ::urdf::InertialSharedPtr & Y) | |
45 | { | ||
46 |
2/2✓ Branch 1 taken 1512 times.
✓ Branch 2 taken 543 times.
|
2055 | if (Y) |
47 | 1512 | return convertFromUrdf(*Y); | |
48 | 543 | return Inertia::Zero(); | |
49 | } | ||
50 | |||
51 | static FrameIndex | ||
52 | 1995 | getParentLinkFrame(const ::urdf::LinkConstSharedPtr link, UrdfVisitorBase & model) | |
53 | { | ||
54 |
4/10✓ Branch 1 taken 1995 times.
✗ Branch 2 not taken.
✗ Branch 6 not taken.
✓ Branch 7 taken 1995 times.
✓ Branch 8 taken 1995 times.
✗ Branch 9 not taken.
✗ Branch 11 not taken.
✓ Branch 12 taken 1995 times.
✗ Branch 15 not taken.
✗ Branch 16 not taken.
|
1995 | PINOCCHIO_CHECK_INPUT_ARGUMENT(link && link->getParent()); |
55 |
1/2✓ Branch 4 taken 1995 times.
✗ Branch 5 not taken.
|
1995 | FrameIndex id = model.getBodyId(link->getParent()->name); |
56 | 1995 | 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 | 1995 | void parseTree(::urdf::LinkConstSharedPtr link, UrdfVisitorBase & model) | |
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 | 1995 | ::urdf::const_pointer_cast<::urdf::Joint>(link->parent_joint); | |
78 | |||
79 |
1/2✓ Branch 1 taken 1995 times.
✗ Branch 2 not taken.
|
1995 | if (joint) // if the link is not the root of the tree |
80 | { | ||
81 |
1/4✗ Branch 4 not taken.
✓ Branch 5 taken 1995 times.
✗ Branch 8 not taken.
✗ Branch 9 not taken.
|
1995 | PINOCCHIO_CHECK_INPUT_ARGUMENT(link->getParent()); |
82 | |||
83 | 1995 | const std::string & joint_name = joint->name; | |
84 | 1995 | const std::string & link_name = link->name; | |
85 | 1995 | const std::string & parent_link_name = link->getParent()->name; | |
86 |
1/2✓ Branch 1 taken 1995 times.
✗ Branch 2 not taken.
|
1995 | std::ostringstream joint_info; |
87 | |||
88 |
1/2✓ Branch 2 taken 1995 times.
✗ Branch 3 not taken.
|
1995 | 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 1995 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1995 times.
✗ Branch 6 not taken.
|
1995 | convertFromUrdf(joint->parent_to_joint_origin_transform).cast<Scalar>(); |
93 | |||
94 |
2/4✓ Branch 2 taken 1995 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1995 times.
✗ Branch 6 not taken.
|
1995 | const Inertia Y = convertFromUrdf(link->inertial).cast<Scalar>(); |
95 | |||
96 |
4/8✓ Branch 1 taken 1995 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1995 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1995 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 1995 times.
✗ Branch 11 not taken.
|
1995 | Vector max_effort(1), max_velocity(1), min_config(1), max_config(1); |
97 |
4/8✓ Branch 1 taken 1995 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1995 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1995 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 1995 times.
✗ Branch 11 not taken.
|
1995 | Vector friction(Vector::Constant(1, 0.)), damping(Vector::Constant(1, 0.)); |
98 |
1/2✓ Branch 4 taken 1995 times.
✗ Branch 5 not taken.
|
1995 | Vector3 axis(joint->axis.x, joint->axis.y, joint->axis.z); |
99 | |||
100 | 1995 | const Scalar infty = std::numeric_limits<Scalar>::infinity(); | |
101 | |||
102 |
4/7✗ Branch 1 not taken.
✓ Branch 2 taken 1264 times.
✓ Branch 3 taken 1 times.
✓ Branch 4 taken 7 times.
✗ Branch 5 not taken.
✓ Branch 6 taken 723 times.
✗ Branch 7 not taken.
|
1995 | 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 | 1264 | case ::urdf::Joint::REVOLUTE: | |
123 |
1/2✓ Branch 1 taken 1264 times.
✗ Branch 2 not taken.
|
1264 | 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 1264 times.
|
1264 | assert(joint->limits); |
127 |
1/2✓ Branch 2 taken 1264 times.
✗ Branch 3 not taken.
|
1264 | if (joint->limits) |
128 | { | ||
129 |
1/2✓ Branch 3 taken 1264 times.
✗ Branch 4 not taken.
|
1264 | max_effort << joint->limits->effort; |
130 |
1/2✓ Branch 3 taken 1264 times.
✗ Branch 4 not taken.
|
1264 | max_velocity << joint->limits->velocity; |
131 |
1/2✓ Branch 3 taken 1264 times.
✗ Branch 4 not taken.
|
1264 | min_config << joint->limits->lower; |
132 |
1/2✓ Branch 3 taken 1264 times.
✗ Branch 4 not taken.
|
1264 | max_config << joint->limits->upper; |
133 | } | ||
134 | |||
135 |
2/2✓ Branch 2 taken 236 times.
✓ Branch 3 taken 1028 times.
|
1264 | if (joint->dynamics) |
136 | { | ||
137 |
1/2✓ Branch 3 taken 236 times.
✗ Branch 4 not taken.
|
236 | friction << joint->dynamics->friction; |
138 |
1/2✓ Branch 3 taken 236 times.
✗ Branch 4 not taken.
|
236 | damping << joint->dynamics->damping; |
139 | } | ||
140 | |||
141 |
6/12✓ Branch 1 taken 1264 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1264 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1264 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 1264 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 1264 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 1264 times.
✗ Branch 17 not taken.
|
2528 | model.addJointAndBody( |
142 |
1/2✓ Branch 1 taken 1264 times.
✗ Branch 2 not taken.
|
1264 | UrdfVisitorBase::REVOLUTE, axis, parentFrameId, jointPlacement, joint->name, Y, |
143 | 1264 | link->name, max_effort, max_velocity, min_config, max_config, friction, damping); | |
144 | 1264 | break; | |
145 | |||
146 | 1 | case ::urdf::Joint::CONTINUOUS: // Revolute joint with no joint limits | |
147 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | joint_info << "joint CONTINUOUS with axis"; |
148 | |||
149 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | min_config.resize(2); |
150 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | max_config.resize(2); |
151 |
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; |
152 |
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; |
153 | |||
154 |
1/2✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
|
1 | if (joint->limits) |
155 | { | ||
156 |
1/2✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
|
1 | max_effort << joint->limits->effort; |
157 |
1/2✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
|
1 | max_velocity << joint->limits->velocity; |
158 | } | ||
159 | else | ||
160 | { | ||
161 | ✗ | max_effort << infty; | |
162 | ✗ | max_velocity << infty; | |
163 | } | ||
164 | |||
165 |
1/2✗ Branch 2 not taken.
✓ Branch 3 taken 1 times.
|
1 | if (joint->dynamics) |
166 | { | ||
167 | ✗ | friction << joint->dynamics->friction; | |
168 | ✗ | damping << joint->dynamics->damping; | |
169 | } | ||
170 | |||
171 |
6/12✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 1 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 1 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 1 times.
✗ Branch 17 not taken.
|
2 | model.addJointAndBody( |
172 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | UrdfVisitorBase::CONTINUOUS, axis, parentFrameId, jointPlacement, joint->name, Y, |
173 | 1 | link->name, max_effort, max_velocity, min_config, max_config, friction, damping); | |
174 | 1 | break; | |
175 | |||
176 | 7 | case ::urdf::Joint::PRISMATIC: | |
177 |
1/2✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
|
7 | joint_info << "joint PRISMATIC with axis"; |
178 | |||
179 | // TODO I think the URDF standard forbids REVOLUTE with no limits. | ||
180 |
1/2✗ Branch 2 not taken.
✓ Branch 3 taken 7 times.
|
7 | assert(joint->limits); |
181 |
1/2✓ Branch 2 taken 7 times.
✗ Branch 3 not taken.
|
7 | if (joint->limits) |
182 | { | ||
183 |
1/2✓ Branch 3 taken 7 times.
✗ Branch 4 not taken.
|
7 | max_effort << joint->limits->effort; |
184 |
1/2✓ Branch 3 taken 7 times.
✗ Branch 4 not taken.
|
7 | max_velocity << joint->limits->velocity; |
185 |
1/2✓ Branch 3 taken 7 times.
✗ Branch 4 not taken.
|
7 | min_config << joint->limits->lower; |
186 |
1/2✓ Branch 3 taken 7 times.
✗ Branch 4 not taken.
|
7 | max_config << joint->limits->upper; |
187 | } | ||
188 | |||
189 |
2/2✓ Branch 2 taken 6 times.
✓ Branch 3 taken 1 times.
|
7 | if (joint->dynamics) |
190 | { | ||
191 |
1/2✓ Branch 3 taken 6 times.
✗ Branch 4 not taken.
|
6 | friction << joint->dynamics->friction; |
192 |
1/2✓ Branch 3 taken 6 times.
✗ Branch 4 not taken.
|
6 | damping << joint->dynamics->damping; |
193 | } | ||
194 | |||
195 |
6/12✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 7 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 7 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 7 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 7 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 7 times.
✗ Branch 17 not taken.
|
14 | model.addJointAndBody( |
196 |
1/2✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
|
7 | UrdfVisitorBase::PRISMATIC, axis, parentFrameId, jointPlacement, joint->name, Y, |
197 | 7 | link->name, max_effort, max_velocity, min_config, max_config, friction, damping); | |
198 | 7 | break; | |
199 | |||
200 | ✗ | case ::urdf::Joint::PLANAR: | |
201 | ✗ | joint_info << "joint PLANAR with normal axis along Z"; | |
202 | |||
203 | ✗ | max_effort = Vector::Constant(3, infty); | |
204 | ✗ | max_velocity = Vector::Constant(3, infty); | |
205 | ✗ | min_config = Vector::Constant(4, -infty); | |
206 | ✗ | max_config = Vector::Constant(4, infty); | |
207 | ✗ | min_config.tail<2>().setConstant(-1.01); | |
208 | ✗ | max_config.tail<2>().setConstant(1.01); | |
209 | |||
210 | ✗ | friction = Vector::Constant(3, 0.); | |
211 | ✗ | damping = Vector::Constant(3, 0.); | |
212 | |||
213 | ✗ | model.addJointAndBody( | |
214 | ✗ | UrdfVisitorBase::PLANAR, axis, parentFrameId, jointPlacement, joint->name, Y, | |
215 | ✗ | link->name, max_effort, max_velocity, min_config, max_config, friction, damping); | |
216 | ✗ | break; | |
217 | |||
218 | 723 | case ::urdf::Joint::FIXED: | |
219 | // In case of fixed joint, if link has inertial tag: | ||
220 | // -add the inertia of the link to his parent in the model | ||
221 | // Otherwise do nothing. | ||
222 | // In all cases: | ||
223 | // -let all the children become children of parent | ||
224 | // -inform the parser of the offset to apply | ||
225 | // -add fixed body in model to display it in gepetto-viewer | ||
226 | |||
227 |
1/2✓ Branch 1 taken 723 times.
✗ Branch 2 not taken.
|
723 | joint_info << "fixed joint"; |
228 |
1/2✓ Branch 1 taken 723 times.
✗ Branch 2 not taken.
|
723 | model.addFixedJointAndBody(parentFrameId, jointPlacement, joint_name, Y, link_name); |
229 | 723 | break; | |
230 | |||
231 | ✗ | default: | |
232 | ✗ | throw std::invalid_argument("The type of joint " + joint_name + " is not supported."); | |
233 | break; | ||
234 | } | ||
235 | |||
236 |
2/4✓ Branch 1 taken 1995 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1995 times.
✗ Branch 5 not taken.
|
1995 | model << "Adding Body" << '\n' |
237 |
4/8✓ Branch 1 taken 1995 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1995 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1995 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 1995 times.
✗ Branch 11 not taken.
|
1995 | << '\"' << link_name << "\" connected to \"" << parent_link_name |
238 |
3/6✓ Branch 1 taken 1995 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1995 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1995 times.
✗ Branch 8 not taken.
|
1995 | << "\" through joint \"" << joint_name << "\"\n" |
239 |
4/8✓ Branch 1 taken 1995 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1995 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1995 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 1995 times.
✗ Branch 11 not taken.
|
1995 | << "joint type: " << joint_info.str() << '\n' |
240 |
1/2✓ Branch 1 taken 1995 times.
✗ Branch 2 not taken.
|
1995 | << "joint placement:\n" |
241 |
2/4✓ Branch 1 taken 1995 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1995 times.
✗ Branch 5 not taken.
|
1995 | << jointPlacement << '\n' |
242 |
2/4✓ Branch 1 taken 1995 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1995 times.
✗ Branch 5 not taken.
|
1995 | << "body info: " << '\n' |
243 |
3/6✓ Branch 1 taken 1995 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1995 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 1995 times.
✗ Branch 9 not taken.
|
1995 | << " mass: " << Y.mass() << '\n' |
244 |
4/8✓ Branch 1 taken 1995 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1995 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 1995 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 1995 times.
✗ Branch 12 not taken.
|
1995 | << " lever: " << Y.lever().transpose() << '\n' |
245 |
1/2✓ Branch 1 taken 1995 times.
✗ Branch 2 not taken.
|
1995 | << " inertia elements (Ixx,Iyx,Iyy,Izx,Izy,Izz): " |
246 |
3/6✓ Branch 3 taken 1995 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 1995 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 1995 times.
✗ Branch 10 not taken.
|
1995 | << Y.inertia().data().transpose() << '\n' |
247 |
1/2✓ Branch 1 taken 1995 times.
✗ Branch 2 not taken.
|
1995 | << '\n'; |
248 | 1995 | } | |
249 | ✗ | else if (link->getParent()) | |
250 | ✗ | throw std::invalid_argument(link->name + " - joint information missing."); | |
251 | |||
252 |
18/30✓ Branch 2 taken 1995 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1995 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 1995 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 1995 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 1995 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 1995 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 1893 times.
✗ Branch 21 not taken.
✓ Branch 23 taken 1893 times.
✗ Branch 24 not taken.
✓ Branch 26 taken 1893 times.
✓ Branch 27 taken 1893 times.
✓ Branch 28 taken 1893 times.
✗ Branch 29 not taken.
✓ Branch 31 taken 1893 times.
✗ Branch 32 not taken.
✓ Branch 33 taken 3888 times.
✗ Branch 34 not taken.
✓ Branch 36 taken 3888 times.
✗ Branch 37 not taken.
✓ Branch 38 taken 1893 times.
✓ Branch 39 taken 1995 times.
✓ Branch 40 taken 1893 times.
✓ Branch 41 taken 1995 times.
|
5781 | BOOST_FOREACH (::urdf::LinkConstSharedPtr child, link->child_links) |
253 | { | ||
254 |
1/2✓ Branch 2 taken 1893 times.
✗ Branch 3 not taken.
|
1893 | parseTree(child, model); |
255 | 1893 | } | |
256 | 1995 | } | |
257 | |||
258 | /// | ||
259 | /// \brief Parse a tree with a specific root joint linking the model to the environment. | ||
260 | /// The function returns an exception as soon as a necessary Inertia or Joint | ||
261 | /// information are missing. | ||
262 | /// | ||
263 | /// \param[in] link The current URDF link. | ||
264 | /// \param[in] model The model where the link must be added. | ||
265 | /// | ||
266 | 60 | void parseRootTree(const ::urdf::ModelInterface * urdfTree, UrdfVisitorBase & model) | |
267 | { | ||
268 |
1/2✓ Branch 2 taken 60 times.
✗ Branch 3 not taken.
|
60 | model.setName(urdfTree->getName()); |
269 | |||
270 | 60 | ::urdf::LinkConstSharedPtr root_link = urdfTree->getRoot(); | |
271 |
4/6✓ Branch 3 taken 60 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 60 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 59 times.
✓ Branch 10 taken 1 times.
|
60 | model.addRootJoint(convertFromUrdf(root_link->inertial).cast<double>(), root_link->name); |
272 | |||
273 |
18/30✓ Branch 2 taken 59 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 59 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 59 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 59 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 59 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 59 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 102 times.
✗ Branch 21 not taken.
✓ Branch 23 taken 102 times.
✗ Branch 24 not taken.
✓ Branch 26 taken 102 times.
✓ Branch 27 taken 102 times.
✓ Branch 28 taken 102 times.
✗ Branch 29 not taken.
✓ Branch 31 taken 102 times.
✗ Branch 32 not taken.
✓ Branch 33 taken 161 times.
✗ Branch 34 not taken.
✓ Branch 36 taken 161 times.
✗ Branch 37 not taken.
✓ Branch 38 taken 102 times.
✓ Branch 39 taken 59 times.
✓ Branch 40 taken 102 times.
✓ Branch 41 taken 59 times.
|
263 | BOOST_FOREACH (::urdf::LinkConstSharedPtr child, root_link->child_links) |
274 | { | ||
275 |
1/2✓ Branch 2 taken 102 times.
✗ Branch 3 not taken.
|
102 | parseTree(child, model); |
276 | 102 | } | |
277 | 60 | } | |
278 | |||
279 | 51 | void parseRootTree(const std::string & filename, UrdfVisitorBase & model) | |
280 | { | ||
281 |
1/2✓ Branch 1 taken 51 times.
✗ Branch 2 not taken.
|
51 | ::urdf::ModelInterfaceSharedPtr urdfTree = ::urdf::parseURDFFile(filename); |
282 |
1/2✓ Branch 1 taken 51 times.
✗ Branch 2 not taken.
|
51 | if (urdfTree) |
283 |
1/2✓ Branch 2 taken 51 times.
✗ Branch 3 not taken.
|
102 | return parseRootTree(urdfTree.get(), model); |
284 | else | ||
285 | ✗ | throw std::invalid_argument( | |
286 | ✗ | "The file " + filename | |
287 | ✗ | + " does not " | |
288 | ✗ | "contain a valid URDF model."); | |
289 | 51 | } | |
290 | |||
291 | 7 | void parseRootTreeFromXML(const std::string & xmlString, UrdfVisitorBase & model) | |
292 | { | ||
293 |
1/2✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
|
7 | ::urdf::ModelInterfaceSharedPtr urdfTree = ::urdf::parseURDF(xmlString); |
294 |
1/2✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
|
7 | if (urdfTree) |
295 |
2/2✓ Branch 2 taken 6 times.
✓ Branch 3 taken 1 times.
|
13 | return parseRootTree(urdfTree.get(), model); |
296 | else | ||
297 | ✗ | throw std::invalid_argument("The XML stream does not contain a valid " | |
298 | ✗ | "URDF model."); | |
299 | 7 | } | |
300 | } // namespace details | ||
301 | } // namespace urdf | ||
302 | } // namespace pinocchio | ||
303 |