joint-mimic.hpp 21.6 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
75
76
77
    explicit ScaledConstraint(const Scalar & scaling_factor)
    : m_scaling_factor(scaling_factor)
    {}
    
78
    ScaledConstraint(const Constraint & constraint,
79
                     const Scalar & scaling_factor)
80
81
    : m_constraint(constraint)
    , m_scaling_factor(scaling_factor)
82
83
    {}
    
84
85
86
87
88
89
90
    ScaledConstraint & operator=(const ScaledConstraint & other)
    {
      m_constraint = other.m_constraint;
      m_scaling_factor = other.m_scaling_factor;
      return *this;
    }
    
91
92
93
94
    template<typename VectorLike>
    JointMotion __mult__(const Eigen::MatrixBase<VectorLike> & v) const
    {
      assert(v.size() == nv());
95
96
      JointMotion jm = m_constraint * v;
      return jm * m_scaling_factor;
97
98
99
100
101
102
    }
    
    template<typename S1, int O1>
    SE3ActionReturnType
    se3Action(const SE3Tpl<S1,O1> & m) const
    {
103
      SE3ActionReturnType res = m_constraint.se3Action(m);
104
105
106
107
108
109
110
111
      return m_scaling_factor * res;
    }
    
    template<typename S1, int O1>
    SE3ActionReturnType
    se3ActionInverse(const SE3Tpl<S1,O1> & m) const
    {
      SE3ActionReturnType res = m_constraint.se3ActionInverse(m);
112
      return m_scaling_factor * res;
113
114
    }
    
115
    int nv_impl() const { return m_constraint.nv(); }
116
117
118
119
120
121
122
    
    struct TransposeConst
    {
      const ScaledConstraint & ref;
      TransposeConst(const ScaledConstraint & ref) : ref(ref) {}
      
      template<typename Derived>
Justin Carpentier's avatar
Justin Carpentier committed
123
      typename ConstraintForceOp<ScaledConstraint,Derived>::ReturnType
124
125
      operator*(const ForceDense<Derived> & f) const
      {
126
        // 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
127
        typedef typename ConstraintForceOp<ScaledConstraint,Derived>::ReturnType ReturnType;
128
        return ReturnType(ref.m_scaling_factor * (ref.m_constraint.transpose() * f));
129
130
131
132
      }
      
      /// [CRBA]  MatrixBase operator* (Constraint::Transpose S, ForceSet::Block)
      template<typename Derived>
Justin Carpentier's avatar
Justin Carpentier committed
133
      typename ConstraintForceSetOp<ScaledConstraint,Derived>::ReturnType
134
135
      operator*(const Eigen::MatrixBase<Derived> & F) const
      {
136
137
        typedef typename ConstraintForceSetOp<ScaledConstraint,Derived>::ReturnType ReturnType;
        return ReturnType(ref.m_scaling_factor * (ref.m_constraint.transpose() * F));
138
139
140
141
142
143
144
145
      }
      
    }; // struct TransposeConst
    
    TransposeConst transpose() const { return TransposeConst(*this); }
    
    DenseBase matrix_impl() const
    {
146
      DenseBase S = m_scaling_factor * m_constraint.matrix();
147
148
149
150
      return S;
    }
    
    template<typename MotionDerived>
Justin Carpentier's avatar
Justin Carpentier committed
151
    typename MotionAlgebraAction<ScaledConstraint,MotionDerived>::ReturnType
152
153
    motionAction(const MotionDense<MotionDerived> & m) const
    {
Justin Carpentier's avatar
Justin Carpentier committed
154
      typedef typename MotionAlgebraAction<ScaledConstraint,MotionDerived>::ReturnType ReturnType;
155
      ReturnType res = m_scaling_factor * m_constraint.motionAction(m);
156
157
158
      return res;
    }
    
159
    inline const Scalar & scaling() const { return m_scaling_factor; }
160
161
    inline Scalar & scaling() { return m_scaling_factor; }
    
