joint-mimic.hpp 20.2 KB
Newer Older
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
//
// Copyright (c) 2019 INRIA
//

#ifndef __pinocchio_joint_mimic_hpp__
#define __pinocchio_joint_mimic_hpp__

#include "pinocchio/macros.hpp"
#include "pinocchio/multibody/joint/joint-base.hpp"

namespace pinocchio
{
  
  template<class Constraint> struct ScaledConstraint;
  
  template<class Constraint>
  struct traits< ScaledConstraint<Constraint> >
  {
    typedef typename traits<Constraint>::Scalar Scalar;
    enum { Options = traits<Constraint>::Options };
    enum {
      LINEAR = traits<Constraint>::LINEAR,
      ANGULAR = traits<Constraint>::ANGULAR
    };
    typedef typename traits<Constraint>::JointMotion JointMotion;
    typedef typename traits<Constraint>::JointForce JointForce;
    typedef typename traits<Constraint>::DenseBase DenseBase;
    typedef typename traits<Constraint>::MatrixReturnType MatrixReturnType;
    typedef typename traits<Constraint>::ConstMatrixReturnType ConstMatrixReturnType;
  }; // traits ScaledConstraint
  
Justin Carpentier's avatar
Justin Carpentier committed
32
33
34
35
36
37
38
39
40
41
  template<class Constraint>
  struct SE3GroupAction< ScaledConstraint<Constraint> >
  { typedef typename SE3GroupAction<Constraint>::ReturnType ReturnType; };
  
  template<class Constraint, typename MotionDerived>
  struct MotionAlgebraAction< ScaledConstraint<Constraint>, MotionDerived >
  { typedef typename MotionAlgebraAction<Constraint,MotionDerived>::ReturnType ReturnType; };
  
