GCC Code Coverage Report


Directory: ./
File: include/pinocchio/multibody/model.hpp
Date: 2025-02-12 21:03:38
Exec Total Coverage
Lines: 32 32 100.0%
Branches: 81 148 54.7%

Line Branch Exec Source
1 //
2 // Copyright (c) 2015-2020 CNRS INRIA
3 // Copyright (c) 2015 Wandercraft, 86 rue de Paris 91400 Orsay, France.
4 //
5
6 #ifndef __pinocchio_multibody_model_hpp__
7 #define __pinocchio_multibody_model_hpp__
8
9 #include "pinocchio/spatial/fwd.hpp"
10 #include "pinocchio/spatial/se3.hpp"
11 #include "pinocchio/spatial/force.hpp"
12 #include "pinocchio/spatial/motion.hpp"
13 #include "pinocchio/spatial/inertia.hpp"
14
15 #include "pinocchio/multibody/fwd.hpp"
16 #include "pinocchio/multibody/frame.hpp"
17 #include "pinocchio/multibody/joint/joint-generic.hpp"
18
19 #include "pinocchio/container/aligned-vector.hpp"
20
21 #include "pinocchio/serialization/serializable.hpp"
22
23 #include <map>
24 #include <iterator>
25
26 namespace pinocchio
27 {
28 template<
29 typename NewScalar,
30 typename Scalar,
31 int Options,
32 template<typename, int> class JointCollectionTpl>
33 struct CastType<NewScalar, ModelTpl<Scalar, Options, JointCollectionTpl>>
34 {
35 typedef ModelTpl<NewScalar, Options, JointCollectionTpl> type;
36 };
37
38 template<typename _Scalar, int _Options, template<typename, int> class JointCollectionTpl>
39 struct traits<ModelTpl<_Scalar, _Options, JointCollectionTpl>>
40 {
41 typedef _Scalar Scalar;
42 };
43
44 template<typename _Scalar, int _Options, template<typename, int> class JointCollectionTpl>
45 struct ModelTpl
46 : serialization::Serializable<ModelTpl<_Scalar, _Options, JointCollectionTpl>>
47 , NumericalBase<ModelTpl<_Scalar, _Options, JointCollectionTpl>>
48 {
49 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
50
51 typedef _Scalar Scalar;
52 enum
53 {
54 Options = _Options
55 };
56
57 typedef JointCollectionTpl<Scalar, Options> JointCollection;
58 typedef DataTpl<Scalar, Options, JointCollectionTpl> Data;
59
60 typedef SE3Tpl<Scalar, Options> SE3;
61 typedef MotionTpl<Scalar, Options> Motion;
62 typedef ForceTpl<Scalar, Options> Force;
63 typedef InertiaTpl<Scalar, Options> Inertia;
64 typedef FrameTpl<Scalar, Options> Frame;
65
66 typedef pinocchio::Index Index;
67 typedef pinocchio::JointIndex JointIndex;
68 typedef pinocchio::GeomIndex GeomIndex;
69 typedef pinocchio::FrameIndex FrameIndex;
70 typedef std::vector<Index> IndexVector;
71
72 typedef JointModelTpl<Scalar, Options, JointCollectionTpl> JointModel;
73 typedef JointDataTpl<Scalar, Options, JointCollectionTpl> JointData;
74
75 typedef PINOCCHIO_ALIGNED_STD_VECTOR(JointModel) JointModelVector;
76 typedef PINOCCHIO_ALIGNED_STD_VECTOR(JointData) JointDataVector;
77
78 typedef PINOCCHIO_ALIGNED_STD_VECTOR(Frame) FrameVector;
79
80 typedef Eigen::Matrix<Scalar, Eigen::Dynamic, 1, Options> VectorXs;
81 typedef Eigen::Matrix<Scalar, 3, 1, Options> Vector3;
82
83 typedef PINOCCHIO_ALIGNED_STD_VECTOR(Inertia) InertiaVector;
84 typedef PINOCCHIO_ALIGNED_STD_VECTOR(SE3) SE3Vector;
85
86 /// \brief Dense vectorized version of a joint configuration vector.
87 typedef VectorXs ConfigVectorType;
88
89 /// \brief Map between a string (key) and a configuration vector
90 typedef std::map<std::string, ConfigVectorType> ConfigVectorMap;
91
92 /// \brief Dense vectorized version of a joint tangent vector (e.g. velocity, acceleration,
93 /// etc).
94 /// It also handles the notion of co-tangent vector (e.g. torque, etc).
95 typedef VectorXs TangentVectorType;
96
97 /// \brief Dimension of the configuration vector representation.
98 int nq;
99
100 /// \brief Dimension of the velocity vector space.
101 int nv;
102
103 /// \brief Number of joints.
104 int njoints;
105
106 /// \brief Number of bodies.
107 int nbodies;
108
109 /// \brief Number of operational frames.
110 int nframes;
111
112 /// \brief Vector of spatial inertias supported by each joint.
113 InertiaVector inertias;
114
115 /// \brief Vector of joint placements: placement of a joint *i* wrt its parent joint frame.
116 SE3Vector jointPlacements;
117
118 /// \brief Vector of joint models.
119 JointModelVector joints;
120
121 /// \brief Vector of starting index of the *i*th joint in the configuration space.
122 std::vector<int> idx_qs;
123
124 /// \brief Vector of dimension of the joint configuration subspace.
125 std::vector<int> nqs;
126
127 /// \brief Starting index of the *i*th joint in the tangent configuration space.
128 std::vector<int> idx_vs;
129
130 /// \brief Dimension of the *i*th joint tangent subspace.
131 std::vector<int> nvs;
132
133 /// \brief Vector of parent joint indexes. The parent of joint *i*, denoted *li*, corresponds to
134 /// li==parents[i].
135 std::vector<JointIndex> parents;
136
137 /// \brief Vector of children index. Chidren of the *i*th joint, denoted *mu(i)* corresponds to
138 /// the set (i==parents[k] for k in mu(i)).
139 std::vector<IndexVector> children;
140
141 /// \brief Name of the joints.
142 std::vector<std::string> names;
143
144 /// \brief Map of reference configurations, indexed by user given names.
145 ConfigVectorMap referenceConfigurations;
146
147 /// \brief Vector of armature values expressed at the joint level
148 /// This vector may contain the contribution of rotor inertia effects for instance.
149 VectorXs armature;
150
151 /// \brief Vector of rotor inertia parameters
152 TangentVectorType rotorInertia;
153
154 /// \brief Vector of rotor gear ratio parameters
155 TangentVectorType rotorGearRatio;
156
157 /// \brief Vector of joint friction parameters
158 TangentVectorType friction;
159
160 /// \brief Vector of joint damping parameters
161 TangentVectorType damping;
162
163 /// \brief Vector of maximal joint torques
164 TangentVectorType effortLimit;
165
166 /// \brief Vector of maximal joint velocities
167 TangentVectorType velocityLimit;
168
169 /// \brief Lower joint configuration limit
170 ConfigVectorType lowerPositionLimit;
171
172 /// \brief Upper joint configuration limit
173 ConfigVectorType upperPositionLimit;
174
175 /// \brief Vector of operational frames registered on the model.
176 FrameVector frames;
177
178 /// \brief Vector of joint supports.
179 /// supports[j] corresponds to the vector of indices of the joints located on the path between
180 /// joint *j* and "universe".
181 /// The first element of supports[j] is "universe", the last one is the index of joint *j*
182 /// itself.
183 std::vector<IndexVector> supports;
184
185 /// \brief Vector of joint subtrees.
186 /// subtree[j] corresponds to the subtree supported by the joint *j*.
187 /// The first element of subtree[j] is the index of the joint *j* itself.
188 std::vector<IndexVector> subtrees;
189
190 /// \brief Spatial gravity of the model.
191 Motion gravity;
192
193 /// \brief Default 3D gravity vector (=(0,0,-9.81)).
194 static const Vector3 gravity981;
195
196 /// \brief Model name.
197 std::string name;
198
199 /// \brief Default constructor. Builds an empty model with no joints.
200 811 ModelTpl()
201 811 : nq(0)
202 811 , nv(0)
203 811 , njoints(1)
204 811 , nbodies(1)
205 811 , nframes(0)
206
2/4
✓ Branch 2 taken 811 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 811 times.
✗ Branch 6 not taken.
811 , inertias(1, Inertia::Zero())
207
2/4
✓ Branch 2 taken 811 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 811 times.
✗ Branch 6 not taken.
811 , jointPlacements(1, SE3::Identity())
208
1/2
✓ Branch 2 taken 811 times.
✗ Branch 3 not taken.
811 , joints(1)
209
1/2
✓ Branch 2 taken 811 times.
✗ Branch 3 not taken.
811 , idx_qs(1, 0)
210
1/2
✓ Branch 2 taken 811 times.
✗ Branch 3 not taken.
811 , nqs(1, 0)
211
1/2
✓ Branch 2 taken 811 times.
✗ Branch 3 not taken.
811 , idx_vs(1, 0)
212
1/2
✓ Branch 2 taken 811 times.
✗ Branch 3 not taken.
811 , nvs(1, 0)
213
1/2
✓ Branch 2 taken 811 times.
✗ Branch 3 not taken.
811 , parents(1, 0)
214
1/2
✓ Branch 2 taken 811 times.
✗ Branch 3 not taken.
811 , children(1)
215
1/2
✓ Branch 2 taken 811 times.
✗ Branch 3 not taken.
811 , names(1)
216
2/4
✓ Branch 3 taken 811 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 811 times.
✗ Branch 7 not taken.
811 , supports(1, IndexVector(1, 0))
217
1/2
✓ Branch 2 taken 811 times.
✗ Branch 3 not taken.
811 , subtrees(1)
218
12/24
✓ Branch 2 taken 811 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 811 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 811 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 811 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 811 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 811 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 811 times.
✗ Branch 21 not taken.
✓ Branch 23 taken 811 times.
✗ Branch 24 not taken.
✓ Branch 26 taken 811 times.
✗ Branch 27 not taken.
✓ Branch 29 taken 811 times.
✗ Branch 30 not taken.
✓ Branch 32 taken 811 times.
✗ Branch 33 not taken.
✓ Branch 35 taken 811 times.
✗ Branch 36 not taken.
1622 , gravity(gravity981, Vector3::Zero())
219 {
220
1/2
✓ Branch 2 taken 811 times.
✗ Branch 3 not taken.
811 names[0] = "universe"; // Should be "universe joint (trivial)"
221 // FIXME Should the universe joint be a FIXED_JOINT even if it is
222 // in the list of joints ? See comment in definition of
223 // Model::addJointFrame and Model::addBodyFrame
224
5/10
✓ Branch 1 taken 811 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 811 times.
✗ Branch 5 not taken.
✓ Branch 8 taken 811 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 811 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 811 times.
✗ Branch 15 not taken.
811 addFrame(Frame("universe", 0, 0, SE3::Identity(), FIXED_JOINT));
225 811 }
226
227 ///
228 /// \brief Copy constructor by casting
229 ///
230 /// \param[in] other model to copy to *this
231 ///
232 template<typename S2, int O2>
233 1 explicit ModelTpl(const ModelTpl<S2, O2> & other)
234
13/26
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 16 taken 1 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 1 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 1 times.
✗ Branch 23 not taken.
✓ Branch 25 taken 1 times.
✗ Branch 26 not taken.
✓ Branch 28 taken 1 times.
✗ Branch 29 not taken.
✓ Branch 31 taken 1 times.
✗ Branch 32 not taken.
✓ Branch 34 taken 1 times.
✗ Branch 35 not taken.
✓ Branch 37 taken 1 times.
✗ Branch 38 not taken.
✓ Branch 40 taken 1 times.
✗ Branch 41 not taken.
✓ Branch 43 taken 1 times.
✗ Branch 44 not taken.
✓ Branch 48 taken 1 times.
✗ Branch 49 not taken.
1 {
235
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
1 *this = other.template cast<Scalar>();
236 1 }
237
238 /// \returns A new copy of *this with the Scalar type casted to NewScalar.
239 template<typename NewScalar>
240 typename CastType<NewScalar, ModelTpl>::type cast() const;
241
242 ///
243 /// \brief Equality comparison operator.
244 ///
245 /// \returns true if *this is equal to other.
246 ///
247 bool operator==(const ModelTpl & other) const;
248
249 ///
250 /// \returns true if *this is NOT equal to other.
251 ///
252 4 bool operator!=(const ModelTpl & other) const
253 {
254 4 return !(*this == other);
255 }
256
257 ///
258 /// \brief Add a joint to the kinematic tree with infinite bounds.
259 ///
260 /// \remarks This method does not add a Frame of same name to the vector of frames.
261 /// Use Model::addJointFrame.
262 /// \remarks The inertia supported by the joint is set to Zero.
263 /// \remark Joints need to be added to the tree in a depth-first order.
264 ///
265 /// \tparam JointModelDerived The type of the joint model.
266 ///
267 /// \param[in] parent Index of the parent joint.
268 /// \param[in] joint_model The joint model.
269 /// \param[in] joint_placement Placement of the joint inside its parent joint.
270 /// \param[in] joint_name Name of the joint. If empty, the name is random.
271 ///
272 /// \return The index of the new joint.
273 ///
274 /// \sa Model::appendBodyToJoint
275 ///
276 JointIndex addJoint(
277 const JointIndex parent,
278 const JointModel & joint_model,
279 const SE3 & joint_placement,
280 const std::string & joint_name);
281
282 ///
283 /// \copydoc ModelTpl::addJoint(const JointIndex,const JointModel &,const SE3 &,const
284 /// std::string &)
285 ///
286 /// \param[in] max_effort Maximal joint torque.
287 /// \param[in] max_velocity Maximal joint velocity.
288 /// \param[in] min_config Lower joint configuration.
289 /// \param[in] max_config Upper joint configuration.
290 ///
291 JointIndex addJoint(
292 const JointIndex parent,
293 const JointModel & joint_model,
294 const SE3 & joint_placement,
295 const std::string & joint_name,
296 const VectorXs & max_effort,
297 const VectorXs & max_velocity,
298 const VectorXs & min_config,
299 const VectorXs & max_config);
300
301 ///
302 /// \copydoc ModelTpl::addJoint(const JointIndex,const JointModel &,const SE3 &,const
303 /// std::string &,const VectorXs &,const VectorXs &,const VectorXs &,const VectorXs &)
304 ///
305 /// \param[in] friction Joint friction parameters.
306 /// \param[in] damping Joint damping parameters.
307 ///
308 JointIndex addJoint(
309 const JointIndex parent,
310 const JointModel & joint_model,
311 const SE3 & joint_placement,
312 const std::string & joint_name,
313 const VectorXs & max_effort,
314 const VectorXs & max_velocity,
315 const VectorXs & min_config,
316 const VectorXs & max_config,
317 const VectorXs & friction,
318 const VectorXs & damping);
319
320 ///
321 /// \brief Add a joint to the frame tree.
322 ///
323 /// \param[in] jointIndex Index of the joint.
324 /// \param[in] frameIndex Index of the parent frame. If negative,
325 /// the parent frame is the frame of the parent joint.
326 ///
327 /// \return The index of the new frame
328 ///
329 FrameIndex addJointFrame(const JointIndex & joint_index, int previous_frame_index = -1);
330
331 ///
332 /// \brief Append a body to a given joint of the kinematic tree.
333 ///
334 /// \param[in] joint_index Index of the supporting joint.
335 /// \param[in] Y Spatial inertia of the body.
336 /// \param[in] body_placement The relative placement of the body regarding to the parent joint.
337 /// Set default to the Identity placement.
338 ///
339 /// \sa Model::addJoint
340 ///
341 void appendBodyToJoint(
342 const JointIndex joint_index,
343 const Inertia & Y,
344 const SE3 & body_placement = SE3::Identity());
345
346 ///
347 /// \brief Add a body to the frame tree.
348 ///
349 /// \param[in] body_name Name of the body.
350 /// \param[in] parentJoint Index of the parent joint.
351 /// \param[in] body_placement The relative placement of the body regarding to the parent joint.
352 /// Set default to the Identity placement. \param[in] parentFrame Index of the parent frame. If
353 /// negative,
354 /// the parent frame is the frame of the parent joint.
355 ///
356 /// \return The index of the new frame
357 ///
358 FrameIndex addBodyFrame(
359 const std::string & body_name,
360 const JointIndex & parentJoint,
361 const SE3 & body_placement = SE3::Identity(),
362 int parentFrame = -1);
363
364 ///
365 /// \brief Return the index of a body given by its name.
366 ///
367 /// \warning If no body is found, return the number of elements at time T.
368 /// This can lead to errors if the model is expanded after this method is called
369 /// (for example to get the id of a parent body)
370 ///
371 /// \param[in] name Name of the body.
372 ///
373 /// \return Index of the body.
374 ///
375 FrameIndex getBodyId(const std::string & name) const;
376
377 ///
378 /// \brief Check if a body given by its name exists.
379 ///
380 /// \param[in] name Name of the body.
381 ///
382 /// \return True if the body exists in the kinematic tree.
383 ///
384 bool existBodyName(const std::string & name) const;
385
386 ///
387 /// \brief Return the index of a joint given by its name.
388 ///
389 /// \warning If no joint is found, return the number of elements at time T.
390 /// This can lead to errors if the model is expanded after this method is called
391 /// (for example to get the id of a parent joint)
392 /// \param[in] name Name of the joint.
393 ///
394 /// \return Index of the joint.
395 ///
396 JointIndex getJointId(const std::string & name) const;
397
398 ///
399 /// \brief Check if a joint given by its name exists.
400 ///
401 /// \param[in] name Name of the joint.
402 ///
403 /// \return True if the joint exists in the kinematic tree.
404 ///
405 bool existJointName(const std::string & name) const;
406
407 ///
408 /// \brief Returns the index of a frame given by its name.
409 /// \sa Model::existFrame to check if the frame exists or not.
410 ///
411 /// \warning If no frame is found, returns the size of the vector of Model::frames.
412 /// This can lead to errors if the model is expanded after this method is called
413 /// (for example to get the id of a parent frame).
414 ///
415 /// \param[in] name Name of the frame.
416 /// \param[in] type Type of the frame.
417 ///
418 /// \return Index of the frame.
419 ///
420 FrameIndex getFrameId(
421 const std::string & name,
422
26/37
✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✓ Branch 5 taken 749 times.
✗ Branch 6 not taken.
✓ Branch 7 taken 1 times.
✓ Branch 8 taken 40 times.
✓ Branch 9 taken 15 times.
✓ Branch 10 taken 1 times.
✗ Branch 11 not taken.
✓ Branch 12 taken 1 times.
✓ Branch 13 taken 24 times.
✓ Branch 14 taken 40 times.
✓ Branch 15 taken 275 times.
✓ Branch 16 taken 24 times.
✗ Branch 17 not taken.
✓ Branch 18 taken 24 times.
✓ Branch 19 taken 2 times.
✓ Branch 20 taken 274 times.
✓ Branch 21 taken 23 times.
✓ Branch 22 taken 2 times.
✓ Branch 23 taken 24 times.
✗ Branch 24 not taken.
✓ Branch 25 taken 28 times.
✓ Branch 26 taken 270 times.
✓ Branch 28 taken 16 times.
✗ Branch 29 not taken.
✓ Branch 33 taken 16 times.
✗ Branch 34 not taken.
✓ Branch 37 taken 315 times.
✗ Branch 38 not taken.
✓ Branch 40 taken 35 times.
✗ Branch 41 not taken.
✓ Branch 45 taken 35 times.
✗ Branch 46 not taken.
✓ Branch 49 taken 358 times.
✗ Branch 50 not taken.
3412 const FrameType & type = (FrameType)(JOINT | FIXED_JOINT | BODY | OP_FRAME | SENSOR)) const;
423
424 ///
425 /// \brief Checks if a frame given by its name exists.
426 ///
427 /// \param[in] name Name of the frame.
428 /// \param[in] type Type of the frame.
429 ///
430 /// \return Returns true if the frame exists.
431 ///
432 bool existFrame(
433 const std::string & name,
434
7/15
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✓ Branch 4 taken 16 times.
✓ Branch 5 taken 1 times.
✓ Branch 6 taken 51 times.
✗ Branch 7 not taken.
✓ Branch 8 taken 11 times.
✓ Branch 9 taken 36 times.
✗ Branch 10 not taken.
✗ Branch 11 not taken.
✗ Branch 12 not taken.
✗ Branch 13 not taken.
✓ Branch 15 taken 40 times.
✗ Branch 16 not taken.
239 const FrameType & type = (FrameType)(JOINT | FIXED_JOINT | BODY | OP_FRAME | SENSOR)) const;
435
436 ///
437 /// \brief Adds a frame to the kinematic tree.
438 /// The inertia stored within the frame will be happened to the inertia supported by the
439 /// joint (frame.parentJoint).
440 ///
441 /// \param[in] frame The frame to add to the kinematic tree.
442 /// \param[in] append_inertia Append the inertia contained in the Frame to the inertia supported
443 /// by the joint.
444 ///
445 /// \return Returns the index of the frame if it has been successfully added or if it already
446 /// exists in the kinematic tree.
447 ///
448 FrameIndex addFrame(const Frame & frame, const bool append_inertia = true);
449
450 ///
451 /// \brief Check the validity of the attributes of Model with respect to the specification of
452 /// some algorithms.
453 ///
454 /// The method is a template so that the checkers can be defined in each algorithms.
455 /// \param[in] checker a class, typically defined in the algorithm module, that
456 /// validates the attributes of model.
457 ///
458 /// \return true if the Model is valid, false otherwise.
459 ///
460 template<typename D>
461 6 bool check(const AlgorithmCheckerBase<D> & checker = AlgorithmCheckerBase<D>()) const
462 {
463 6 return checker.checkModel(*this);
464 }
465
466 ///
467 /// \brief Check if joints have configuration limits
468 ///
469 /// \return Returns list of boolean of size model.nq.
470 ///
471 std::vector<bool> hasConfigurationLimit();
472
473 ///
474 /// \brief Check if joints have configuration limits
475 ///
476 /// \return Returns list of boolean of size model.nq.
477 ///
478 std::vector<bool> hasConfigurationLimitInTangent();
479
480 /// Run check(fusion::list) with DEFAULT_CHECKERS as argument.
481 bool check() const;
482
483 ///
484 /// \brief Run checkData on data and current model.
485 ///
486 /// \param[in] data to be checked wrt *this.
487 ///
488 /// \return true if the data is valid, false otherwise.
489 ///
490 bool check(const Data & data) const;
491
492 protected:
493 ///
494 /// \brief Add the joint_id to its parent subtrees.
495 ///
496 /// \param[in] joint_id The id of the joint to add to the subtrees
497 ///
498 void addJointIndexToParentSubtrees(const JointIndex joint_id);
499 };
500
501 } // namespace pinocchio
502
503 /* --- Details -------------------------------------------------------------- */
504 /* --- Details -------------------------------------------------------------- */
505 /* --- Details -------------------------------------------------------------- */
506 #include "pinocchio/multibody/model.hxx"
507
508 #if PINOCCHIO_ENABLE_TEMPLATE_INSTANTIATION
509 #include "pinocchio/multibody/model.txx"
510 #endif // PINOCCHIO_ENABLE_TEMPLATE_INSTANTIATION
511
512 #endif // ifndef __pinocchio_multibody_model_hpp__
513