GCC Code Coverage Report
Directory: ./ Exec Total Coverage
File: include/pinocchio/multibody/joint/joint-mimic.hpp Lines: 145 172 84.3 %
Date: 2024-01-23 21:41:47 Branches: 61 122 50.0 %

Line Branch Exec Source
1
//
2
// Copyright (c) 2019-2021 INRIA
3
//
4
5
#ifndef __pinocchio_multibody_joint_mimic_hpp__
6
#define __pinocchio_multibody_joint_mimic_hpp__
7
8
#include "pinocchio/macros.hpp"
9
#include "pinocchio/multibody/joint/joint-base.hpp"
10
11
namespace pinocchio
12
{
13
14
  template<class Constraint> struct ScaledConstraint;
15
16
  template<class Constraint>
17
  struct traits< ScaledConstraint<Constraint> >
18
  {
19
    typedef typename traits<Constraint>::Scalar Scalar;
20
    enum { Options = traits<Constraint>::Options };
21
    enum {
22
      LINEAR = traits<Constraint>::LINEAR,
23
      ANGULAR = traits<Constraint>::ANGULAR
24
    };
25
    typedef typename traits<Constraint>::JointMotion JointMotion;
26
    typedef typename traits<Constraint>::JointForce JointForce;
27
    typedef typename traits<Constraint>::DenseBase DenseBase;
28
    typedef typename traits<Constraint>::MatrixReturnType MatrixReturnType;
29
    typedef typename traits<Constraint>::ConstMatrixReturnType ConstMatrixReturnType;
30
  }; // traits ScaledConstraint
31
32
  template<class Constraint>
33
  struct SE3GroupAction< ScaledConstraint<Constraint> >
34
  { typedef typename SE3GroupAction<Constraint>::ReturnType ReturnType; };
35
36
  template<class Constraint, typename MotionDerived>
37
  struct MotionAlgebraAction< ScaledConstraint<Constraint>, MotionDerived >
38
  { typedef typename MotionAlgebraAction<Constraint,MotionDerived>::ReturnType ReturnType; };
39
40
  template<class Constraint, typename ForceDerived>
41
  struct ConstraintForceOp< ScaledConstraint<Constraint>, ForceDerived>
42
  {
43
    typedef typename Constraint::Scalar Scalar;
44
    typedef typename ConstraintForceOp<Constraint,ForceDerived>::ReturnType OriginalReturnType;
45
46
    typedef typename ScalarMatrixProduct<Scalar,OriginalReturnType>::type IdealReturnType;
47
    typedef Eigen::Matrix<Scalar,IdealReturnType::RowsAtCompileTime,IdealReturnType::ColsAtCompileTime,Constraint::Options> ReturnType;
48
  };
49
50
  template<class Constraint, typename ForceSet>
51
  struct ConstraintForceSetOp< ScaledConstraint<Constraint>, ForceSet>
52
  {
53
    typedef typename Constraint::Scalar Scalar;
54
    typedef typename ConstraintForceSetOp<Constraint,ForceSet>::ReturnType OriginalReturnType;
55
    typedef typename ScalarMatrixProduct<Scalar,OriginalReturnType>::type IdealReturnType;
56
    typedef Eigen::Matrix<Scalar,Constraint::NV,ForceSet::ColsAtCompileTime,Constraint::Options | Eigen::RowMajor> ReturnType;
57
  };
58
59
  template<class Constraint>
60
  struct ScaledConstraint
61
  : ConstraintBase< ScaledConstraint<Constraint> >
62
  {
63
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
64
65
    PINOCCHIO_CONSTRAINT_TYPEDEF_TPL(ScaledConstraint)
66
    enum { NV = Constraint::NV };
67
    typedef ConstraintBase<ScaledConstraint> Base;
68
    using Base::nv;
69
70
    typedef typename SE3GroupAction<Constraint>::ReturnType SE3ActionReturnType;
71
72
228
    ScaledConstraint() {}
73
74
156
    explicit ScaledConstraint(const Scalar & scaling_factor)
75
156
    : m_scaling_factor(scaling_factor)
76
156
    {}
77
78
284
    ScaledConstraint(const Constraint & constraint,
79
                     const Scalar & scaling_factor)
80
    : m_constraint(constraint)
81
284
    , m_scaling_factor(scaling_factor)
82
284
    {}
83
84
6
    ScaledConstraint(const ScaledConstraint & other)
85
    : m_constraint(other.m_constraint)
86
6
    , m_scaling_factor(other.m_scaling_factor)
87
6
    {}
88
89
186
    ScaledConstraint & operator=(const ScaledConstraint & other)
90
    {
91
      m_constraint = other.m_constraint;
92
186
      m_scaling_factor = other.m_scaling_factor;
93
186
      return *this;
94
    }
95
96
    template<typename VectorLike>
97
28
    JointMotion __mult__(const Eigen::MatrixBase<VectorLike> & v) const
98
    {
99

28
      assert(v.size() == nv());
100
28
      JointMotion jm = m_constraint * v;
101
56
      return jm * m_scaling_factor;
102
    }
103
104
    template<typename S1, int O1>
105
    SE3ActionReturnType
106
40
    se3Action(const SE3Tpl<S1,O1> & m) const
107
    {
108
40
      SE3ActionReturnType res = m_constraint.se3Action(m);
109

40
      return m_scaling_factor * res;
110
    }
111
112
    template<typename S1, int O1>
113
    SE3ActionReturnType
114
18
    se3ActionInverse(const SE3Tpl<S1,O1> & m) const
115
    {
116
18
      SE3ActionReturnType res = m_constraint.se3ActionInverse(m);
117

18
      return m_scaling_factor * res;
118
    }
119
120
146
    int nv_impl() const { return m_constraint.nv(); }
121
122
    struct TransposeConst
123
    {
124
      const ScaledConstraint & ref;
125
56
      TransposeConst(const ScaledConstraint & ref) : ref(ref) {}
126
127
      template<typename Derived>
128
      typename ConstraintForceOp<ScaledConstraint,Derived>::ReturnType
129
28
      operator*(const ForceDense<Derived> & f) const
130
      {
131
        // TODO: I don't know why, but we should a dense a return type, otherwise it failes at the evaluation level;
132
        typedef typename ConstraintForceOp<ScaledConstraint,Derived>::ReturnType ReturnType;
133

28
        return ReturnType(ref.m_scaling_factor * (ref.m_constraint.transpose() * f));
134
      }
135
136
      /// [CRBA]  MatrixBase operator* (Constraint::Transpose S, ForceSet::Block)
137
      template<typename Derived>
138
      typename ConstraintForceSetOp<ScaledConstraint,Derived>::ReturnType
139
28
      operator*(const Eigen::MatrixBase<Derived> & F) const
140
      {
141
        typedef typename ConstraintForceSetOp<ScaledConstraint,Derived>::ReturnType ReturnType;
142

28
        return ReturnType(ref.m_scaling_factor * (ref.m_constraint.transpose() * F));
143
      }
144
145
    }; // struct TransposeConst
146
147
56
    TransposeConst transpose() const { return TransposeConst(*this); }
148
149
144
    DenseBase matrix_impl() const
150
    {
151

144
      DenseBase S = m_scaling_factor * m_constraint.matrix();
152
144
      return S;
153
    }
154
155
    template<typename MotionDerived>
156
    typename MotionAlgebraAction<ScaledConstraint,MotionDerived>::ReturnType
157
34
    motionAction(const MotionDense<MotionDerived> & m) const
158
    {
159
      typedef typename MotionAlgebraAction<ScaledConstraint,MotionDerived>::ReturnType ReturnType;
160

34
      ReturnType res = m_scaling_factor * m_constraint.motionAction(m);
161
34
      return res;
162
    }
163
164
46
    inline const Scalar & scaling() const { return m_scaling_factor; }
165
168
    inline Scalar & scaling() { return m_scaling_factor; }
166
167
46
    inline const Constraint & constraint() const { return m_constraint; }
168
168
    inline Constraint & constraint() { return m_constraint; }
169
170
96
    bool isEqual(const ScaledConstraint & other) const
171
    {
172
96
      return m_constraint == other.m_constraint
173

96
      && m_scaling_factor == other.m_scaling_factor;
174
    }
175
176
  protected:
177
178
    Constraint m_constraint;
179
    Scalar m_scaling_factor;
180
  }; // struct ScaledConstraint
181
182
  template<typename S1, int O1, typename _Constraint>
183
  struct MultiplicationOp<InertiaTpl<S1,O1>, ScaledConstraint<_Constraint> >
184
  {
185
    typedef InertiaTpl<S1,O1> Inertia;
186
    typedef ScaledConstraint<_Constraint> Constraint;
187
    typedef typename Constraint::Scalar Scalar;
188
189
    typedef typename MultiplicationOp<Inertia,_Constraint>::ReturnType OriginalReturnType;
190
//    typedef typename ScalarMatrixProduct<Scalar,OriginalReturnType>::type ReturnType;
191
    typedef OriginalReturnType ReturnType;
192
  };
193
194
  /* [CRBA] ForceSet operator* (Inertia Y,Constraint S) */
195
  namespace impl
196
  {
197
    template<typename S1, int O1, typename _Constraint>
198
    struct LhsMultiplicationOp<InertiaTpl<S1,O1>, ScaledConstraint<_Constraint> >
199
    {
200
      typedef InertiaTpl<S1,O1> Inertia;
201
      typedef ScaledConstraint<_Constraint> Constraint;
202
      typedef typename MultiplicationOp<Inertia,Constraint>::ReturnType ReturnType;
203
204
34
      static inline ReturnType run(const Inertia & Y,
205
                                   const Constraint & scaled_constraint)
206
      {
207

34
        return scaled_constraint.scaling() * (Y * scaled_constraint.constraint());
208
      }
209
    };
210
  } // namespace impl
211
212
  template<typename M6Like, typename _Constraint>
213
  struct MultiplicationOp<Eigen::MatrixBase<M6Like>, ScaledConstraint<_Constraint> >
214
  {
215
    typedef typename MultiplicationOp<Inertia,_Constraint>::ReturnType OriginalReturnType;
216
    typedef typename PINOCCHIO_EIGEN_PLAIN_TYPE(OriginalReturnType) ReturnType;
217
  };
218
219
  /* [ABA] operator* (Inertia Y,Constraint S) */
220
  namespace impl
221
  {
222
    template<typename M6Like, typename _Constraint>
223
    struct LhsMultiplicationOp<Eigen::MatrixBase<M6Like>, ScaledConstraint<_Constraint> >
224
    {
225
      typedef ScaledConstraint<_Constraint> Constraint;
226
      typedef typename MultiplicationOp<Eigen::MatrixBase<M6Like>,Constraint>::ReturnType ReturnType;
227
228
12
      static inline ReturnType run(const Eigen::MatrixBase<M6Like> & Y,
229
                                   const Constraint & scaled_constraint)
230
      {
231

12
        return scaled_constraint.scaling() * (Y.derived() * scaled_constraint.constraint());
232
      }
233
    };
234
  } // namespace impl
235
236
  template<class Joint> struct JointMimic;
237
  template<class JointModel> struct JointModelMimic;
238
  template<class JointData> struct JointDataMimic;
239
240
  template<class Joint>
241
  struct traits< JointMimic<Joint> >
242
  {
243
    enum
244
    {
245
      NQ = traits<Joint>::NV,
246
      NV = traits<Joint>::NQ
247
    };
248
    typedef typename traits<Joint>::Scalar Scalar;
249
    enum { Options = traits<Joint>::Options };
250
251
    typedef typename traits<Joint>::JointDataDerived JointDataBase;
252
    typedef typename traits<Joint>::JointModelDerived JointModelBase;
253
254
    typedef JointDataMimic<JointDataBase> JointDataDerived;
255
    typedef JointModelMimic<JointModelBase> JointModelDerived;
256
257
    typedef ScaledConstraint<typename traits<Joint>::Constraint_t> Constraint_t;
258
    typedef typename traits<Joint>::Transformation_t Transformation_t;
259
    typedef typename traits<Joint>::Motion_t Motion_t;
260
    typedef typename traits<Joint>::Bias_t Bias_t;
261
262
    // [ABA]
263
    typedef typename traits<Joint>::U_t U_t;
264
    typedef typename traits<Joint>::D_t D_t;
265
    typedef typename traits<Joint>::UD_t UD_t;
266
267
    PINOCCHIO_JOINT_DATA_BASE_ACCESSOR_DEFAULT_RETURN_TYPE
268
269
    typedef typename traits<Joint>::ConfigVector_t ConfigVector_t;
270
    typedef typename traits<Joint>::TangentVector_t TangentVector_t;
271
  };
272
273
  template<class Joint>
274
  struct traits< JointDataMimic<Joint> >
275
  { typedef JointMimic<typename traits<Joint>::JointDerived> JointDerived; };
276
277
  template<class Joint>
278
  struct traits< JointModelMimic<Joint> >
279
  { typedef JointMimic<typename traits<Joint>::JointDerived> JointDerived; };
280
281
  template<class JointData>
282
  struct JointDataMimic
283
  : public JointDataBase< JointDataMimic<JointData> >
284
  {
285
72
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
286
287
    typedef typename traits<JointDataMimic>::JointDerived JointDerived;
288
    typedef JointDataBase< JointDataMimic<JointData> > Base;
289
290
    PINOCCHIO_JOINT_DATA_TYPEDEF_TEMPLATE(JointDerived);
291
292
156
    JointDataMimic()
293
    : m_scaling((Scalar)0)
294
    , m_q_transform(ConfigVector_t::Zero())
295
    , m_v_transform(TangentVector_t::Zero())
296

156
    , S((Scalar)0)
297
156
    {}
298
299
186
    JointDataMimic(const JointDataMimic & other)
300
186
    { *this = other; }
301
302
76
    JointDataMimic(const JointDataBase<JointData> & jdata,
303
                   const Scalar & scaling)
304
    : m_jdata_ref(jdata.derived())
305
    , m_scaling(scaling)
306
76
    , S(m_jdata_ref.S,scaling)
307
76
    {}
308
309
186
    JointDataMimic & operator=(const JointDataMimic & other)
310
    {
311
186
      m_jdata_ref = other.m_jdata_ref;
312
186
      m_scaling = other.m_scaling;
313
186
      m_q_transform = other.m_q_transform;
314
186
      m_v_transform = other.m_v_transform;
315
186
      S = Constraint_t(m_jdata_ref.S,other.m_scaling);
316
186
      return *this;
317
    }
318
319
    using Base::isEqual;
320
54
    bool isEqual(const JointDataMimic & other) const
321
    {
322
54
      return Base::isEqual(other)
323
54
      && m_jdata_ref == other.m_jdata_ref
324
54
      && m_scaling == other.m_scaling
325
54
      && m_q_transform == other.m_q_transform
326

108
      && m_v_transform == other.m_v_transform
327
      ;
328
    }
329
330
246
    static std::string classname()
331
    {
332


246
      return std::string("JointDataMimic<") + JointData::classname() + std::string(">");
333
    }
334
335
12
    std::string shortname() const
336
    {
337


12
      return std::string("JointDataMimic<") + m_jdata_ref.shortname() + std::string(">");
338
    }
339
340
    // Accessors
341
162
    ConstraintTypeConstRef S_accessor() const { return S; }
342
78
    ConstraintTypeRef S_accessor() { return S; }
343
344
162
    TansformTypeConstRef M_accessor() const { return m_jdata_ref.M; }
345
104
    TansformTypeRef M_accessor() { return m_jdata_ref.M; }
346
347
114
    MotionTypeConstRef v_accessor() const { return m_jdata_ref.v; }
348
70
    MotionTypeRef v_accessor() { return m_jdata_ref.v; }
349
350
114
    BiasTypeConstRef c_accessor() const { return m_jdata_ref.c; }
351
48
    BiasTypeRef c_accessor() { return m_jdata_ref.c; }
352
353
114
    UTypeConstRef U_accessor() const { return m_jdata_ref.U; }
354
45
    UTypeRef U_accessor() { return m_jdata_ref.U; }
355
356
114
    DTypeConstRef Dinv_accessor() const { return m_jdata_ref.Dinv; }
357
45
    DTypeRef Dinv_accessor() { return m_jdata_ref.Dinv; }
358
359
114
    UDTypeConstRef UDinv_accessor() const { return m_jdata_ref.UDinv; }
360
45
    UDTypeRef UDinv_accessor() { return m_jdata_ref.UDinv; }
361
362
    template<class JointModel>
363
    friend struct JointModelMimic;
364
365
    const JointData & jdata() const { return m_jdata_ref; }
366
84
    JointData & jdata() { return m_jdata_ref; }
367
368
    const Scalar & scaling() const { return m_scaling; }
369
84
    Scalar & scaling() { return m_scaling; }
370
371
84
    ConfigVector_t & jointConfiguration() { return m_q_transform; }
372
    const ConfigVector_t & jointConfiguration() const { return m_q_transform; }
373
374
84
    TangentVector_t & jointVelocity() { return m_v_transform; }
375
    const TangentVector_t & jointVelocity() const { return m_v_transform; }
376
377
  protected:
378
379
    JointData m_jdata_ref;
380
    Scalar m_scaling;
381
382
    /// \brief Transform configuration vector
383
    ConfigVector_t m_q_transform;
384
    /// \brief Transform velocity vector.
385
    TangentVector_t m_v_transform;
386
387
  public:
388
389
    // data
390
    Constraint_t S;
391
392
  }; // struct JointDataMimic
393
394
  template<typename NewScalar, typename JointModel>
395
  struct CastType< NewScalar, JointModelMimic<JointModel> >
396
  {
397
    typedef typename CastType<NewScalar,JointModel>::type JointModelNewType;
398
    typedef JointModelMimic<JointModelNewType> type;
399
  };
400
401
  template<class JointModel>
402
  struct JointModelMimic
403
  : public JointModelBase< JointModelMimic<JointModel> >
404
  {
405
86
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
406
407
    typedef typename traits<JointModelMimic>::JointDerived JointDerived;
408
409
    PINOCCHIO_JOINT_TYPEDEF_TEMPLATE(JointDerived);
410
411
    typedef JointModelBase<JointModelMimic> Base;
412
    using Base::id;
413
    using Base::idx_q;
414
    using Base::idx_v;
415
    using Base::nq;
416
    using Base::nv;
417
    using Base::setIndexes;
418
419
240
    JointModelMimic()
420
240
    {}
421
422
120
    JointModelMimic(const JointModelBase<JointModel> & jmodel,
423
                    const Scalar & scaling,
424
                    const Scalar & offset)
425
    : m_jmodel_ref(jmodel.derived())
426
    , m_scaling(scaling)
427
120
    , m_offset(offset)
428
120
    {}
429
430
    Base & base() { return *static_cast<Base*>(this); }
431
    const Base & base() const { return *static_cast<const Base*>(this); }
432
433
166
    inline int nq_impl() const { return 0; }
434
137
    inline int nv_impl() const { return 0; }
435
436
328
    inline int idx_q_impl() const { return m_jmodel_ref.idx_q(); }
437
188
    inline int idx_v_impl() const { return m_jmodel_ref.idx_v(); }
438
439
136
    void setIndexes_impl(JointIndex id, int /*q*/, int /*v*/)
440
    {
441
136
      Base::i_id = id; // Only the id of the joint in the model is different.
442
136
      Base::i_q = m_jmodel_ref.idx_q();
443
136
      Base::i_v = m_jmodel_ref.idx_v();
444
136
    }
445
446
76
    JointDataDerived createData() const
447
    {
448
76
      return JointDataDerived(m_jmodel_ref.createData(),scaling());
449
    }
450
451
    const std::vector<bool> hasConfigurationLimit() const
452
    {
453
      return m_jmodel_ref.hasConfigurationLimit();
454
    }
455
456
    const std::vector<bool> hasConfigurationLimitInTangent() const
457
    {
458
      return m_jmodel_ref.hasConfigurationLimitInTangent();
459
    }
460
461
    template<typename ConfigVector>
462
    EIGEN_DONT_INLINE
463
46
    void calc(JointDataDerived & jdata,
464
              const typename Eigen::MatrixBase<ConfigVector> & qs) const
465
    {
466
      typedef typename ConfigVectorAffineTransform<JointDerived>::Type AffineTransform;
467
468
46
      AffineTransform::run(qs.head(m_jmodel_ref.nq()),
469
46
                           m_scaling,m_offset,jdata.m_q_transform);
470
46
      m_jmodel_ref.calc(jdata.m_jdata_ref,jdata.m_q_transform);
471
46
    }
472
473
    template<typename ConfigVector, typename TangentVector>
474
    EIGEN_DONT_INLINE
475
52
    void calc(JointDataDerived & jdata,
476
              const typename Eigen::MatrixBase<ConfigVector> & qs,
477
              const typename Eigen::MatrixBase<TangentVector> & vs) const
478
    {
479
      typedef typename ConfigVectorAffineTransform<JointDerived>::Type AffineTransform;
480
481
52
      AffineTransform::run(qs.head(m_jmodel_ref.nq()),
482
52
                           m_scaling,m_offset,jdata.m_q_transform);
483

52
      jdata.m_v_transform = m_scaling * vs.head(m_jmodel_ref.nv());
484
52
      m_jmodel_ref.calc(jdata.m_jdata_ref,
485
52
                        jdata.m_q_transform,
486
52
                        jdata.m_v_transform);
487
52
    }
488
489
    template<typename Matrix6Like>
490
24
    void calc_aba(JointDataDerived & data,
491
                  const Eigen::MatrixBase<Matrix6Like> & I,
492
                  const bool update_I) const
493
    {
494
      // TODO: fixme
495
24
      m_jmodel_ref.calc_aba(data.m_jdata_ref,
496
24
                            PINOCCHIO_EIGEN_CONST_CAST(Matrix6Like,I),
497
                            update_I);
498
24
    }
499
500
252
    static std::string classname()
501
    {
502


252
      return std::string("JointModelMimic<") + JointModel::classname() + std::string(">");;
503
    }
504
505
132
    std::string shortname() const
506
    {
507


132
      return std::string("JointModelMimic<") + m_jmodel_ref.shortname() + std::string(">");
508
    }
509
510
    /// \returns An expression of *this with the Scalar type casted to NewScalar.
511
    template<typename NewScalar>
512
24
    typename CastType<NewScalar,JointModelMimic>::type cast() const
513
    {
514
      typedef typename CastType<NewScalar,JointModelMimic>::type ReturnType;
515
516
24
      ReturnType res(m_jmodel_ref.template cast<NewScalar>(),
517
                     (NewScalar)m_scaling,
518
24
                     (NewScalar)m_offset);
519
24
      res.setIndexes(id(),idx_q(),idx_v());
520
24
      return res;
521
    }
522
523
18
    const JointModel & jmodel() const { return m_jmodel_ref; }
524
84
    JointModel & jmodel() { return m_jmodel_ref; }
525
526
76
    const Scalar & scaling() const { return m_scaling; }
527
42
    Scalar & scaling() { return m_scaling; }
528
529
    const Scalar & offset() const { return m_offset; }
530
84
    Scalar & offset() { return m_offset; }
531
532
  protected:
533
534
    // data
535
    JointModel m_jmodel_ref;
536
    Scalar m_scaling, m_offset;
537
538
  public:
539
540
    /* Acces to dedicated segment in robot config space.  */
541
    // Const access
542
    template<typename D>
543
    typename SizeDepType<NQ>::template SegmentReturn<D>::ConstType
544
12
    jointConfigSelector_impl(const Eigen::MatrixBase<D> & a) const
545
    {
546
12
      return SizeDepType<NQ>::segment(a.derived(),
547
                                      m_jmodel_ref.idx_q(),
548
12
                                      m_jmodel_ref.nq());
549
    }
550
551
    // Non-const access
552
    template<typename D>
553
    typename SizeDepType<NQ>::template SegmentReturn<D>::Type
554
3
    jointConfigSelector_impl(Eigen::MatrixBase<D> & a) const
555
    {
556
3
      return SizeDepType<NQ>::segment(a.derived(),
557
                                      m_jmodel_ref.idx_q(),
558
3
                                      m_jmodel_ref.nq());
559
    }
560
561
    /* Acces to dedicated segment in robot config velocity space.  */
562
    // Const access
563
    template<typename D>
564
    typename SizeDepType<NV>::template SegmentReturn<D>::ConstType
565
    jointVelocitySelector_impl(const Eigen::MatrixBase<D> & a) const
566
    {
567
      return SizeDepType<NV>::segment(a.derived(),
568
                                      m_jmodel_ref.idx_v(),
569
                                      m_jmodel_ref.nv());
570
    }
571
572
    // Non-const access
573
    template<typename D>
574
    typename SizeDepType<NV>::template SegmentReturn<D>::Type
575
    jointVelocitySelector_impl(Eigen::MatrixBase<D> & a) const
576
    {
577
      return SizeDepType<NV>::segment(a.derived(),
578
                                      m_jmodel_ref.idx_v(),
579
                                      m_jmodel_ref.nv());
580
    }
581
582
    /* Acces to dedicated columns in a ForceSet or MotionSet matrix.*/
583
    // Const access
584
    template<typename D>
585
    typename SizeDepType<NV>::template ColsReturn<D>::ConstType
586
    jointCols_impl(const Eigen::MatrixBase<D> & A) const
587
    {
588
      return SizeDepType<NV>::middleCols(A.derived(),
589
                                         m_jmodel_ref.idx_v(),
590
                                         m_jmodel_ref.nv());
591
    }
592
593
    // Non-const access
594
    template<typename D>
595
    typename SizeDepType<NV>::template ColsReturn<D>::Type
596
    jointCols_impl(Eigen::MatrixBase<D> & A) const
597
    {
598
      return SizeDepType<NV>::middleCols(A.derived(),
599
                                         m_jmodel_ref.idx_v(),
600
                                         m_jmodel_ref.nv());
601
    }
602
603
    /* Acces to dedicated rows in a matrix.*/
604
    // Const access
605
    template<typename D>
606
    typename SizeDepType<NV>::template RowsReturn<D>::ConstType
607
    jointRows_impl(const Eigen::MatrixBase<D> & A) const
608
    {
609
      return SizeDepType<NV>::middleRows(A.derived(),
610
                                         m_jmodel_ref.idx_v(),
611
                                         m_jmodel_ref.nv());
612
    }
613
614
    // Non-const access
615
    template<typename D>
616
    typename SizeDepType<NV>::template RowsReturn<D>::Type
617
    jointRows_impl(Eigen::MatrixBase<D> & A) const
618
    {
619
      return SizeDepType<NV>::middleRows(A.derived(),
620
                                         m_jmodel_ref.idx_v(),
621
                                         m_jmodel_ref.nv());
622
    }
623
624
    /// \brief Returns a block of dimension nv()xnv() located at position idx_v(),idx_v() in the matrix Mat
625
    // Const access
626
    template<typename D>
627
    typename SizeDepType<NV>::template BlockReturn<D>::ConstType
628
    jointBlock_impl(const Eigen::MatrixBase<D> & Mat) const
629
    {
630
      return SizeDepType<NV>::block(Mat.derived(),
631
                                    m_jmodel_ref.idx_v(),m_jmodel_ref.idx_v(),
632
                                    m_jmodel_ref.nv(),m_jmodel_ref.nv());
633
    }
634
635
    // Non-const access
636
    template<typename D>
637
    typename SizeDepType<NV>::template BlockReturn<D>::Type
638
    jointBlock_impl(Eigen::MatrixBase<D> & Mat) const
639
    {
640
      return SizeDepType<NV>::block(Mat.derived(),
641
                                    m_jmodel_ref.idx_v(),m_jmodel_ref.idx_v(),
642
                                    m_jmodel_ref.nv(),m_jmodel_ref.nv());
643
    }
644
645
  }; // struct JointModelMimic
646
647
} // namespace pinocchio
648
649
#endif // ifndef __pinocchio_multibody_joint_mimic_hpp__