  template<class Constraint, typename ForceDerived>
  struct ConstraintForceOp< ScaledConstraint<Constraint>, ForceDerived>
42
  {
Justin Carpentier's avatar
Justin Carpentier committed
43
44
    typedef typename Constraint::Scalar Scalar;
    typedef typename ConstraintForceOp<Constraint,ForceDerived>::ReturnType OriginalReturnType;
45
    
Justin Carpentier's avatar
Justin Carpentier committed
46
47
48
    typedef typename ScalarMatrixProduct<Scalar,OriginalReturnType>::type IdealReturnType;
    typedef Eigen::Matrix<Scalar,IdealReturnType::RowsAtCompileTime,IdealReturnType::ColsAtCompileTime,Constraint::Options> ReturnType;
  };
49
  
Justin Carpentier's avatar
Justin Carpentier committed
50
51
52
53
54
  template<class Constraint, typename ForceSet>
  struct ConstraintForceSetOp< ScaledConstraint<Constraint>, ForceSet>
  {
    typedef typename Constraint::Scalar Scalar;
    typedef typename ConstraintForceSetOp<Constraint,ForceSet>::ReturnType OriginalReturnType;
55
56
    typedef typename ScalarMatrixProduct<Scalar,OriginalReturnType>::type IdealReturnType;
    typedef Eigen::Matrix<Scalar,Constraint::NV,ForceSet::ColsAtCompileTime,Constraint::Options | Eigen::RowMajor> ReturnType;
Justin Carpentier's avatar
Justin Carpentier committed
57
58
  };
    
59
60
61
62
63
64
65
66
67
68
69
  template<class Constraint>
  struct ScaledConstraint
  : ConstraintBase< ScaledConstraint<Constraint> >
  {
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
    
    PINOCCHIO_CONSTRAINT_TYPEDEF_TPL(ScaledConstraint)
    enum { NV = Constraint::NV };
    typedef ConstraintBase<ScaledConstraint> Base;
    using Base::nv;
    
Justin Carpentier's avatar
Justin Carpentier committed
70
    typedef typename SE3GroupAction<Constraint>::ReturnType SE3ActionReturnType;
71
    
72
73
    ScaledConstraint() {}
    
74
    ScaledConstraint(const Constraint & constraint,
75
                     const Scalar & scaling_factor)
76
77
    : m_constraint(constraint)
    , m_scaling_factor(scaling_factor)
78
79
    {}
    
80
81
82
83
84
85
86
    ScaledConstraint & operator=(const ScaledConstraint & other)
    {
      m_constraint = other.m_constraint;
      m_scaling_factor = other.m_scaling_factor;
      return *this;
    }
    
87
88
89
90
    template<typename VectorLike>
    JointMotion __mult__(const Eigen::MatrixBase<VectorLike> & v) const
    {
      assert(v.size() == nv());
91
92
      JointMotion jm = m_constraint * v;
      return jm * m_scaling_factor;
93
94
95
96
97
98
    }
    
    template<typename S1, int O1>
    SE3ActionReturnType
    se3Action(const SE3Tpl<S1,O1> & m) const
    {
99
      SE3ActionReturnType res = m_constraint.se3Action(m);
100
101
102
103
104
105
106
107
      return m_scaling_factor * res;
    }
    
    template<typename S1, int O1>
    SE3ActionReturnType
    se3ActionInverse(const SE3Tpl<S1,O1> & m) const
    {
      SE3ActionReturnType res = m_constraint.se3ActionInverse(m);
108
      return m_scaling_factor * res;
109
110
    }
    
111
    int nv_impl() const { return m_constraint.nv(); }
112
113
114
115
116
117
118
    
    struct TransposeConst
    {
      const ScaledConstraint & ref;
      TransposeConst(const ScaledConstraint & ref) : ref(ref) {}
      
      template<typename Derived>
Justin Carpentier's avatar
Justin Carpentier committed
119
      typename ConstraintForceOp<ScaledConstraint,Derived>::ReturnType
120
121
      operator*(const ForceDense<Derived> & f) const
      {
122
        // TODO: I don't know why, but we should a dense a return type, otherwise it failes at the evaluation level;
Justin Carpentier's avatar
Justin Carpentier committed
123
        typedef typename ConstraintForceOp<ScaledConstraint,Derived>::ReturnType ReturnType;
124
        return ReturnType(ref.m_scaling_factor * (ref.m_constraint.transpose() * f));
125
126
127
128
      }
      
      /// [CRBA]  MatrixBase operator* (Constraint::Transpose S, ForceSet::Block)
      template<typename Derived>
Justin Carpentier's avatar
Justin Carpentier committed
129
      typename ConstraintForceSetOp<ScaledConstraint,Derived>::ReturnType
130
131
      operator*(const Eigen::MatrixBase<Derived> & F) const
      {
132
133
        typedef typename ConstraintForceSetOp<ScaledConstraint,Derived>::ReturnType ReturnType;
        return ReturnType(ref.m_scaling_factor * (ref.m_constraint.transpose() * F));
134
135
136
137
138
139
140
141
      }
      
    }; // struct TransposeConst
    
    TransposeConst transpose() const { return TransposeConst(*this); }
    
    DenseBase matrix_impl() const
    {
142
      DenseBase S = m_scaling_factor * m_constraint.matrix();
143
144
145
146
      return S;
    }
    
    template<typename MotionDerived>
Justin Carpentier's avatar
Justin Carpentier committed
147
    typename MotionAlgebraAction<ScaledConstraint,MotionDerived>::ReturnType
148
149
    motionAction(const MotionDense<MotionDerived> & m) const
    {
Justin Carpentier's avatar
Justin Carpentier committed
150
      typedef typename MotionAlgebraAction<ScaledConstraint,MotionDerived>::ReturnType ReturnType;
151
      ReturnType res = m_scaling_factor * m_constraint.motionAction(m);
152
153
154
      return res;
    }
    
155
    inline const Scalar & scaling() const { return m_scaling_factor; }
156
157
    inline Scalar & scaling() { return m_scaling_factor; }
    
158
    inline const Constraint & constraint() const { return m_constraint; }
159
    inline Constraint & constraint() { return m_constraint; }
160
    
161
162
163
164
165
166
    bool isEqual(const ScaledConstraint & other) const
    {
      return m_constraint == other.m_constraint
      && m_scaling_factor == other.m_scaling_factor;
    }
    
167
168
  protected:
    
169
    Constraint m_constraint;
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
    Scalar m_scaling_factor;
  }; // struct ScaledConstraint
  
  template<typename S1, int O1, typename _Constraint>
  struct MultiplicationOp<InertiaTpl<S1,O1>, ScaledConstraint<_Constraint> >
  {
    typedef InertiaTpl<S1,O1> Inertia;
    typedef ScaledConstraint<_Constraint> Constraint;
    typedef typename Constraint::Scalar Scalar;
    