162
    inline const Constraint & constraint() const { return m_constraint; }
163
    inline Constraint & constraint() { return m_constraint; }
164
    
165
166
167
168
169
170
    bool isEqual(const ScaledConstraint & other) const
    {
      return m_constraint == other.m_constraint
      && m_scaling_factor == other.m_scaling_factor;
    }
    
171
172
  protected:
    
173
    Constraint m_constraint;
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
226
227
228
229
    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
230
231
232
233
234
235
236
237
  
  template<class Joint> struct JointMimic;
  template<class JointModel> struct JointModelMimic;
  template<class JointData> struct JointDataMimic;
  
  template<class Joint>
  struct traits< JointMimic<Joint> >
  {
238
239
    enum
    {
240
241
242
243
244
245
      NQ = traits<Joint>::NV,
      NV = traits<Joint>::NQ
    };
    typedef typename traits<Joint>::Scalar Scalar;
    enum { Options = traits<Joint>::Options };
    
246
247
248
249
250
    typedef typename traits<Joint>::JointDataDerived JointDataBase;
    typedef typename traits<Joint>::JointModelDerived JointModelBase;
    
    typedef JointDataMimic<JointDataBase> JointDataDerived;
    typedef JointModelMimic<JointModelBase> JointModelDerived;
251
    
252
    typedef ScaledConstraint<typename traits<Joint>::Constraint_t> Constraint_t;
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
    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> >
270
  { typedef JointMimic<typename traits<Joint>::JointDerived> JointDerived; };
271
272
273
  
  template<class Joint>
  struct traits< JointModelMimic<Joint> >
274
  { typedef JointMimic<typename traits<Joint>::JointDerived> JointDerived; };
275
276
277
  
  template<class JointData>
  struct JointDataMimic
278
  : public JointDataBase< JointDataMimic<JointData> >
279
280
  {
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
281
282
283

    typedef typename traits<JointDataMimic>::JointDerived JointDerived;
    typedef JointDataBase< JointDataMimic<JointData> > Base;
284
    
285
    PINOCCHIO_JOINT_DATA_TYPEDEF_TEMPLATE(JointDerived);
286
    
287
288
289
290
291
292
    JointDataMimic()
    : m_scaling((Scalar)0)
    , m_q_transform(ConfigVector_t::Zero())
    , m_v_transform(TangentVector_t::Zero())
    , S((Scalar)0)
    {}
293
    
294
295
    JointDataMimic(const JointDataBase<JointData> & jdata,
                   const Scalar & scaling)
296
297
    : m_jdata_ref(jdata.derived())
    , m_scaling(scaling)
Justin Carpentier's avatar
Justin Carpentier committed
298
    , S(m_jdata_ref.S,scaling)
299
    {}
300
    
301
302
    JointDataMimic & operator=(const JointDataMimic & other)
    {
303
304
      m_jdata_ref = other.m_jdata_ref;
      m_scaling = other.m_scaling;
305
306
      m_q_transform = other.m_q_transform;
      m_v_transform = other.m_v_transform;
Justin Carpentier's avatar
Justin Carpentier committed
307
      S = Constraint_t(m_jdata_ref.S,other.m_scaling);
308
309
      return *this;
    }
310
    
Justin Carpentier's avatar
Justin Carpentier committed
311
312
313
314
315
316
    bool isEqual(const JointDataMimic & other) const
    {
      return Base::isEqual(other)
      && m_jdata_ref == other.m_jdata_ref
      && m_scaling == other.m_scaling
      && m_q_transform == other.m_q_transform
317
318
      && m_v_transform == other.m_v_transform
      ;
Justin Carpentier's avatar
Justin Carpentier committed
319
320
    }
    
321
322
    static std::string classname()
    {
323
      return std::string("JointDataMimic<") + JointData::classname() + std::string(">");
324
325
326
327
    }
    
    std::string shortname() const
    {
328
      return std::string("JointDataMimic<") + m_jdata_ref.shortname() + std::string(">");
329
330
331
    }
    
    // Accessors
Justin Carpentier's avatar
Justin Carpentier committed
332
333
    ConstraintTypeConstRef S_accessor() const { return S; }
    ConstraintTypeRef S_accessor() { return S; }
334
335
336
337
338
339
340
341
342
343
344
345
346
347
348
349
350
351
    
    TansformTypeConstRef M_accessor() const { return m_jdata_ref.M; }
    TansformTypeRef M_accessor() { return m_jdata_ref.M; }
    
    MotionTypeConstRef v_accessor() const { return m_jdata_ref.v; }
    MotionTypeRef v_accessor() { return m_jdata_ref.v; }
    
    BiasTypeConstRef c_accessor() const { return m_jdata_ref.c; }
    BiasTypeRef c_accessor() { return m_jdata_ref.c; }
    
    UTypeConstRef U_accessor() const { return m_jdata_ref.U; }
    UTypeRef U_accessor() { return m_jdata_ref.U; }
    
    DTypeConstRef Dinv_accessor() const { return m_jdata_ref.Dinv; }
    DTypeRef Dinv_accessor() { return m_jdata_ref.Dinv; }
    
    UDTypeConstRef UDinv_accessor() const { return m_jdata_ref.UDinv; }
    UDTypeRef UDinv_accessor() { return m_jdata_ref.UDinv; }
352
    
353
354
355
    template<class JointModel>
    friend struct JointModelMimic;
    
356
357
358
359
360
361
    const JointData & jdata() const { return m_jdata_ref; }
    JointData & jdata() { return m_jdata_ref; }
    
    const Scalar & scaling() const { return m_scaling; }
    Scalar & scaling() { return m_scaling; }
    
362
363
364
365
366
367
    ConfigVector_t & jointConfiguration() { return m_q_transform; }
    const ConfigVector_t & jointConfiguration() const { return m_q_transform; }
    
    TangentVector_t & jointVelocity() { return m_v_transform; }
    const TangentVector_t & jointVelocity() const { return m_v_transform; }
    
368
369
  protected:
    
370
371
    JointData m_jdata_ref;
    Scalar m_scaling;
372
    
373
    /// \brief Transform configuration vector
374
    ConfigVector_t m_q_transform;
375
    /// \brief Transform velocity vector.
376
    TangentVector_t m_v_transform;
377
    
378
  public:
379
    
380
    // data
Justin Carpentier's avatar
Justin Carpentier committed
381
    Constraint_t S;
382
383
384
    
  }; // struct JointDataMimic
  
385
386
387
388
389
390
391
  template<typename NewScalar, typename JointModel>
  struct CastType< NewScalar, JointModelMimic<JointModel> >
  {
    typedef typename CastType<NewScalar,JointModel>::type JointModelNewType;
    typedef JointModelMimic<JointModelNewType> type;
  };
  
392
393
394
395
396
397
  template<class JointModel>
  struct JointModelMimic
  : public JointModelBase< JointModelMimic<JointModel> >
  {
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
    
398
    typedef typename traits<JointModelMimic>::JointDerived JointDerived;
399
    
400
    PINOCCHIO_JOINT_TYPEDEF_TEMPLATE(JointDerived);
401
    
402
    typedef JointModelBase<JointModelMimic> Base;
403
404
405
    using Base::id;
    using Base::idx_q;
    using Base::idx_v;
406
407
    using Base::nq;
    using Base::nv;
408
409
    using Base::setIndexes;
    
410
    JointModelMimic()
411
    {}
412
413
414
    
    JointModelMimic(const JointModelBase<JointModel> & jmodel,
                    const Scalar & scaling,
415
                    const Scalar & offset)
416
417
418
    : m_jmodel_ref(jmodel.derived())
    , m_scaling(scaling)
    , m_offset(offset)
419
420
421
422
423
    {}
    
    Base & base() { return *static_cast<Base*>(this); }
    const Base & base() const { return *static_cast<const Base*>(this); }
    
424
425
    inline int nq_impl() const { return 0; }
    inline int nv_impl() const { return 0; }
426
    
427
428
    inline int idx_q_impl() const { return m_jmodel_ref.idx_q(); }
    inline int idx_v_impl() const { return m_jmodel_ref.idx_v(); }
429
430
431
432
    
    void setIndexes_impl(JointIndex id, int /*q*/, int /*v*/)
    {
      Base::i_id = id; // Only the id of the joint in the model is different.
433
434
      Base::i_q = m_jmodel_ref.idx_q();
      Base::i_v = m_jmodel_ref.idx_v();
435
436
437
    }
    
    JointDataDerived createData() const
438
    {
439
      return JointDataDerived(m_jmodel_ref.createData(),scaling());
440
    }
441
442
443
    
    template<typename ConfigVector>
    EIGEN_DONT_INLINE
444
    void calc(JointDataDerived & jdata,
445
446
              const typename Eigen::MatrixBase<ConfigVector> & qs) const
    {
447
      typedef typename ConfigVectorAffineTransform<JointDerived>::Type AffineTransform;
448
      
449
      AffineTransform::run(qs.head(m_jmodel_ref.nq()),
450
451
                           m_scaling,m_offset,jdata.m_q_transform);
      m_jmodel_ref.calc(jdata.m_jdata_ref,jdata.m_q_transform);
452
453
454
455
    }
    
    template<typename ConfigVector, typename TangentVector>
    EIGEN_DONT_INLINE
456
    void calc(JointDataDerived & jdata,
457
458
459
              const typename Eigen::MatrixBase<ConfigVector> & qs,
              const typename Eigen::MatrixBase<TangentVector> & vs) const
    {
460
      typedef typename ConfigVectorAffineTransform<JointDerived>::Type AffineTransform;
461
      
462
      AffineTransform::run(qs.head(m_jmodel_ref.nq()),
463
464
465
466
467
                           m_scaling,m_offset,jdata.m_q_transform);
      jdata.m_v_transform = m_scaling * vs.head(m_jmodel_ref.nv());
      m_jmodel_ref.calc(jdata.m_jdata_ref,
                        jdata.m_q_transform,
                        jdata.m_v_transform);
468
469
470
471
472
473
474
    }
    
    template<typename Matrix6Like>
    void calc_aba(JointDataDerived & data,
                  const Eigen::MatrixBase<Matrix6Like> & I,
                  const bool update_I) const
    {
475
476
      // TODO: fixme
      m_jmodel_ref.calc_aba(data.m_jdata_ref,
477
478
                            PINOCCHIO_EIGEN_CONST_CAST(Matrix6Like,I),
                            update_I);
479
480
481
482
483
484
485
486
487
    }
    
    static std::string classname()
    {
      return std::string("JointModelMimic<") + JointModel::classname() + std::string(">");;
    }
    
    std::string shortname() const
    {
488
      return std::string("JointModelMimic<") + m_jmodel_ref.shortname() + std::string(">");
489
490
491
492
493
494
495
    }
    
    /// \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;
496
497
498
499
      
      ReturnType res(m_jmodel_ref.template cast<NewScalar>(),
                     (NewScalar)m_scaling,
                     (NewScalar)m_offset);
500
501
502
503
      res.setIndexes(id(),idx_q(),idx_v());
      return res;
    }
    
504
505
506
507
508
509
510
511
512
    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; }
    
513
514
515
  protected:
    
    // data
516
517
    JointModel m_jmodel_ref;
    Scalar m_scaling, m_offset;
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
583
584
585
586
587
588
589
590
591
592
593
594
595
596
597
598
599
600
601
602
603
604
605
606
607
608
609
610
611
612
613
614
615
616
617
618
619
620
621
622
623
624
    
  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());
    }
625

626
627
628
629
630
  }; // struct JointModelMimic
  
} // namespace pinocchio

#endif // ifndef __pinocchio_joint_mimic_hpp__