GCC Code Coverage Report
Directory: ./ Exec Total Coverage
File: include/pinocchio/multibody/model.hpp Lines: 109 127 85.8 %
Date: 2023-08-09 08:43:58 Branches: 203 373 54.4 %

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 <iostream>
24
#include <map>
25
#include <iterator>
26
27
namespace pinocchio
28
{
29
30
  template<typename _Scalar, int _Options, template<typename,int> class JointCollectionTpl>
31
  struct ModelTpl
32
  : serialization::Serializable< ModelTpl<_Scalar,_Options,JointCollectionTpl> >
33
  {
34
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
35
36
    typedef _Scalar Scalar;
37
    enum { Options = _Options };
38
39
    typedef JointCollectionTpl<Scalar,Options> JointCollection;
40
    typedef DataTpl<Scalar,Options,JointCollectionTpl> Data;
41
42
    typedef SE3Tpl<Scalar,Options> SE3;
43
    typedef MotionTpl<Scalar,Options> Motion;
44
    typedef ForceTpl<Scalar,Options> Force;
45
    typedef InertiaTpl<Scalar,Options> Inertia;
46
    typedef FrameTpl<Scalar,Options> Frame;
47
48
    typedef pinocchio::Index Index;
49
    typedef pinocchio::JointIndex JointIndex;
50
    typedef pinocchio::GeomIndex GeomIndex;
51
    typedef pinocchio::FrameIndex FrameIndex;
52
    typedef std::vector<Index> IndexVector;
53
54
    typedef JointModelTpl<Scalar,Options,JointCollectionTpl> JointModel;
55
    typedef JointDataTpl<Scalar,Options,JointCollectionTpl> JointData;
56
57
    typedef PINOCCHIO_ALIGNED_STD_VECTOR(JointModel) JointModelVector;
58
    typedef PINOCCHIO_ALIGNED_STD_VECTOR(JointData) JointDataVector;
59
60
    typedef PINOCCHIO_ALIGNED_STD_VECTOR(Frame) FrameVector;
61
62
    typedef Eigen::Matrix<Scalar,Eigen::Dynamic,1,Options> VectorXs;
63
    typedef Eigen::Matrix<Scalar,3,1,Options> Vector3;
64
65
    /// \brief Dense vectorized version of a joint configuration vector.
66
    typedef VectorXs ConfigVectorType;
67
68
    /// \brief Map between a string (key) and a configuration vector
69
    typedef std::map<std::string, ConfigVectorType>  ConfigVectorMap;
70
71
    /// \brief Dense vectorized version of a joint tangent vector (e.g. velocity, acceleration, etc).
72
    ///        It also handles the notion of co-tangent vector (e.g. torque, etc).
73
    typedef VectorXs TangentVectorType;
74
75
    /// \brief Dimension of the configuration vector representation.
76
    int nq;
77
78
    /// \brief Dimension of the velocity vector space.
79
    int nv;
80
81
    /// \brief Number of joints.
82
    int njoints;
83
84
    /// \brief Number of bodies.
85
    int nbodies;
86
87
    /// \brief Number of operational frames.
88
    int nframes;
89
90
    /// \brief Spatial inertias of the body *i* expressed in the supporting joint frame *i*.
91
    PINOCCHIO_ALIGNED_STD_VECTOR(Inertia) inertias;
92
93
    /// \brief Placement (SE3) of the input of joint *i* regarding to the parent joint output *li*.
94
    PINOCCHIO_ALIGNED_STD_VECTOR(SE3) jointPlacements;
95
96
    /// \brief Model of joint *i*, encapsulated in a JointModelAccessor.
97
    JointModelVector joints;
98
99
    /// \brief Starting index of the joint *i* in the configuration space
100
    std::vector<int> idx_qs;
101
102
    /// \brief Dimension of the joint *i* configuration subspace
103
    std::vector<int> nqs;
104
105
    /// \brief Starting index of the joint *i* in the tangent configuration space
106
    std::vector<int> idx_vs;
107
108
    /// \brief Dimension of the joint *i* tangent subspace
109
    std::vector<int> nvs;
110
111
    /// \brief Joint parent of joint *i*, denoted *li* (li==parents[i]).
112
    std::vector<JointIndex> parents;
113
114
    /// \brief Name of joint *i*
115
    std::vector<std::string> names;
116
117
    /// \brief Map of reference configurations, indexed by user given names.
118
    ConfigVectorMap referenceConfigurations;
119
120
    /// \brief Vector of rotor inertia parameters
121
    TangentVectorType rotorInertia;
122
123
    /// \brief Vector of rotor gear ratio parameters
124
    TangentVectorType rotorGearRatio;
125
126
    /// \brief Vector of joint friction parameters
127
    TangentVectorType friction;
128
129
    /// \brief Vector of joint damping parameters
130
    TangentVectorType damping;
131
132
    /// \brief Vector of maximal joint torques
133
    TangentVectorType effortLimit;
134
135
    /// \brief Vector of maximal joint velocities
136
    TangentVectorType velocityLimit;
137
138
    /// \brief Lower joint configuration limit
139
    ConfigVectorType lowerPositionLimit;
140
141
    /// \brief Upper joint configuration limit
142
    ConfigVectorType upperPositionLimit;
143
144
    /// \brief Vector of operational frames registered on the model.
145
    FrameVector frames;
146
147
    /// \brief Vector of joint supports.
148
    /// supports[j] corresponds to the collection of all joints located on the path between body *j*  and the root.
149
    /// The last element of supports[j] is the index of the joint *j* itself.
150
    std::vector<IndexVector> supports;
151
152
    /// \brief Vector of joint subtrees.
153
    /// subtree[j] corresponds to the subtree supported by the joint *j*.
154
    /// The first element of subtree[j] is the index of the joint *j* itself.
155
    std::vector<IndexVector> subtrees;
156
157
    /// \brief Spatial gravity of the model.
158
    Motion gravity;
159
160
    /// \brief Default 3D gravity vector (=(0,0,-9.81)).
161
    static const Vector3 gravity981;
162
163
    /// \brief Model name;
164
    std::string name;
165
166
    /// \brief Default constructor. Builds an empty model with no joints.
167
604
    ModelTpl()
168
    : nq(0)
169
    , nv(0)
170
    , njoints(1)
171
    , nbodies(1)
172
    , nframes(0)
173
    , inertias(1, Inertia::Zero())
174
    , jointPlacements(1, SE3::Identity())
175
    , joints(1)
176
    , idx_qs(1,0)
177
    , nqs(1,0)
178
    , idx_vs(1,0)
179
    , nvs(1,0)
180
    , parents(1, 0)
181
    , names(1)
182
    , supports(1,IndexVector(1,0))
183
    , subtrees(1)
184












604
    , gravity(gravity981,Vector3::Zero())
185
    {
186
604
      names[0]     = "universe";     // Should be "universe joint (trivial)"
187
      // FIXME Should the universe joint be a FIXED_JOINT even if it is
188
      // in the list of joints ? See comment in definition of
189
      // Model::addJointFrame and Model::addBodyFrame
190


604
      addFrame(Frame("universe", 0, 0, SE3::Identity(), FIXED_JOINT));
191
604
    }
192
835
    ~ModelTpl() {} // std::cout << "Destroy model" << std::endl; }
193
194
    /// \returns An expression of *this with the Scalar type casted to NewScalar.
195
    template<typename NewScalar>
196
10
    ModelTpl<NewScalar,Options,JointCollectionTpl> cast() const
197
    {
198
      typedef ModelTpl<NewScalar,Options,JointCollectionTpl> ReturnType;
199
10
      ReturnType res;
200
10
      res.nq = nq; res.nv = nv;
201
10
      res.njoints = njoints;
202
10
      res.nbodies = nbodies;
203
10
      res.nframes = nframes;
204
10
      res.parents = parents;
205
10
      res.names = names;
206
10
      res.supports = supports;
207
10
      res.subtrees = subtrees;
208
10
      res.gravity = gravity.template cast<NewScalar>();
209
10
      res.name = name;
210
211
10
      res.idx_qs = idx_qs;
212
10
      res.nqs = nqs;
213
10
      res.idx_vs = idx_vs;
214
10
      res.nvs = nvs;
215
216
      // Eigen Vectors
217

10
      res.rotorInertia = rotorInertia.template cast<NewScalar>();
218

10
      res.rotorGearRatio = rotorGearRatio.template cast<NewScalar>();
219

10
      res.friction = friction.template cast<NewScalar>();
220

10
      res.damping = damping.template cast<NewScalar>();
221

10
      res.effortLimit = effortLimit.template cast<NewScalar>();
222

10
      res.velocityLimit = velocityLimit.template cast<NewScalar>();
223

10
      res.lowerPositionLimit = lowerPositionLimit.template cast<NewScalar>();
224

10
      res.upperPositionLimit = upperPositionLimit.template cast<NewScalar>();
225
226
10
      typename ConfigVectorMap::const_iterator it;
227
10
      for (it = referenceConfigurations.begin();
228
10
           it != referenceConfigurations.end(); it++ )
229
      {
230
        res.referenceConfigurations.insert(std::make_pair(it->first, it->second.template cast<NewScalar>()));
231
      }
232
233
      // reserve vectors
234
10
      res.inertias.resize(inertias.size());
235
10
      res.jointPlacements.resize(jointPlacements.size());
236
10
      res.joints.resize(joints.size());
237
10
      res.frames.resize(frames.size());
238
239
      /// copy into vectors
240
290
      for(size_t k = 0; k < joints.size(); ++k)
241
      {
242

280
        res.inertias[k] = inertias[k].template cast<NewScalar>();
243
280
        res.jointPlacements[k] = jointPlacements[k].template cast<NewScalar>();
244

280
        res.joints[k] = joints[k].template cast<NewScalar>();
245
      }
246
247
560
      for(size_t k = 0; k < frames.size(); ++k)
248
      {
249

550
        res.frames[k] = frames[k].template cast<NewScalar>();
250
      }
251
252
20
      return res;
253
    }
254
255
    ///
256
    /// \brief Equality comparison operator.
257
    ///
258
    /// \returns true if *this is equal to other.
259
    ///
260
75
    bool operator==(const ModelTpl & other) const
261
    {
262
75
      bool res =
263
75
         other.nq == nq
264
73
      && other.nv == nv
265
73
      && other.njoints == njoints
266
73
      && other.nbodies == nbodies
267
73
      && other.nframes == nframes
268

73
      && other.parents == parents
269

73
      && other.names == names
270

73
      && other.subtrees == subtrees
271

73
      && other.gravity == gravity
272

148
      && other.name == name;
273
274
75
      res &=
275
75
         other.idx_qs == idx_qs
276

73
      && other.nqs == nqs
277

73
      && other.idx_vs == idx_vs
278

148
      && other.nvs == nvs;
279
280
75
      if(other.referenceConfigurations.size() != referenceConfigurations.size())
281
        return false;
282
283
75
      typename ConfigVectorMap::const_iterator it = referenceConfigurations.begin();
284
75
      typename ConfigVectorMap::const_iterator it_other = other.referenceConfigurations.begin();
285
81
      for(long k = 0; k < (long)referenceConfigurations.size(); ++k)
286
      {
287

8
        std::advance(it,k); std::advance(it_other,k);
288
289

8
        if(it->second.size() != it_other->second.size())
290
2
          return false;
291

6
        if(it->second != it_other->second)
292
          return false;
293
      }
294
295

73
      if(other.rotorInertia.size() != rotorInertia.size())
296
        return false;
297
73
      res &= other.rotorInertia == rotorInertia;
298
73
      if(!res) return res;
299
300

73
      if(other.friction.size() != friction.size())
301
        return false;
302
73
      res &= other.friction == friction;
303
73
      if(!res) return res;
304
305

73
      if(other.damping.size() != damping.size())
306
        return false;
307
73
      res &= other.damping == damping;
308
73
      if(!res) return res;
309
310

73
      if(other.rotorGearRatio.size() != rotorGearRatio.size())
311
        return false;
312
73
      res &= other.rotorGearRatio == rotorGearRatio;
313
73
      if(!res) return res;
314
315

73
      if(other.effortLimit.size() != effortLimit.size())
316
        return false;
317
73
      res &= other.effortLimit == effortLimit;
318
73
      if(!res) return res;
319
320

73
      if(other.velocityLimit.size() != velocityLimit.size())
321
        return false;
322
73
      res &= other.velocityLimit == velocityLimit;
323
73
      if(!res) return res;
324
325

73
      if(other.lowerPositionLimit.size() != lowerPositionLimit.size())
326
        return false;
327
73
      res &= other.lowerPositionLimit == lowerPositionLimit;
328
73
      if(!res) return res;
329
330

73
      if(other.upperPositionLimit.size() != upperPositionLimit.size())
331
        return false;
332
73
      res &= other.upperPositionLimit == upperPositionLimit;
333
73
      if(!res) return res;
334
335
2232
      for(size_t k = 1; k < inertias.size(); ++k)
336
      {
337
2159
        res &= other.inertias[k] == inertias[k];
338
2159
        if(!res) return res;
339
      }
340
341
2232
      for(size_t k = 1; k < other.jointPlacements.size(); ++k)
342
      {
343
2159
        res &= other.jointPlacements[k] == jointPlacements[k];
344
2159
        if(!res) return res;
345
      }
346
347
73
      res &=
348
73
         other.joints == joints
349

73
      && other.frames == frames;
350
351
73
      return res;
352
    }
353
354
    ///
355
    /// \returns true if *this is NOT equal to other.
356
    ///
357
1
    bool operator!=(const ModelTpl & other) const
358
1
    { return !(*this == other); }
359
360
    ///
361
    /// \brief Add a joint to the kinematic tree with infinite bounds.
362
    ///
363
    /// \remark This method also adds a Frame of same name to the vector of frames.
364
    /// \remark The inertia supported by the joint is set to Zero.
365
    /// \remark Joints need to be added to the tree in a depth-first order.
366
    ///
367
    /// \tparam JointModelDerived The type of the joint model.
368
    ///
369
    /// \param[in] parent Index of the parent joint.
370
    /// \param[in] joint_model The joint model.
371
    /// \param[in] joint_placement Placement of the joint inside its parent joint.
372
    /// \param[in] joint_name Name of the joint. If empty, the name is random.
373
    ///
374
    /// \return The index of the new joint.
375
    ///
376
    /// \sa Model::appendBodyToJoint
377
    ///
378
    JointIndex addJoint(const JointIndex parent,
379
                        const JointModel & joint_model,
380
                        const SE3 & joint_placement,
381
                        const std::string & joint_name);
382
383
    ///
384
    /// \copydoc ModelTpl::addJoint(const JointIndex,const JointModel &,const SE3 &,const std::string &)
385
    ///
386
    /// \param[in] max_effort Maximal joint torque.
387
    /// \param[in] max_velocity Maximal joint velocity.
388
    /// \param[in] min_config Lower joint configuration.
389
    /// \param[in] max_config Upper joint configuration.
390
    ///
391
    JointIndex addJoint(const JointIndex parent,
392
                        const JointModel & joint_model,
393
                        const SE3 & joint_placement,
394
                        const std::string & joint_name,
395
                        const VectorXs & max_effort,
396
                        const VectorXs & max_velocity,
397
                        const VectorXs & min_config,
398
                        const VectorXs & max_config);
399
400
    ///
401
    /// \copydoc ModelTpl::addJoint(const JointIndex,const JointModel &,const SE3 &,const std::string &,const VectorXs &,const VectorXs &,const VectorXs &,const VectorXs &)
402
    ///
403
    /// \param[in] friction Joint friction parameters.
404
    /// \param[in] damping Joint damping parameters.
405
    ///
406
    JointIndex addJoint(const JointIndex parent,
407
                        const JointModel & joint_model,
408
                        const SE3 & joint_placement,
409
                        const std::string & joint_name,
410
                        const VectorXs & max_effort,
411
                        const VectorXs & max_velocity,
412
                        const VectorXs & min_config,
413
                        const VectorXs & max_config,
414
                        const VectorXs & friction,
415
                        const VectorXs & damping);
416
417
    ///
418
    /// \brief Add a joint to the frame tree.
419
    ///
420
    /// \param[in] jointIndex Index of the joint.
421
    /// \param[in] frameIndex Index of the parent frame. If negative,
422
    ///            the parent frame is the frame of the parent joint.
423
    ///
424
    /// \return The index of the new frame
425
    ///
426
    FrameIndex addJointFrame(const JointIndex & joint_index,
427
                             int previous_frame_index = -1);
428
429
    ///
430
    /// \brief Append a body to a given joint of the kinematic tree.
431
    ///
432
    /// \param[in] joint_index Index of the supporting joint.
433
    /// \param[in] Y Spatial inertia of the body.
434
    /// \param[in] body_placement The relative placement of the body regarding to the parent joint. Set default to the Identity placement.
435
    ///
436
    /// \sa Model::addJoint
437
    ///
438
    void appendBodyToJoint(const JointIndex joint_index, const Inertia & Y,
439
                           const SE3 & body_placement = SE3::Identity());
440
441
    ///
442
    /// \brief Add a body to the frame tree.
443
    ///
444
    /// \param[in] body_name Name of the body.
445
    /// \param[in] parentJoint Index of the parent joint.
446
    /// \param[in] body_placement The relative placement of the body regarding to the parent joint. Set default to the Identity placement.
447
    /// \param[in] previousFrame Index of the parent frame. If negative,
448
    ///            the parent frame is the frame of the parent joint.
449
    ///
450
    /// \return The index of the new frame
451
    ///
452
    FrameIndex addBodyFrame(const std::string & body_name,
453
                            const JointIndex  & parentJoint,
454
                            const SE3         & body_placement = SE3::Identity(),
455
                            int                 previousFrame  = -1);
456
457
    ///
458
    /// \brief Return the index of a body given by its name.
459
    ///
460
    /// \warning If no body is found, return the number of elements at time T.
461
    /// This can lead to errors if the model is expanded after this method is called
462
    /// (for example to get the id of a parent body)
463
    ///
464
    /// \param[in] name Name of the body.
465
    ///
466
    /// \return Index of the body.
467
    ///
468
    FrameIndex getBodyId(const std::string & name) const;
469
470
    ///
471
    /// \brief Check if a body given by its name exists.
472
    ///
473
    /// \param[in] name Name of the body.
474
    ///
475
    /// \return True if the body exists in the kinematic tree.
476
    ///
477
    bool existBodyName(const std::string & name) const;
478
479
480
    ///
481
    /// \brief Return the index of a joint given by its name.
482
    ///
483
    /// \warning If no joint is found, return the number of elements at time T.
484
    /// This can lead to errors if the model is expanded after this method is called
485
    /// (for example to get the id of a parent joint)
486
    /// \param[in] name Name of the joint.
487
    ///
488
    /// \return Index of the joint.
489
    ///
490
    JointIndex getJointId(const std::string & name) const;
491
492
    ///
493
    /// \brief Check if a joint given by its name exists.
494
    ///
495
    /// \param[in] name Name of the joint.
496
    ///
497
    /// \return True if the joint exists in the kinematic tree.
498
    ///
499
    bool existJointName(const std::string & name) const;
500
501
    ///
502
    /// \brief Returns the index of a frame given by its name.
503
    ///        \sa Model::existFrame to check if the frame exists or not.
504
    ///
505
    /// \warning If no frame is found, returns the size of the vector of Model::frames.
506
    /// This can lead to errors if the model is expanded after this method is called
507
    /// (for example to get the id of a parent frame).
508
    ///
509
    /// \param[in] name Name of the frame.
510
    /// \param[in] type Type of the frame.
511
    ///
512
    /// \return Index of the frame.
513
    ///
514
    FrameIndex getFrameId(const std::string & name,
515











3029
                          const FrameType & type = (FrameType) (JOINT | FIXED_JOINT | BODY | OP_FRAME | SENSOR )) const;
516
517
    ///
518
    /// \brief Checks if a frame given by its name exists.
519
    ///
520
    /// \param[in] name Name of the frame.
521
    /// \param[in] type Type of the frame.
522
    ///
523
    /// \return Returns true if the frame exists.
524
    ///
525
    bool existFrame(const std::string & name,
526




239
                    const FrameType& type = (FrameType) (JOINT | FIXED_JOINT | BODY | OP_FRAME | SENSOR )) const;
527
528
    ///
529
    /// \brief Adds a frame to the kinematic tree.
530
    ///        The inertia stored within the frame will be happened to the inertia supported by the joint (frame.parent).
531
    ///
532
    /// \param[in] frame The frame to add to the kinematic tree.
533
    /// \param[in] append_inertia Append the inertia contained in the Frame to the inertia supported by the joint.
534
    ///
535
    /// \return Returns the index of the frame if it has been successfully added or if it already exists in the kinematic tree.
536
    ///
537
    FrameIndex addFrame(const Frame & frame,
538
                        const bool append_inertia = true);
539
540
    ///
541
    /// \brief Check the validity of the attributes of Model with respect to the specification of some
542
    /// algorithms.
543
    ///
544
    /// The method is a template so that the checkers can be defined in each algorithms.
545
    /// \param[in] checker a class, typically defined in the algorithm module, that
546
    /// validates the attributes of model.
547
    ///
548
    /// \return true if the Model is valid, false otherwise.
549
    ///
550
    template<typename D>
551
12
    inline bool check(const AlgorithmCheckerBase<D> & checker = AlgorithmCheckerBase<D>()) const
552
12
    { return checker.checkModel(*this); }
553
554
    ///
555
    /// \brief Check if joints have configuration limits
556
    ///
557
    /// \return Returns list of boolean of size model.nq.
558
    ///
559
    std::vector<bool> hasConfigurationLimit();
560
561
    ///
562
    /// \brief Check if joints have configuration limits
563
    ///
564
    /// \return Returns list of boolean of size model.nq.
565
    ///
566
    std::vector<bool> hasConfigurationLimitInTangent();
567
568
    /// Run check(fusion::list) with DEFAULT_CHECKERS as argument.
569
    inline bool check() const;
570
571
    ///
572
    /// \brief Run checkData on data and current model.
573
    ///
574
    /// \param[in] data to be checked wrt *this.
575
    ///
576
    /// \return true if the data is valid, false otherwise.
577
    ///
578
    inline bool check(const Data & data) const;
579
580
  protected:
581
582
    ///
583
    /// \brief Add the joint_id to its parent subtrees.
584
    ///
585
    /// \param[in] joint_id The id of the joint to add to the subtrees
586
    ///
587
    void addJointIndexToParentSubtrees(const JointIndex joint_id);
588
  };
589
590
} // namespace pinocchio
591
592
/* --- Details -------------------------------------------------------------- */
593
/* --- Details -------------------------------------------------------------- */
594
/* --- Details -------------------------------------------------------------- */
595
#include "pinocchio/multibody/model.hxx"
596
597
#endif // ifndef __pinocchio_multibody_model_hpp__