    typedef typename MultiplicationOp<Inertia,_Constraint>::ReturnType OriginalReturnType;
//    typedef typename ScalarMatrixProduct<Scalar,OriginalReturnType>::type ReturnType;
    typedef OriginalReturnType ReturnType;
  };
  
  /* [CRBA] ForceSet operator* (Inertia Y,Constraint S) */
  namespace impl
  {
    template<typename S1, int O1, typename _Constraint>
    struct LhsMultiplicationOp<InertiaTpl<S1,O1>, ScaledConstraint<_Constraint> >
    {
      typedef InertiaTpl<S1,O1> Inertia;
      typedef ScaledConstraint<_Constraint> Constraint;
      typedef typename MultiplicationOp<Inertia,Constraint>::ReturnType ReturnType;
      
      static inline ReturnType run(const Inertia & Y,
                                   const Constraint & scaled_constraint)
      {
        return scaled_constraint.scaling() * (Y * scaled_constraint.constraint());
      }
    };
  } // namespace impl
  
  template<typename M6Like, typename _Constraint>
  struct MultiplicationOp<Eigen::MatrixBase<M6Like>, ScaledConstraint<_Constraint> >
  {
    typedef typename MultiplicationOp<Inertia,_Constraint>::ReturnType OriginalReturnType;
    typedef typename PINOCCHIO_EIGEN_PLAIN_TYPE(OriginalReturnType) ReturnType;
  };
  
  /* [ABA] operator* (Inertia Y,Constraint S) */
  namespace impl
  {
    template<typename M6Like, typename _Constraint>
    struct LhsMultiplicationOp<Eigen::MatrixBase<M6Like>, ScaledConstraint<_Constraint> >
    {
      typedef ScaledConstraint<_Constraint> Constraint;
      typedef typename MultiplicationOp<Eigen::MatrixBase<M6Like>,Constraint>::ReturnType ReturnType;
      
      static inline ReturnType run(const Eigen::MatrixBase<M6Like> & Y,
                                   const Constraint & scaled_constraint)
      {
        return scaled_constraint.scaling() * (Y.derived() * scaled_constraint.constraint());
      }
    };
  } // namespace impl
226
227
228
229
230
231
232
233
  
  template<class Joint> struct JointMimic;
  template<class JointModel> struct JointModelMimic;
  template<class JointData> struct JointDataMimic;
  
  template<class Joint>
  struct traits< JointMimic<Joint> >
  {
234
235
    enum
    {
236
237
238
239
240
241
      NQ = traits<Joint>::NV,
      NV = traits<Joint>::NQ
    };
    typedef typename traits<Joint>::Scalar Scalar;
    enum { Options = traits<Joint>::Options };
    
242
243
244
245
246
    typedef typename traits<Joint>::JointDataDerived JointDataBase;
    typedef typename traits<Joint>::JointModelDerived JointModelBase;
    
    typedef JointDataMimic<JointDataBase> JointDataDerived;
    typedef JointModelMimic<JointModelBase> JointModelDerived;
247
    
248
    typedef ScaledConstraint<typename traits<Joint>::Constraint_t> Constraint_t;
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
    typedef typename traits<Joint>::Transformation_t Transformation_t;
    typedef typename traits<Joint>::Motion_t Motion_t;
    typedef typename traits<Joint>::Bias_t Bias_t;

    // [ABA]
    typedef typename traits<Joint>::U_t U_t;
    typedef typename traits<Joint>::D_t D_t;
    typedef typename traits<Joint>::UD_t UD_t;
    
    PINOCCHIO_JOINT_DATA_BASE_ACCESSOR_DEFAULT_RETURN_TYPE
    
    typedef typename traits<Joint>::ConfigVector_t ConfigVector_t;
    typedef typename traits<Joint>::TangentVector_t TangentVector_t;
  };
  
  template<class Joint>
  struct traits< JointDataMimic<Joint> >
266
  { typedef JointMimic<typename traits<Joint>::JointDerived> JointDerived; };
267
268
269
  
  template<class Joint>
  struct traits< JointModelMimic<Joint> >
270
  { typedef JointMimic<typename traits<Joint>::JointDerived> JointDerived; };
271
272
273
  
