GCC Code Coverage Report
Directory: ./ Exec Total Coverage
File: include/pinocchio/multibody/joint/joint-composite.hpp Lines: 122 153 79.7 %
Date: 2023-08-09 08:43:58 Branches: 114 226 50.4 %

Line Branch Exec Source
1
//
2
// Copyright (c) 2016-2021 CNRS INRIA
3
//
4
5
#ifndef __pinocchio_joint_composite_hpp__
6
#define __pinocchio_joint_composite_hpp__
7
8
#include "pinocchio/multibody/joint/fwd.hpp"
9
#include "pinocchio/multibody/joint/joint-collection.hpp"
10
#include "pinocchio/multibody/joint/joint-basic-visitors.hpp"
11
#include "pinocchio/container/aligned-vector.hpp"
12
#include "pinocchio/spatial/act-on-set.hpp"
13
14
#include "pinocchio/serialization/fwd.hpp"
15
16
namespace pinocchio
17
{
18
19
  template<typename Scalar, int Options, template<typename S, int O> class JointCollectionTpl>
20
  struct JointCompositeTpl;
21
22
  template<typename _Scalar, int _Options, template<typename S, int O> class JointCollectionTpl>
23
  struct traits< JointCompositeTpl<_Scalar,_Options,JointCollectionTpl> >
24
  {
25
    typedef _Scalar Scalar;
26
27
    enum {
28
      Options = _Options,
29
      NQ = Eigen::Dynamic,
30
      NV = Eigen::Dynamic
31
    };
32
33
    typedef JointCollectionTpl<Scalar,Options> JointCollection;
34
    typedef JointDataCompositeTpl<Scalar,Options,JointCollectionTpl> JointDataDerived;
35
    typedef JointModelCompositeTpl<Scalar,Options,JointCollectionTpl> JointModelDerived;
36
    typedef ConstraintTpl<Eigen::Dynamic,Scalar,Options> Constraint_t;
37
    typedef SE3Tpl<Scalar,Options> Transformation_t;
38
    typedef MotionTpl<Scalar,Options> Motion_t;
39
    typedef MotionTpl<Scalar,Options> Bias_t;
40
41
    // [ABA]
42
    typedef Eigen::Matrix<Scalar,6,Eigen::Dynamic,Options> U_t;
43
    typedef Eigen::Matrix<Scalar,Eigen::Dynamic,Eigen::Dynamic,Options> D_t;
44
    typedef Eigen::Matrix<Scalar,6,Eigen::Dynamic,Options> UD_t;
45
46
    PINOCCHIO_JOINT_DATA_BASE_ACCESSOR_DEFAULT_RETURN_TYPE
47
48
    typedef Eigen::Matrix<Scalar,Eigen::Dynamic,1,Options> ConfigVector_t;
49
    typedef Eigen::Matrix<Scalar,Eigen::Dynamic,1,Options> TangentVector_t;
50
  };
51
52
  template<typename Scalar, int Options, template<typename S, int O> class JointCollectionTpl>
53
  struct traits< JointModelCompositeTpl<Scalar,Options,JointCollectionTpl> >
54
  { typedef JointCompositeTpl<Scalar,Options,JointCollectionTpl> JointDerived; };
55
56
  template<typename Scalar, int Options, template<typename S, int O> class JointCollectionTpl>
57
  struct traits< JointDataCompositeTpl<Scalar,Options,JointCollectionTpl> >
58
  { typedef JointCompositeTpl<Scalar,Options,JointCollectionTpl> JointDerived; };
59
60
  template<typename _Scalar, int _Options, template<typename S, int O> class JointCollectionTpl>
61
  struct JointDataCompositeTpl
62
  : public JointDataBase< JointDataCompositeTpl<_Scalar,_Options,JointCollectionTpl> >
63
  {
64
36
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
65
66
    typedef JointDataBase<JointDataCompositeTpl> Base;
67
    typedef JointCompositeTpl<_Scalar,_Options,JointCollectionTpl> JointDerived;
68
    PINOCCHIO_JOINT_DATA_TYPEDEF_TEMPLATE(JointDerived);
69
464
    PINOCCHIO_JOINT_DATA_BASE_DEFAULT_ACCESSOR
70
71
    typedef JointCollectionTpl<Scalar,Options> JointCollection;
72
    typedef JointDataTpl<Scalar,Options,JointCollectionTpl> JointDataVariant;
73
74
    typedef PINOCCHIO_ALIGNED_STD_VECTOR(JointDataVariant) JointDataVector;
75
76
    // JointDataComposite()  {} // can become necessary if we want a vector of JointDataComposite ?
77
78
26
    JointDataCompositeTpl()
79
    : joints()
80
    , iMlast(0)
81
    , pjMi(0)
82
    , S(0)
83
    , M(Transformation_t::Identity())
84
    , v(Motion_t::Zero())
85
    , c(Motion_t::Zero())
86
    , U(6,0), Dinv(0,0), UDinv(6,0)
87






26
    , StU(0,0)
88
26
    {}
89
90
91
45
    JointDataCompositeTpl(const JointDataVector & joint_data, const int /*nq*/, const int nv)
92
    : joints(joint_data), iMlast(joint_data.size()), pjMi(joint_data.size())
93
    , S(Constraint_t::Zero(nv))
94
    , M(Transformation_t::Identity())
95
    , v(Motion_t::Zero())
96
    , c(Motion_t::Zero())
97
    , U(U_t::Zero(6,nv))
98
    , Dinv(D_t::Zero(nv,nv))
99
    , UDinv(UD_t::Zero(6,nv))
100








45
    , StU(D_t::Zero(nv,nv))
101
45
    {}
102
103
    /// \brief Vector of joints
104
    JointDataVector joints;
105
106
    /// \brief Transforms from previous joint to last joint
107
    PINOCCHIO_ALIGNED_STD_VECTOR(Transformation_t) iMlast;
108
109
    /// \brief Transforms from previous joint to joint i
110
    PINOCCHIO_ALIGNED_STD_VECTOR(Transformation_t) pjMi;
111
112
    Constraint_t S;
113
    Transformation_t M;
114
    Motion_t v;
115
    Bias_t c;
116
117
    // // [ABA] specific data
118
    U_t U;
119
    D_t Dinv;
120
    UD_t UDinv;
121
122
    D_t StU;
123
124
43
    static std::string classname() { return std::string("JointDataComposite"); }
125
2
    std::string shortname() const { return classname(); }
126
127
  };
128
129
  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl>
130
1
  inline std::ostream & operator <<(std::ostream & os, const JointDataCompositeTpl<Scalar,Options,JointCollectionTpl> & jdata)
131
  {
132
    typedef typename JointDataCompositeTpl<Scalar,Options,JointCollectionTpl>::JointDataVector JointDataVector;
133
134
1
    os << "JointDataComposite containing following models:\n" ;
135
3
    for (typename JointDataVector::const_iterator it = jdata.joints.begin();
136
3
         it != jdata.joints.end(); ++it)
137


2
      os << "  " << shortname(*it) << std::endl;
138
1
    return os;
139
  }
140
141
142
  template<typename NewScalar, typename Scalar, int Options, template<typename S, int O> class JointCollectionTpl>
143
  struct CastType< NewScalar, JointModelCompositeTpl<Scalar,Options,JointCollectionTpl> >
144
  {
145
    typedef JointModelCompositeTpl<NewScalar,Options,JointCollectionTpl> type;
146
  };
147
148
  template<typename _Scalar, int _Options, template<typename S, int O> class JointCollectionTpl>
149
  struct JointModelCompositeTpl
150
  : public JointModelBase< JointModelCompositeTpl<_Scalar,_Options,JointCollectionTpl> >
151
  {
152
108
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
153
154
    typedef JointModelBase<JointModelCompositeTpl> Base;
155
    typedef JointCompositeTpl<_Scalar,_Options,JointCollectionTpl> JointDerived;
156
    PINOCCHIO_JOINT_TYPEDEF_TEMPLATE(JointDerived);
157
158
    typedef JointCollectionTpl<Scalar,Options> JointCollection;
159
    typedef JointModelTpl<Scalar,Options,JointCollectionTpl> JointModel;
160
    typedef JointModel JointModelVariant;
161
162
    typedef SE3Tpl<Scalar,Options> SE3;
163
    typedef MotionTpl<Scalar,Options> Motion;
164
    typedef InertiaTpl<Scalar,Options> Inertia;
165
166
    typedef PINOCCHIO_ALIGNED_STD_VECTOR(JointModel) JointModelVector;
167
168
    using Base::id;
169
    using Base::idx_q;
170
    using Base::idx_v;
171
    using Base::setIndexes;
172
    using Base::nq;
173
    using Base::nv;
174
175
    /// \brief Default contructor
176
42
    JointModelCompositeTpl()
177
    : joints()
178
    , jointPlacements()
179
    , m_nq(0)
180
    , m_nv(0)
181
42
    , njoints(0)
182
42
    {}
183
184
    /// \brief Default contructor with a defined size
185
8
    JointModelCompositeTpl(const size_t size)
186
    : joints()
187
    , jointPlacements()
188
    , m_nq(0)
189
    , m_nv(0)
190
8
    , njoints(0)
191
    {
192

8
      joints.reserve(size); jointPlacements.reserve(size);
193

8
      m_idx_q.reserve(size); m_idx_v.reserve(size);
194

8
      m_nqs.reserve(size); m_nvs.reserve(size);
195
8
    }
196
197
    ///
198
    /// \brief Constructor with one joint.
199
    ///
200
    /// \param jmodel Model of the first joint.
201
    /// \param placement Placement of the first joint w.r.t. the joint origin.
202
    ///
203
    template<typename JointModel>
204
55
    JointModelCompositeTpl(const JointModelBase<JointModel> & jmodel,
205
                           const SE3 & placement = SE3::Identity())
206
55
    : joints(1,(JointModelVariant)jmodel.derived())
207
    , jointPlacements(1,placement)
208
    , m_nq(jmodel.nq())
209
    , m_nv(jmodel.nv())
210
    , m_idx_q(1,0), m_nqs(1,jmodel.nq())
211
    , m_idx_v(1,0), m_nvs(1,jmodel.nv())
212





55
    , njoints(1)
213
55
    {}
214
215
    ///
216
    /// \brief Copy constructor.
217
    ///
218
    /// \param other JointModel to copy.
219
    ///
220
70
    JointModelCompositeTpl(const JointModelCompositeTpl & other)
221
    : Base(other)
222
70
    , joints(other.joints)
223
70
    , jointPlacements(other.jointPlacements)
224
70
    , m_nq(other.m_nq)
225
70
    , m_nv(other.m_nv)
226
140
    , m_idx_q(other.m_idx_q), m_nqs(other.m_nqs)
227
140
    , m_idx_v(other.m_idx_v), m_nvs(other.m_nvs)
228


70
    , njoints(other.njoints)
229
70
    {}
230
231
232
    ///
233
    /// \brief Add a joint to the vector of joints.
234
    ///
235
    /// \param jmodel Model of the joint to add.
236
    /// \param placement Placement of the joint relatively to its predecessor.
237
    ///
238
    /// \return A reference to *this
239
    ///
240
    template<typename JointModel>
241
65
    JointModelDerived & addJoint(const JointModelBase<JointModel> & jmodel,
242
                                 const SE3 & placement = SE3::Identity())
243
    {
244
65
      joints.push_back((JointModelVariant)jmodel.derived());
245
65
      jointPlacements.push_back(placement);
246
247
65
      m_nq += jmodel.nq(); m_nv += jmodel.nv();
248
249
65
      updateJointIndexes();
250
65
      njoints++;
251
252
65
      return *this;
253
    }
254
255
45
    JointDataDerived createData() const
256
    {
257

90
      typename JointDataDerived::JointDataVector jdata(joints.size());
258
120
      for (int i = 0; i < (int)joints.size(); ++i)
259

75
        jdata[(size_t)i] = ::pinocchio::createData<Scalar,Options,JointCollectionTpl>(joints[(size_t)i]);
260

90
      return JointDataDerived(jdata,nq(),nv());
261
    }
262
263
    const std::vector<bool> hasConfigurationLimit() const
264
    {
265
      std::vector<bool> vec;
266
      for (size_t i = 0; i < joints.size(); ++i)
267
        {
268
          const std::vector<bool> & joint_cf_limit = joints[i].hasConfigurationLimit();
269
          vec.insert(vec.end(),
270
                    joint_cf_limit.begin(),
271
                    joint_cf_limit.end());
272
        }
273
      return vec;
274
    }
275
276
    const std::vector<bool> hasConfigurationLimitInTangent() const
277
    {
278
      std::vector<bool> vec;
279
      for (size_t i = 0; i < joints.size(); ++i)
280
        {
281
          const std::vector<bool> & joint_cf_limit = joints[i].hasConfigurationLimitInTangent();
282
          vec.insert(vec.end(),
283
                    joint_cf_limit.begin(),
284
                    joint_cf_limit.end());
285
        }
286
      return vec;
287
    }
288
289
    template<typename, int, template<typename S, int O> class, typename>
290
    friend struct JointCompositeCalcZeroOrderStep;
291
292
    template<typename ConfigVectorType>
293
    void calc(JointDataDerived & data, const Eigen::MatrixBase<ConfigVectorType> & qs) const;
294
295
    template<typename, int, template<typename S, int O> class, typename, typename>
296
    friend struct JointCompositeCalcFirstOrderStep;
297
298
    template<typename ConfigVectorType, typename TangentVectorType>
299
    void calc(JointDataDerived & data,
300
              const Eigen::MatrixBase<ConfigVectorType> & qs,
301
              const Eigen::MatrixBase<TangentVectorType> & vs) const;
302
303
    template<typename Matrix6Like>
304
38
    void calc_aba(JointDataDerived & data,
305
                  const Eigen::MatrixBase<Matrix6Like> & I,
306
                  const bool update_I) const
307
    {
308

38
      data.U.noalias() = I * data.S.matrix();
309

38
      data.StU.noalias() = data.S.matrix().transpose() * data.U;
310
311
      // compute inverse
312
//      data.Dinv.setIdentity();
313
//      data.StU.llt().solveInPlace(data.Dinv);
314
38
      internal::PerformStYSInversion<Scalar>::run(data.StU,data.Dinv);
315

38
      data.UDinv.noalias() = data.U * data.Dinv;
316
317
38
      if (update_I)
318

36
        PINOCCHIO_EIGEN_CONST_CAST(Matrix6Like,I).noalias() -= data.UDinv * data.U.transpose();
319
38
    }
320
321
412
    int nv_impl() const { return m_nv; }
322
232
    int nq_impl() const { return m_nq; }
323
324
    /**
325
     * @brief      Update the indexes of subjoints in the stack
326
     */
327
55
    void setIndexes_impl(JointIndex id, int q, int v)
328
    {
329
55
      Base::setIndexes_impl(id, q, v);
330
55
      updateJointIndexes();
331
55
    }
332
333
61
    static std::string classname() { return std::string("JointModelComposite"); }
334
20
    std::string shortname() const { return classname(); }
335
336
    JointModelCompositeTpl & operator=(const JointModelCompositeTpl & other)
337
    {
338
      Base::operator=(other);
339
      m_nq = other.m_nq;
340
      m_nv = other.m_nv;
341
      m_idx_q = other.m_idx_q;
342
      m_idx_v = other.m_idx_v;
343
      m_nqs = other.m_nqs;
344
      m_nvs = other.m_nvs;
345
      joints = other.joints;
346
      jointPlacements = other.jointPlacements;
347
      njoints = other.njoints;
348
349
      return *this;
350
    }
351
352
    using Base::isEqual;
353
30
    bool isEqual(const JointModelCompositeTpl & other) const
354
    {
355
30
      return Base::isEqual(other)
356
29
      && nq() == other.nq()
357
29
      && nv() == other.nv()
358
29
      && m_idx_q == other.m_idx_q
359
29
      && m_idx_v == other.m_idx_v
360
29
      && m_nqs == other.m_nqs
361
29
      && m_nvs == other.m_nvs
362
29
      && joints == other.joints
363
29
      && jointPlacements == other.jointPlacements
364

59
      && njoints == other.njoints;
365
    }
366
367
    /// \returns An expression of *this with the Scalar type casted to NewScalar.
368
    template<typename NewScalar>
369
8
    JointModelCompositeTpl<NewScalar,Options,JointCollectionTpl> cast() const
370
    {
371
      typedef JointModelCompositeTpl<NewScalar,Options,JointCollectionTpl> ReturnType;
372
8
      ReturnType res((size_t)njoints);
373


8
      res.setIndexes(id(),idx_q(),idx_v());
374
8
      res.m_nq = m_nq;
375
8
      res.m_nv = m_nv;
376
8
      res.m_idx_q = m_idx_q;
377
8
      res.m_idx_v = m_idx_v;
378
8
      res.m_nqs = m_nqs;
379
8
      res.m_nvs = m_nvs;
380
8
      res.njoints = njoints;
381
382
8
      res.joints.resize(joints.size());
383
8
      res.jointPlacements.resize(jointPlacements.size());
384
24
      for(size_t k = 0; k < jointPlacements.size(); ++k)
385
      {
386

16
        res.joints[k] = joints[k].template cast<NewScalar>();
387
16
        res.jointPlacements[k] = jointPlacements[k].template cast<NewScalar>();
388
      }
389
390
8
      return res;
391
    }
392
393
    /// \brief Vector of joints contained in the joint composite.
394
    JointModelVector joints;
395
    /// \brief Vector of joint placements. Those placements correspond to the origin of the joint relatively to their parent.
396
    PINOCCHIO_ALIGNED_STD_VECTOR(SE3) jointPlacements;
397
398
    template<typename D>
399
    typename SizeDepType<NQ>::template SegmentReturn<D>::ConstType
400
18
    jointConfigSelector(const Eigen::MatrixBase<D>& a) const { return a.segment(Base::i_q,nq()); }
401
    template<typename D>
402
    typename SizeDepType<NQ>::template SegmentReturn<D>::Type
403
18
    jointConfigSelector( Eigen::MatrixBase<D>& a) const { return a.segment(Base::i_q,nq()); }
404
405
    template<typename D>
406
    typename SizeDepType<NV>::template SegmentReturn<D>::ConstType
407
18
    jointVelocitySelector(const Eigen::MatrixBase<D>& a) const { return a.segment(Base::i_v,nv());  }
408
    template<typename D>
409
    typename SizeDepType<NV>::template SegmentReturn<D>::Type
410
18
    jointVelocitySelector( Eigen::MatrixBase<D>& a) const { return a.segment(Base::i_v,nv());  }
411
412
    template<typename D>
413
    typename SizeDepType<NV>::template ColsReturn<D>::ConstType
414
18
    jointCols(const Eigen::MatrixBase<D>& A) const { return A.middleCols(Base::i_v,nv());  }
415
    template<typename D>
416
    typename SizeDepType<NV>::template ColsReturn<D>::Type
417
18
    jointCols(Eigen::MatrixBase<D>& A) const { return A.middleCols(Base::i_v,nv());  }
418
419
    template<typename D>
420
    typename SizeDepType<Eigen::Dynamic>::template SegmentReturn<D>::ConstType
421
    jointConfigSelector_impl(const Eigen::MatrixBase<D>& a) const { return a.segment(Base::i_q,nq()); }
422
    template<typename D>
423
    typename SizeDepType<Eigen::Dynamic>::template SegmentReturn<D>::Type
424
    jointConfigSelector_impl(Eigen::MatrixBase<D>& a) const { return a.segment(Base::i_q,nq()); }
425
    template<typename D>
426
    typename SizeDepType<Eigen::Dynamic>::template SegmentReturn<D>::ConstType
427
1
    jointVelocitySelector_impl(const Eigen::MatrixBase<D>& a) const { return a.segment(Base::i_v,nv()); }
428
    template<typename D>
429
    typename SizeDepType<Eigen::Dynamic>::template SegmentReturn<D>::Type
430
    jointVelocitySelector_impl(Eigen::MatrixBase<D>& a) const { return a.segment(Base::i_v,nv()); }
431
432
    template<typename D>
433
    typename SizeDepType<Eigen::Dynamic>::template ColsReturn<D>::ConstType
434
    jointCols_impl(const Eigen::MatrixBase<D>& A) const { return A.middleCols(Base::i_v,nv()); }
435
    template<typename D>
436
    typename SizeDepType<Eigen::Dynamic>::template ColsReturn<D>::Type
437
    jointCols_impl(Eigen::MatrixBase<D>& A) const { return A.middleCols(Base::i_v,nv()); }
438
439
440
  protected:
441
442
    friend struct Serialize<JointModelCompositeTpl>;
443
444
    template<typename, int, template<typename,int> class>
445
    friend struct JointModelCompositeTpl;
446
447
    /// \brief Update the indexes of the joints contained in the composition according
448
    /// to the position of the joint composite.
449
94
    void updateJointIndexes()
450
    {
451
94
      int idx_q = this->idx_q();
452
94
      int idx_v = this->idx_v();
453
454
94
      m_idx_q.resize(joints.size());
455
94
      m_idx_v.resize(joints.size());
456
94
      m_nqs.resize(joints.size());
457
94
      m_nvs.resize(joints.size());
458
459
294
      for(size_t i = 0; i < joints.size(); ++i)
460
      {
461
200
        JointModelVariant & joint = joints[i];
462
463
200
        m_idx_q[i] = idx_q; m_idx_v[i] = idx_v;
464
200
        ::pinocchio::setIndexes(joint,i,idx_q,idx_v);
465
200
        m_nqs[i] = ::pinocchio::nq(joint);
466
200
        m_nvs[i] = ::pinocchio::nv(joint);
467
200
        idx_q += m_nqs[i]; idx_v += m_nvs[i];
468
      }
469
94
    }
470
471
472
    /// \brief Dimensions of the config and tangent space of the composite joint.
473
    int m_nq, m_nv;
474
475
    /// Keep information of both the dimension and the position of the joints in the composition.
476
477
    /// \brief Index in the config vector
478
    std::vector<int> m_idx_q;
479
    /// \brief Dimension of the segment in the config vector
480
    std::vector<int> m_nqs;
481
    /// \brief Index in the tangent vector
482
    std::vector<int> m_idx_v;
483
    /// \brief Dimension of the segment in the tangent vector
484
    std::vector<int> m_nvs;
485
486
  public:
487
    /// \brief Number of joints contained in the JointModelComposite
488
    int njoints;
489
  };
490
491
492
  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl>
493
18
  inline std::ostream & operator <<(std::ostream & os, const JointModelCompositeTpl<Scalar,Options,JointCollectionTpl> & jmodel)
494
  {
495
    typedef typename JointModelCompositeTpl<Scalar,Options,JointCollectionTpl>::JointModelVector JointModelVector;
496
497
18
    os << "JointModelComposite containing following models:\n" ;
498
40
    for (typename JointModelVector::const_iterator it = jmodel.joints.begin();
499
40
         it != jmodel.joints.end(); ++it)
500


22
      os << "  " << shortname(*it) << std::endl;
501
502
18
    return os;
503
  }
504
505
} // namespace pinocchio
506
507
#include <boost/type_traits.hpp>
508
509
namespace boost
510
{
511
  template<typename Scalar, int Options, template<typename S, int O> class JointCollectionTpl>
512
  struct has_nothrow_constructor< ::pinocchio::JointModelCompositeTpl<Scalar,Options,JointCollectionTpl> >
513
  : public integral_constant<bool,true> {};
514
515
  template<typename Scalar, int Options, template<typename S, int O> class JointCollectionTpl>
516
  struct has_nothrow_copy< ::pinocchio::JointModelCompositeTpl<Scalar,Options,JointCollectionTpl> >
517
  : public integral_constant<bool,true> {};
518
519
  template<typename Scalar, int Options, template<typename S, int O> class JointCollectionTpl>
520
  struct has_nothrow_constructor< ::pinocchio::JointDataCompositeTpl<Scalar,Options,JointCollectionTpl> >
521
  : public integral_constant<bool,true> {};
522
523
  template<typename Scalar, int Options, template<typename S, int O> class JointCollectionTpl>
524
  struct has_nothrow_copy< ::pinocchio::JointDataCompositeTpl<Scalar,Options,JointCollectionTpl> >
525
  : public integral_constant<bool,true> {};
526
}
527
528
/* --- Details -------------------------------------------------------------- */
529
/* --- Details -------------------------------------------------------------- */
530
/* --- Details -------------------------------------------------------------- */
531
#include "pinocchio/multibody/joint/joint-composite.hxx"
532
533
#endif // ifndef __pinocchio_joint_composite_hpp__