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 |