  template<class JointData>
  struct JointDataMimic
274
  : public JointDataBase< JointDataMimic<JointData> >
275
276
  {
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
277
278
279

    typedef typename traits<JointDataMimic>::JointDerived JointDerived;
    typedef JointDataBase< JointDataMimic<JointData> > Base;
280
    
281
    PINOCCHIO_JOINT_DATA_TYPEDEF_TEMPLATE(JointDerived);
282
    
283
284
    JointDataMimic() {}
    
285
286
287
288
289
290
    JointDataMimic(const JointDataBase<JointData> & jdata,
                   const Scalar & scaling)
    : jdata_ref(jdata.derived())
    , scaling(scaling)
    , S(jdata_ref.S,scaling)
    {}
291
    
292
293
294
295
296
297
298
    JointDataMimic & operator=(const JointDataMimic & other)
    {
      jdata_ref = other.jdata_ref;
      scaling = other.scaling;
      S = Constraint_t(jdata_ref.S,other.scaling);
      return *this;
    }
299
300
301
    
    static std::string classname()
    {
302
      return std::string("JointDataMimic<") + JointData::classname() + std::string(">");
303
304
305
306
    }
    
    std::string shortname() const
    {
307
      return std::string("JointDataMimic<") + jdata_ref.shortname() + std::string(">");
308
309
310
    }
    
    // Accessors
311
    ConstraintTypeConstRef S_accessor() const { return S; }
312
    ConstraintTypeRef S_accessor() { return S; }
313
    TansformTypeConstRef M_accessor() const { return jdata_ref.M; }
314
    TansformTypeRef M_accessor() { return jdata_ref.M; }
315
    MotionTypeConstRef v_accessor() const { return jdata_ref.v; }
316
    MotionTypeRef v_accessor() { return jdata_ref.v; }
317
    BiasTypeConstRef c_accessor() const { return jdata_ref.c; }
318
    BiasTypeRef c_accessor() { return jdata_ref.c; }
319
320
321
322
323
    UTypeConstRef U_accessor() const { return jdata_ref.U; }
    UTypeRef U_accessor() { return jdata_ref.U; }
    DTypeConstRef Dinv_accessor() const { return jdata_ref.Dinv; }
    UDTypeConstRef UDinv_accessor() const { return jdata_ref.UDinv; }
    
324
325
326
327
328
329
330
331
    template<class JointModel>
    friend struct JointModelMimic;
    
  protected:
    
    JointData jdata_ref;
    Scalar scaling;
    
332
333
334
335
336
    /// \brief Transform configuration vector
    ConfigVector_t q_transform;
    /// \brief Transform velocity vector.
    TangentVector_t v_transform;
    
337
  public:
338
    
339
340
    // data
    Constraint_t S;
341
342
343
    
  }; // struct JointDataMimic
  
344
345
346
347
348
349
350
  template<typename NewScalar, typename JointModel>
  struct CastType< NewScalar, JointModelMimic<JointModel> >
  {
    typedef typename CastType<NewScalar,JointModel>::type JointModelNewType;
    typedef JointModelMimic<JointModelNewType> type;
  };
  
351
352
353
354
355
356
  template<class JointModel>
  struct JointModelMimic
  : public JointModelBase< JointModelMimic<JointModel> >
  {
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
    
357
    typedef typename traits<JointModelMimic>::JointDerived JointDerived;
358
    
359
    PINOCCHIO_JOINT_TYPEDEF_TEMPLATE(JointDerived);
360
    
361
    typedef JointModelBase<JointModelMimic> Base;
362
363
364
    using Base::id;
    using Base::idx_q;
    using Base::idx_v;
365
366
    using Base::nq;
    using Base::nv;
367
368
    using Base::setIndexes;
    
369
    JointModelMimic()
370
    {}
371
372
373
    
    JointModelMimic(const JointModelBase<JointModel> & jmodel,
                    const Scalar & scaling,
374
                    const Scalar & offset)
375
376
377
    : m_jmodel_ref(jmodel.derived())
    , m_scaling(scaling)
    , m_offset(offset)
378
379
380
381
382
    {}
    
    Base & base() { return *static_cast<Base*>(this); }
    const Base & base() const { return *static_cast<const Base*>(this); }
    
383
384
    inline int nq_impl() const { return 0; }
    inline int nv_impl() const { return 0; }
385
    
386
387
    inline int idx_q_impl() const { return m_jmodel_ref.idx_q(); }
    inline int idx_v_impl() const { return m_jmodel_ref.idx_v(); }
388
389
390
391
    
    void setIndexes_impl(JointIndex id, int /*q*/, int /*v*/)
    {
      Base::i_id = id; // Only the id of the joint in the model is different.
392
393
      Base::i_q = m_jmodel_ref.idx_q();
      Base::i_v = m_jmodel_ref.idx_v();
394
395
396
    }
    
    JointDataDerived createData() const
397
    {
398
      return JointDataDerived(m_jmodel_ref.createData(),scaling());
399
    }
400
401
402
    
    template<typename ConfigVector>
    EIGEN_DONT_INLINE
403
    void calc(JointDataDerived & jdata,
404
405
              const typename Eigen::MatrixBase<ConfigVector> & qs) const
    {
406
      typedef typename ConfigVectorAffineTransform<JointDerived>::Type AffineTransform;
407
      
408
409
      AffineTransform::run(qs.head(m_jmodel_ref.nq()),
                           m_scaling,m_offset,jdata.q_transform);
410
      m_jmodel_ref.calc(jdata.jdata_ref,jdata.q_transform);
411
412
413
414
    }
    
    template<typename ConfigVector, typename TangentVector>
    EIGEN_DONT_INLINE
415
    void calc(JointDataDerived & jdata,
416
417
418
              const typename Eigen::MatrixBase<ConfigVector> & qs,
              const typename Eigen::MatrixBase<TangentVector> & vs) const
    {
419
      typedef typename ConfigVectorAffineTransform<JointDerived>::Type AffineTransform;
420
      
421
422
423
      AffineTransform::run(qs.head(m_jmodel_ref.nq()),
                           m_scaling,m_offset,jdata.q_transform);
      jdata.v_transform = m_scaling * vs.head(m_jmodel_ref.nv());
424
425
426
      m_jmodel_ref.calc(jdata.jdata_ref,
                        jdata.q_transform,
                        jdata.v_transform);
427
428
429
430
431
432
433
    }
    
    template<typename Matrix6Like>
    void calc_aba(JointDataDerived & data,
                  const Eigen::MatrixBase<Matrix6Like> & I,
                  const bool update_I) const
    {
434
435
436
      m_jmodel_ref.calc_aba(data.jdata_ref,
                            PINOCCHIO_EIGEN_CONST_CAST(Matrix6Like,I),
                            update_I);
437
438
439
440
441
442
443
444
445
    }
    
    static std::string classname()
    {
      return std::string("JointModelMimic<") + JointModel::classname() + std::string(">");;
    }
    
    std::string shortname() const
    {
446
      return std::string("JointModelMimic<") + m_jmodel_ref.shortname() + std::string(">");
447
448
449
450
451
452
453
    }
    
    /// \returns An expression of *this with the Scalar type casted to NewScalar.
    template<typename NewScalar>
    typename CastType<NewScalar,JointModelMimic>::type cast() const
    {
      typedef typename CastType<NewScalar,JointModelMimic>::type ReturnType;
454
455
456
457
      
      ReturnType res(m_jmodel_ref.template cast<NewScalar>(),
                     (NewScalar)m_scaling,
                     (NewScalar)m_offset);
458
459
460
461
      res.setIndexes(id(),idx_q(),idx_v());
      return res;
    }
    
462
463
464
465
466
467
468
469
470
    const JointModel & jmodel() const { return m_jmodel_ref; }
    JointModel & jmodel() { return m_jmodel_ref; }
    
    const Scalar & scaling() const { return m_scaling; }
    Scalar & scaling() { return m_scaling; }
    
    const Scalar & offset() const { return m_offset; }
    Scalar & offset() { return m_offset; }
    
471
472
473
  protected:
    
    // data
474
475
    JointModel m_jmodel_ref;
    Scalar m_scaling, m_offset;
476
477
478
479
480
481
482
483
484
485
486
487
488
489
490
491
492
493
494
495
496
497
498
499
500
501
502
503
504
505
506
507
508
509
510
511
512
513
514
515
516
517
518
519
520
521
522
523
524
525
526
527
528
529
530
531
532
533
534
535
536
537
538
539
540
541
542
543
544
545
546
547
548
549
550
551
552
553
554
555
556
557
558
559
560
561
562
563
564
565
566
567
568
569
570
571
572
573
574
575
576
577
578
579
580
581
582
    
  public:
    
    /* Acces to dedicated segment in robot config space.  */
    // Const access
    template<typename D>
    typename SizeDepType<NQ>::template SegmentReturn<D>::ConstType
    jointConfigSelector_impl(const Eigen::MatrixBase<D> & a) const
    {
      return SizeDepType<NQ>::segment(a.derived(),
                                      m_jmodel_ref.idx_q(),
                                      m_jmodel_ref.nq());
    }
    
    // Non-const access
    template<typename D>
    typename SizeDepType<NQ>::template SegmentReturn<D>::Type
    jointConfigSelector_impl(Eigen::MatrixBase<D> & a) const
    {
      return SizeDepType<NQ>::segment(a.derived(),
                                      m_jmodel_ref.idx_q(),
                                      m_jmodel_ref.nq());
    }
    
    /* Acces to dedicated segment in robot config velocity space.  */
    // Const access
    template<typename D>
    typename SizeDepType<NV>::template SegmentReturn<D>::ConstType
    jointVelocitySelector_impl(const Eigen::MatrixBase<D> & a) const
    {
      return SizeDepType<NV>::segment(a.derived(),
                                      m_jmodel_ref.idx_v(),
                                      m_jmodel_ref.nv());
    }
    
    // Non-const access
    template<typename D>
    typename SizeDepType<NV>::template SegmentReturn<D>::Type
    jointVelocitySelector_impl(Eigen::MatrixBase<D> & a) const
    {
      return SizeDepType<NV>::segment(a.derived(),
                                      m_jmodel_ref.idx_v(),
                                      m_jmodel_ref.nv());
    }
    
    /* Acces to dedicated columns in a ForceSet or MotionSet matrix.*/
    // Const access
    template<typename D>
    typename SizeDepType<NV>::template ColsReturn<D>::ConstType
    jointCols_impl(const Eigen::MatrixBase<D> & A) const
    {
      return SizeDepType<NV>::middleCols(A.derived(),
                                         m_jmodel_ref.idx_v(),
                                         m_jmodel_ref.nv());
    }
    
    // Non-const access
    template<typename D>
    typename SizeDepType<NV>::template ColsReturn<D>::Type
    jointCols_impl(Eigen::MatrixBase<D> & A) const
    {
      return SizeDepType<NV>::middleCols(A.derived(),
                                         m_jmodel_ref.idx_v(),
                                         m_jmodel_ref.nv());
    }
    
    /* Acces to dedicated rows in a matrix.*/
    // Const access
    template<typename D>
    typename SizeDepType<NV>::template RowsReturn<D>::ConstType
    jointRows_impl(const Eigen::MatrixBase<D> & A) const
    {
      return SizeDepType<NV>::middleRows(A.derived(),
                                         m_jmodel_ref.idx_v(),
                                         m_jmodel_ref.nv());
    }
    
    // Non-const access
    template<typename D>
    typename SizeDepType<NV>::template RowsReturn<D>::Type
    jointRows_impl(Eigen::MatrixBase<D> & A) const
    {
      return SizeDepType<NV>::middleRows(A.derived(),
                                         m_jmodel_ref.idx_v(),
                                         m_jmodel_ref.nv());
    }
    
    /// \brief Returns a block of dimension nv()xnv() located at position idx_v(),idx_v() in the matrix Mat
    // Const access
    template<typename D>
    typename SizeDepType<NV>::template BlockReturn<D>::ConstType
    jointBlock_impl(const Eigen::MatrixBase<D> & Mat) const
    {
      return SizeDepType<NV>::block(Mat.derived(),
                                    m_jmodel_ref.idx_v(),m_jmodel_ref.idx_v(),
                                    m_jmodel_ref.nv(),m_jmodel_ref.nv());
    }
    
    // Non-const access
    template<typename D>
    typename SizeDepType<NV>::template BlockReturn<D>::Type
    jointBlock_impl(Eigen::MatrixBase<D> & Mat) const
    {
      return SizeDepType<NV>::block(Mat.derived(),
                                    m_jmodel_ref.idx_v(),m_jmodel_ref.idx_v(),
                                    m_jmodel_ref.nv(),m_jmodel_ref.nv());
    }
583

584
585
586
587
588
  }; // struct JointModelMimic
  
} // namespace pinocchio

#endif // ifndef __pinocchio_joint_mimic_hpp__