joint-revolute.hpp 24.9 KB
Newer Older
1
//
2
// Copyright (c) 2015-2019 CNRS INRIA
3
// Copyright (c) 2015-2016 Wandercraft, 86 rue de Paris 91400 Orsay, France.
4
5
//

6
7
#ifndef __pinocchio_joint_revolute_hpp__
#define __pinocchio_joint_revolute_hpp__
8

9
#include "pinocchio/math/sincos.hpp"
10
#include "pinocchio/spatial/inertia.hpp"
jcarpent's avatar
jcarpent committed
11
#include "pinocchio/multibody/constraint.hpp"
12
#include "pinocchio/multibody/joint/joint-base.hpp"
13
#include "pinocchio/spatial/spatial-axis.hpp"
jcarpent's avatar
jcarpent committed
14
#include "pinocchio/utils/axis-label.hpp"
15

16
namespace pinocchio
17
18
{

19
  template<typename Scalar, int Options, int axis> struct MotionRevoluteTpl;
20
  
21
22
  template<typename Scalar, int Options, int axis>
  struct SE3GroupAction< MotionRevoluteTpl<Scalar,Options,axis> >
23
  {
24
25
26
27
28
29
30
31
    typedef MotionTpl<Scalar,Options> ReturnType;
  };
  
  template<typename Scalar, int Options, int axis, typename MotionDerived>
  struct MotionAlgebraAction< MotionRevoluteTpl<Scalar,Options,axis>, MotionDerived>
  {
    typedef MotionTpl<Scalar,Options> ReturnType;
  };
Valenza Florian's avatar
Valenza Florian committed
32

33
34
  template<typename _Scalar, int _Options, int axis>
  struct traits< MotionRevoluteTpl<_Scalar,_Options,axis> >
35
  {
36
37
38
39
40
    typedef _Scalar Scalar;
    enum { Options = _Options };
    typedef Eigen::Matrix<Scalar,3,1,Options> Vector3;
    typedef Eigen::Matrix<Scalar,6,1,Options> Vector6;
    typedef Eigen::Matrix<Scalar,6,6,Options> Matrix6;
gabrielebndn's avatar
gabrielebndn committed
41
42
    typedef typename PINOCCHIO_EIGEN_REF_CONST_TYPE(Vector6) ToVectorConstReturnType;
    typedef typename PINOCCHIO_EIGEN_REF_TYPE(Vector6) ToVectorReturnType;
43
44
45
46
47
    typedef Vector3 AngularType;
    typedef Vector3 LinearType;
    typedef const Vector3 ConstAngularType;
    typedef const Vector3 ConstLinearType;
    typedef Matrix6 ActionMatrixType;
48
    typedef MotionTpl<Scalar,Options> MotionPlain;
49
    typedef MotionPlain PlainReturnType;
50
51
52
53
    enum {
      LINEAR = 0,
      ANGULAR = 3
    };
54
  }; // traits MotionRevoluteTpl
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
  
  template<typename Scalar, int Options, int axis> struct TransformRevoluteTpl;
  
  template<typename _Scalar, int _Options, int _axis>
  struct traits< TransformRevoluteTpl<_Scalar,_Options,_axis> >
  {
    enum {
      axis = _axis,
      Options = _Options,
      LINEAR = 0,
      ANGULAR = 3
    };
    typedef _Scalar Scalar;
    typedef SE3Tpl<Scalar,Options> PlainType;
    typedef Eigen::Matrix<Scalar,3,1,Options> Vector3;
    typedef Eigen::Matrix<Scalar,3,3,Options> Matrix3;
    typedef Matrix3 AngularType;
    typedef Matrix3 AngularRef;
    typedef Matrix3 ConstAngularRef;
    typedef typename Vector3::ConstantReturnType LinearType;
    typedef typename Vector3::ConstantReturnType LinearRef;
    typedef const typename Vector3::ConstantReturnType ConstLinearRef;
    typedef typename traits<PlainType>::ActionMatrixType ActionMatrixType;
    typedef typename traits<PlainType>::HomogeneousMatrixType HomogeneousMatrixType;
  }; // traits TransformRevoluteTpl
  
81
82
83
84
  template<typename Scalar, int Options, int axis>
  struct SE3GroupAction< TransformRevoluteTpl<Scalar,Options,axis> >
  { typedef typename traits <TransformRevoluteTpl<Scalar,Options,axis> >::PlainType ReturnType; };

85
86
87
88
  template<typename _Scalar, int _Options, int axis>
  struct TransformRevoluteTpl : SE3Base< TransformRevoluteTpl<_Scalar,_Options,axis> >
  {
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
gabrielebndn's avatar
gabrielebndn committed
89
    PINOCCHIO_SE3_TYPEDEF_TPL(TransformRevoluteTpl);
90
91
92
93
94
95
96
97
98
99
    typedef typename traits<TransformRevoluteTpl>::PlainType PlainType;
    
    TransformRevoluteTpl() {}
    TransformRevoluteTpl(const Scalar & sin, const Scalar & cos)
    : m_sin(sin), m_cos(cos)
    {}
    
    PlainType plain() const
    {
      PlainType res(PlainType::Identity());
100
      _setRotation (res.rotation());
101
102
103
104
105
106
      return res;
    }
    
    operator PlainType() const { return plain(); }
    
    template<typename S2, int O2>
107
    typename SE3GroupAction<TransformRevoluteTpl>::ReturnType
108
109
    se3action(const SE3Tpl<S2,O2> & m) const
    {
110
      typedef typename SE3GroupAction<TransformRevoluteTpl>::ReturnType ReturnType;
111
112
113
114
115
116
117
118
119
120
121
122
123
      ReturnType res;
      switch(axis)
      {
        case 0:
        {
          res.rotation().col(0) = m.rotation().col(0);
          res.rotation().col(1).noalias() = m_cos * m.rotation().col(1) + m_sin * m.rotation().col(2);
          res.rotation().col(2).noalias() = res.rotation().col(0).cross(res.rotation().col(1));
          break;
        }
        case 1:
        {
          res.rotation().col(2).noalias() = m_cos * m.rotation().col(2) + m_sin * m.rotation().col(0);
124
          res.rotation().col(1) = m.rotation().col(1);
125
126
127
128
129
130
131
          res.rotation().col(0).noalias() = res.rotation().col(1).cross(res.rotation().col(2));
          break;
        }
        case 2:
        {
          res.rotation().col(0).noalias() = m_cos * m.rotation().col(0) + m_sin * m.rotation().col(1);
          res.rotation().col(1).noalias() = res.rotation().col(2).cross(res.rotation().col(0));
132
          res.rotation().col(2) = m.rotation().col(2);
133
134
135
136
137
138
139
140
141
142
143
144
145
          break;
        }
        default:
        {
          assert(false && "must nerver happened");
          break;
        }
      }
      res.translation() = m.translation();
      return res;
    }
    
    const Scalar & sin() const { return m_sin; }
Justin Carpentier's avatar
Justin Carpentier committed
146
147
    Scalar & sin() { return m_sin; }
    
148
    const Scalar & cos() const { return m_cos; }
Justin Carpentier's avatar
Justin Carpentier committed
149
    Scalar & cos() { return m_cos; }
150
    
151
152
    template<typename OtherScalar>
    void setValues(const OtherScalar & sin, const OtherScalar & cos)
153
    { m_sin = sin; m_cos = cos; }
154

155
    LinearType translation() const { return LinearType::PlainObject::Zero(3); };
156
157
158
159
160
    AngularType rotation() const {
      AngularType m(AngularType::Identity(3));
      _setRotation (m);
      return m;
    }
161
    
162
163
164
165
166
    bool isEqual(const TransformRevoluteTpl & other) const
    {
      return m_cos == other.m_cos && m_sin == other.m_sin;
    }
    
167
168
169
  protected:
    
    Scalar m_sin, m_cos;
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
    inline void _setRotation (typename PlainType::AngularRef& rot) const
    {
      switch(axis)
      {
        case 0:
        {
          rot.coeffRef(1,1) = m_cos; rot.coeffRef(1,2) = -m_sin;
          rot.coeffRef(2,1) = m_sin; rot.coeffRef(2,2) =  m_cos;
          break;
        }
        case 1:
        {
          rot.coeffRef(0,0) =  m_cos; rot.coeffRef(0,2) = m_sin;
          rot.coeffRef(2,0) = -m_sin; rot.coeffRef(2,2) = m_cos;
          break;
        }
        case 2:
        {
          rot.coeffRef(0,0) = m_cos; rot.coeffRef(0,1) = -m_sin;
          rot.coeffRef(1,0) = m_sin; rot.coeffRef(1,1) =  m_cos;
          break;
        }
        default:
        {
          assert(false && "must nerver happened");
          break;
        }
      }
    }
199
  };
200

201
  template<typename _Scalar, int _Options, int axis>
202
203
  struct MotionRevoluteTpl
  : MotionBase< MotionRevoluteTpl<_Scalar,_Options,axis> >
Valenza Florian's avatar
Valenza Florian committed
204
  {
205
206
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
    
207
208
    MOTION_TYPEDEF_TPL(MotionRevoluteTpl);
    typedef SpatialAxis<axis+ANGULAR> Axis;
209
    typedef typename Axis::CartesianAxis3 CartesianAxis3;
210

211
    MotionRevoluteTpl() {}
212
    
213
    MotionRevoluteTpl(const Scalar & w) : m_w(w)  {}
214
215
216
    
    template<typename Vector1Like>
    MotionRevoluteTpl(const Eigen::MatrixBase<Vector1Like> & v)
217
    : m_w(v[0])
218
219
220
221
    {
      using namespace Eigen;
      EIGEN_STATIC_ASSERT_SIZE_1x1(Vector1Like);
    }
222
    
223
    inline PlainReturnType plain() const { return Axis() * m_w; }
224
    
225
226
227
    template<typename OtherScalar>
    MotionRevoluteTpl __mult__(const OtherScalar & alpha) const
    {
228
      return MotionRevoluteTpl(alpha*m_w);
229
230
    }
    
231
232
233
234
235
    template<typename MotionDerived>
    void setTo(MotionDense<MotionDerived> & m) const
    {
      m.linear().setZero();
      for(Eigen::DenseIndex k = 0; k < 3; ++k)
236
        m.angular()[k] = k == axis ? m_w : (Scalar)0;
237
238
    }
    
239
    template<typename MotionDerived>
240
    inline void addTo(MotionDense<MotionDerived> & v) const
241
    {
242
      typedef typename MotionDense<MotionDerived>::Scalar OtherScalar;
243
      v.angular()[axis] += (OtherScalar)m_w;
244
    }
245
    
246
    template<typename S2, int O2, typename D2>
247
    inline void se3Action_impl(const SE3Tpl<S2,O2> & m, MotionDense<D2> & v) const
248
    {
249
      v.angular().noalias() = m.rotation().col(axis) * m_w;
250
251
252
253
254
255
256
257
258
259
260
261
      v.linear().noalias() = m.translation().cross(v.angular());
    }
    
    template<typename S2, int O2>
    MotionPlain se3Action_impl(const SE3Tpl<S2,O2> & m) const
    {
      MotionPlain res;
      se3Action_impl(m,res);
      return res;
    }
    
    template<typename S2, int O2, typename D2>
262
263
    void se3ActionInverse_impl(const SE3Tpl<S2,O2> & m,
                               MotionDense<D2> & v) const
264
265
    {
      // Linear
266
      CartesianAxis3::alphaCross(m_w,m.translation(),v.angular());
267
      v.linear().noalias() = m.rotation().transpose() * v.angular();
268
269
      
      // Angular
270
      v.angular().noalias() = m.rotation().transpose().col(axis) * m_w;
271
272
273
274
275
276
277
278
279
280
281
    }
    
    template<typename S2, int O2>
    MotionPlain se3ActionInverse_impl(const SE3Tpl<S2,O2> & m) const
    {
      MotionPlain res;
      se3ActionInverse_impl(m,res);
      return res;
    }
    
    template<typename M1, typename M2>
282
    EIGEN_STRONG_INLINE
283
284
285
    void motionAction(const MotionDense<M1> & v, MotionDense<M2> & mout) const
    {
      // Linear
286
      CartesianAxis3::alphaCross(-m_w,v.linear(),mout.linear());
287

288
      // Angular
289
      CartesianAxis3::alphaCross(-m_w,v.angular(),mout.angular());
290
291
292
293
294
295
296
297
298
299
    }
    
    template<typename M1>
    MotionPlain motionAction(const MotionDense<M1> & v) const
    {
      MotionPlain res;
      motionAction(v,res);
      return res;
    }
    
300
301
302
    Scalar & angularRate() { return m_w; }
    const Scalar & angularRate() const { return m_w; }
    
303
304
305
306
307
    bool isEqual_impl(const MotionRevoluteTpl & other) const
    {
      return m_w == other.m_w;
    }
    
308
309
310
  protected:
    
    Scalar m_w;
311
312
313
314
315
316
  }; // struct MotionRevoluteTpl

  template<typename S1, int O1, int axis, typename MotionDerived>
  typename MotionDerived::MotionPlain
  operator+(const MotionRevoluteTpl<S1,O1,axis> & m1,
            const MotionDense<MotionDerived> & m2)
Valenza Florian's avatar
Valenza Florian committed
317
  {
318
319
320
    typename MotionDerived::MotionPlain res(m2);
    res += m1;
    return res;
Valenza Florian's avatar
Valenza Florian committed
321
  }
322
323
324
325
326
327
328
329
  
  template<typename MotionDerived, typename S2, int O2, int axis>
  EIGEN_STRONG_INLINE
  typename MotionDerived::MotionPlain
  operator^(const MotionDense<MotionDerived> & m1, const MotionRevoluteTpl<S2,O2,axis>& m2)
  {
    return m2.motionAction(m1);
  }
Valenza Florian's avatar
Valenza Florian committed
330

331
  template<typename Scalar, int Options, int axis> struct ConstraintRevoluteTpl;
332
  
333
334
335
336
337
338
339
  template<typename Scalar, int Options, int axis>
  struct SE3GroupAction< ConstraintRevoluteTpl<Scalar,Options,axis> >
  { typedef Eigen::Matrix<Scalar,6,1,Options> ReturnType; };
  
  template<typename Scalar, int Options, int axis, typename MotionDerived>
  struct MotionAlgebraAction< ConstraintRevoluteTpl<Scalar,Options,axis>, MotionDerived >
  { typedef Eigen::Matrix<Scalar,6,1,Options> ReturnType; };
340
341
342
    
  template<typename Scalar, int Options, int axis, typename ForceDerived>
  struct ConstraintForceOp< ConstraintRevoluteTpl<Scalar,Options,axis>, ForceDerived>
343
  { typedef typename ForceDense<ForceDerived>::ConstAngularType::template ConstFixedSegmentReturnType<1>::Type ReturnType; };
344
345
346
347
  
  template<typename Scalar, int Options, int axis, typename ForceSet>
  struct ConstraintForceSetOp< ConstraintRevoluteTpl<Scalar,Options,axis>, ForceSet>
  { typedef typename Eigen::MatrixBase<ForceSet>::ConstRowXpr ReturnType; };
348

349
350
  template<typename _Scalar, int _Options, int axis>
  struct traits< ConstraintRevoluteTpl<_Scalar,_Options,axis> >
Valenza Florian's avatar
Valenza Florian committed
351
  {
352
353
    typedef _Scalar Scalar;
    enum { Options = _Options };
Valenza Florian's avatar
Valenza Florian committed
354
355
356
    enum {
      LINEAR = 0,
      ANGULAR = 3
357
    };
358
    typedef MotionRevoluteTpl<Scalar,Options,axis> JointMotion;
359
360
    typedef Eigen::Matrix<Scalar,1,1,Options> JointForce;
    typedef Eigen::Matrix<Scalar,6,1,Options> DenseBase;
361
362
    typedef DenseBase MatrixReturnType;
    typedef const DenseBase ConstMatrixReturnType;
363
  }; // traits ConstraintRevoluteTpl
364

365
  template<typename _Scalar, int _Options, int axis>
366
367
  struct ConstraintRevoluteTpl
  : ConstraintBase< ConstraintRevoluteTpl<_Scalar,_Options,axis> >
368
369
  {
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
370
    
371
    PINOCCHIO_CONSTRAINT_TYPEDEF_TPL(ConstraintRevoluteTpl)
372
    enum { NV = 1 };
373
    
374
    typedef SpatialAxis<ANGULAR+axis> Axis;
375
376
    
    ConstraintRevoluteTpl() {}
377
378

    template<typename Vector1Like>
379
380
    JointMotion __mult__(const Eigen::MatrixBase<Vector1Like> & v) const
    { return JointMotion(v[0]); }
381
    
382
    template<typename S1, int O1>
383
    typename SE3GroupAction<ConstraintRevoluteTpl>::ReturnType
384
    se3Action(const SE3Tpl<S1,O1> & m) const
385
    {
386
      typedef typename SE3GroupAction<ConstraintRevoluteTpl>::ReturnType ReturnType;
387
      ReturnType res;
388
389
      res.template segment<3>(LINEAR) = m.translation().cross(m.rotation().col(axis));
      res.template segment<3>(ANGULAR) = m.rotation().col(axis);
Valenza Florian's avatar
Valenza Florian committed
390
391
      return res;
    }
392
393
394
395
396
397
398
399
400
401
402
403
    
    template<typename S1, int O1>
    typename SE3GroupAction<ConstraintRevoluteTpl>::ReturnType
    se3ActionInverse(const SE3Tpl<S1,O1> & m) const
    {
      typedef typename SE3GroupAction<ConstraintRevoluteTpl>::ReturnType ReturnType;
      typedef typename Axis::CartesianAxis3 CartesianAxis3;
      ReturnType res;
      res.template segment<3>(LINEAR).noalias() = m.rotation().transpose()*CartesianAxis3::cross(m.translation());
      res.template segment<3>(ANGULAR) = m.rotation().transpose().col(axis);
      return res;
    }
Valenza Florian's avatar
Valenza Florian committed
404

405
406
    int nv_impl() const { return NV; }
    
Valenza Florian's avatar
Valenza Florian committed
407
408
    struct TransposeConst
    {
409
410
      const ConstraintRevoluteTpl & ref;
      TransposeConst(const ConstraintRevoluteTpl & ref) : ref(ref) {}
411

412
413
414
      template<typename ForceDerived>
      typename ConstraintForceOp<ConstraintRevoluteTpl,ForceDerived>::ReturnType
      operator*(const ForceDense<ForceDerived> & f) const
Valenza Florian's avatar
Valenza Florian committed
415
      { return f.angular().template segment<1>(axis); }
416

417
418
419
420
      /// [CRBA]  MatrixBase operator* (Constraint::Transpose S, ForceSet::Block)
      template<typename Derived>
      typename ConstraintForceSetOp<ConstraintRevoluteTpl,Derived>::ReturnType
      operator*(const Eigen::MatrixBase<Derived> & F) const
421
      {
Valenza Florian's avatar
Valenza Florian committed
422
        assert(F.rows()==6);
423
        return F.row(ANGULAR + axis);
424
      }
Valenza Florian's avatar
Valenza Florian committed
425
426
427
428
429
430
431
432
433
434
    }; // struct TransposeConst

    TransposeConst transpose() const { return TransposeConst(*this); }

    /* CRBA joint operators
     *   - ForceSet::Block = ForceSet
     *   - ForceSet operator* (Inertia Y,Constraint S)
     *   - MatrixBase operator* (Constraint::Transpose S, ForceSet::Block)
     *   - SE3::act(ForceSet::Block)
     */
435
436
437
438
439
    DenseBase matrix_impl() const
    {
      DenseBase S;
      MotionRef<DenseBase> v(S);
      v << Axis();
440
      return S;
Valenza Florian's avatar
Valenza Florian committed
441
    }
442
    
443
    template<typename MotionDerived>
444
    typename MotionAlgebraAction<ConstraintRevoluteTpl,MotionDerived>::ReturnType
445
    motionAction(const MotionDense<MotionDerived> & m) const
446
    {
447
      typedef typename MotionAlgebraAction<ConstraintRevoluteTpl,MotionDerived>::ReturnType ReturnType;
448
449
      ReturnType res;
      MotionRef<ReturnType> v(res);
450
      v = m.cross(Axis());
451
452
      return res;
    }
453
454
455
    
    bool isEqual(const ConstraintRevoluteTpl &) const { return true; }
    
456
  }; // struct ConstraintRevoluteTpl
457

458
  template<typename _Scalar, int _Options, int _axis>
459
  struct JointRevoluteTpl
Valenza Florian's avatar
Valenza Florian committed
460
  {
461
462
463
464
465
466
467
    typedef _Scalar Scalar;
    
    enum
    {
      Options = _Options,
      axis = _axis
    };
468
  };
469
470
471
472
473
474
  
  template<typename S1, int O1,typename S2, int O2, int axis>
  struct MultiplicationOp<InertiaTpl<S1,O1>, ConstraintRevoluteTpl<S2,O2,axis> >
  {
    typedef Eigen::Matrix<S2,6,1,O2> ReturnType;
  };
475

476
  /* [CRBA] ForceSet operator* (Inertia Y,Constraint S) */
477
  namespace impl
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
    template<typename S1, int O1, typename S2, int O2>
    struct LhsMultiplicationOp<InertiaTpl<S1,O1>, ConstraintRevoluteTpl<S2,O2,0> >
    {
      typedef InertiaTpl<S1,O1> Inertia;
      typedef ConstraintRevoluteTpl<S2,O2,0> Constraint;
      typedef typename MultiplicationOp<Inertia,Constraint>::ReturnType ReturnType;
      static inline ReturnType run(const Inertia & Y,
                                   const Constraint & /*constraint*/)
      {
        ReturnType res;
        
        /* Y(:,3) = ( 0,-z, y,  I00+yy+zz,  I01-xy   ,  I02-xz   ) */
        const S1
        &m = Y.mass(),
        &x = Y.lever()[0],
        &y = Y.lever()[1],
        &z = Y.lever()[2];
        const typename Inertia::Symmetric3 & I = Y.inertia();
        
        res <<
        (S2)0,
        -m*z,
        m*y,
        I(0,0)+m*(y*y+z*z),
        I(0,1)-m*x*y,
        I(0,2)-m*x*z;
        
        return res;
      }
    };
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
    template<typename S1, int O1, typename S2, int O2>
    struct LhsMultiplicationOp<InertiaTpl<S1,O1>, ConstraintRevoluteTpl<S2,O2,1> >
    {
      typedef InertiaTpl<S1,O1> Inertia;
      typedef ConstraintRevoluteTpl<S2,O2,1> Constraint;
      typedef typename MultiplicationOp<Inertia,Constraint>::ReturnType ReturnType;
      static inline ReturnType run(const Inertia & Y,
                                   const Constraint & /*constraint*/)
      {
        ReturnType res;
        
        /* Y(:,4) = ( z, 0,-x,  I10-xy   ,  I11+xx+zz,  I12-yz   ) */
        const S1
        &m = Y.mass(),
        &x = Y.lever()[0],
        &y = Y.lever()[1],
        &z = Y.lever()[2];
        const typename Inertia::Symmetric3 & I = Y.inertia();
        
        res <<
        m*z,
        (S2)0,
        -m*x,
        I(1,0)-m*x*y,
        I(1,1)+m*(x*x+z*z),
        I(1,2)-m*y*z;
        
        return res;
      }
    };
    
    template<typename S1, int O1, typename S2, int O2>
    struct LhsMultiplicationOp<InertiaTpl<S1,O1>, ConstraintRevoluteTpl<S2,O2,2> >
    {
      typedef InertiaTpl<S1,O1> Inertia;
      typedef ConstraintRevoluteTpl<S2,O2,2> Constraint;
      typedef typename MultiplicationOp<Inertia,Constraint>::ReturnType ReturnType;
      static inline ReturnType run(const Inertia & Y,
                                   const Constraint & /*constraint*/)
      {
        ReturnType res;
        
        /* Y(:,5) = (-y, x, 0,  I20-xz   ,  I21-yz   ,  I22+xx+yy) */
        const S1
        &m = Y.mass(),
        &x = Y.lever()[0],
        &y = Y.lever()[1],
        &z = Y.lever()[2];
        const typename Inertia::Symmetric3 & I = Y.inertia();
        
        res <<
        -m*y,
        m*x,
        (S2)0,
        I(2,0)-m*x*z,
        I(2,1)-m*y*z,
        I(2,2)+m*(x*x+y*y);
        
        return res;
      }
    };
  } // namespace impl
  
  template<typename M6Like,typename S2, int O2, int axis>
  struct MultiplicationOp<Eigen::MatrixBase<M6Like>, ConstraintRevoluteTpl<S2,O2,axis> >
575
  {
576
577
    typedef typename M6Like::ConstColXpr ReturnType;
  };
578
  
579
580
  /* [ABA] operator* (Inertia Y,Constraint S) */
  namespace impl
581
  {
582
583
584
585
586
587
588
589
590
591
592
593
594
    template<typename M6Like, typename Scalar, int Options, int axis>
    struct LhsMultiplicationOp<Eigen::MatrixBase<M6Like>, ConstraintRevoluteTpl<Scalar,Options,axis> >
    {
      typedef ConstraintRevoluteTpl<Scalar,Options,axis> Constraint;
      typedef typename MultiplicationOp<Eigen::MatrixBase<M6Like>,Constraint>::ReturnType ReturnType;
      static inline ReturnType run(const Eigen::MatrixBase<M6Like> & Y,
                                   const Constraint & /*constraint*/)
      {
        EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(M6Like,6,6);
        return Y.col(Inertia::ANGULAR + axis);
      }
    };
  } // namespace impl
595

596
597
  template<typename _Scalar, int _Options, int axis>
  struct traits< JointRevoluteTpl<_Scalar,_Options,axis> >
598
  {
599
600
601
602
    enum {
      NQ = 1,
      NV = 1
    };
603
604
605
606
607
    typedef _Scalar Scalar;
    enum { Options = _Options };
    typedef JointDataRevoluteTpl<Scalar,Options,axis> JointDataDerived;
    typedef JointModelRevoluteTpl<Scalar,Options,axis> JointModelDerived;
    typedef ConstraintRevoluteTpl<Scalar,Options,axis> Constraint_t;
608
    typedef TransformRevoluteTpl<Scalar,Options,axis> Transformation_t;
609
    typedef MotionRevoluteTpl<Scalar,Options,axis> Motion_t;
610
    typedef MotionZeroTpl<Scalar,Options> Bias_t;
Justin Carpentier's avatar
Justin Carpentier committed
611

612
    // [ABA]
613
614
615
    typedef Eigen::Matrix<Scalar,6,NV,Options> U_t;
    typedef Eigen::Matrix<Scalar,NV,NV,Options> D_t;
    typedef Eigen::Matrix<Scalar,6,NV,Options> UD_t;
616
    
Gabriele Buondonno's avatar
Gabriele Buondonno committed
617
    PINOCCHIO_JOINT_DATA_BASE_ACCESSOR_DEFAULT_RETURN_TYPE
618

619
620
    typedef Eigen::Matrix<Scalar,NQ,1,Options> ConfigVector_t;
    typedef Eigen::Matrix<Scalar,NV,1,Options> TangentVector_t;
621
622
  };

623
624
625
626
627
628
629
  template<typename Scalar, int Options, int axis>
  struct traits< JointDataRevoluteTpl<Scalar,Options,axis> >
  { typedef JointRevoluteTpl<Scalar,Options,axis> JointDerived; };
  
  template<typename Scalar, int Options, int axis>
  struct traits< JointModelRevoluteTpl<Scalar,Options,axis> >
  { typedef JointRevoluteTpl<Scalar,Options,axis> JointDerived; };
630

631
632
  template<typename _Scalar, int _Options, int axis>
  struct JointDataRevoluteTpl : public JointDataBase< JointDataRevoluteTpl<_Scalar,_Options,axis> >
633
  {
634
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
635
    typedef JointRevoluteTpl<_Scalar,_Options,axis> JointDerived;
636
    PINOCCHIO_JOINT_DATA_TYPEDEF_TEMPLATE(JointDerived);
Gabriele Buondonno's avatar
Gabriele Buondonno committed
637
    PINOCCHIO_JOINT_DATA_BASE_DEFAULT_ACCESSOR
638
639
640
641
642

    Constraint_t S;
    Transformation_t M;
    Motion_t v;
    Bias_t c;
643

644
645
646
647
648
    // [ABA] specific data
    U_t U;
    D_t Dinv;
    UD_t UDinv;

649
    JointDataRevoluteTpl()
650
651
652
    : M((Scalar)0,(Scalar)1)
    , v((Scalar)0)
    , U(U_t::Zero())
653
654
655
    , Dinv(D_t::Zero())
    , UDinv(UD_t::Zero())
    {}
656

657
658
659
660
    static std::string classname()
    {
      return std::string("JointDataR") + axisLabel<axis>();
    }
661
662
    std::string shortname() const { return classname(); }
    
663
  }; // struct JointDataRevoluteTpl
664
665
666
667
668
669
  
  template<typename NewScalar, typename Scalar, int Options, int axis>
  struct CastType< NewScalar, JointModelRevoluteTpl<Scalar,Options,axis> >
  {
    typedef JointModelRevoluteTpl<NewScalar,Options,axis> type;
  };
670

671
  template<typename _Scalar, int _Options, int axis>
672
673
  struct JointModelRevoluteTpl
  : public JointModelBase< JointModelRevoluteTpl<_Scalar,_Options,axis> >
674
  {
675
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
676
    typedef JointRevoluteTpl<_Scalar,_Options,axis> JointDerived;
677
    PINOCCHIO_JOINT_TYPEDEF_TEMPLATE(JointDerived);
678

jcarpent's avatar
jcarpent committed
679
680
681
682
683
    typedef JointModelBase<JointModelRevoluteTpl> Base;
    using Base::id;
    using Base::idx_q;
    using Base::idx_v;
    using Base::setIndexes;
684
    
685
    JointDataDerived createData() const { return JointDataDerived(); }
686
    
687
688
    JointModelRevoluteTpl() {}
    
689
    template<typename ConfigVector>
690
    EIGEN_DONT_INLINE
691
692
    void calc(JointDataDerived & data,
              const typename Eigen::MatrixBase<ConfigVector> & qs) const
693
    {
694
695
696
697
      typedef typename ConfigVector::Scalar OtherScalar;
      
      const OtherScalar & q = qs[idx_q()];
      OtherScalar ca,sa; SINCOS(q,&sa,&ca);
698
      data.M.setValues(sa,ca);
699
700
    }

701
    template<typename ConfigVector, typename TangentVector>
702
    EIGEN_DONT_INLINE
703
704
705
    void calc(JointDataDerived & data,
              const typename Eigen::MatrixBase<ConfigVector> & qs,
              const typename Eigen::MatrixBase<TangentVector> & vs) const
706
    {
707
      calc(data,qs.derived());
708

709
      data.v.angularRate() = static_cast<Scalar>(vs[idx_v()]);
710
    }
711
    
712
    template<typename Matrix6Like>
713
714
715
    void calc_aba(JointDataDerived & data,
                  const Eigen::MatrixBase<Matrix6Like> & I,
                  const bool update_I) const
716
717
    {
      data.U = I.col(Inertia::ANGULAR + axis);
718
      data.Dinv[0] = Scalar(1)/I(Inertia::ANGULAR + axis,Inertia::ANGULAR + axis);
719
      data.UDinv.noalias() = data.U * data.Dinv[0];
720
721
      
      if (update_I)
gabrielebndn's avatar
gabrielebndn committed
722
        PINOCCHIO_EIGEN_CONST_CAST(Matrix6Like,I) -= data.UDinv * data.U.transpose();
723
    }
724
    
725
726
727
728
    static std::string classname()
    {
      return std::string("JointModelR") + axisLabel<axis>();
    }
729
    std::string shortname() const { return classname(); }
730
731
732
733
734
735
736
737
738
739
740
    
    /// \returns An expression of *this with the Scalar type casted to NewScalar.
    template<typename NewScalar>
    JointModelRevoluteTpl<NewScalar,Options,axis> cast() const
    {
      typedef JointModelRevoluteTpl<NewScalar,Options,axis> ReturnType;
      ReturnType res;
      res.setIndexes(id(),idx_q(),idx_v());
      return res;
    }
    
741
  }; // struct JointModelRevoluteTpl
742

743
744
745
  typedef JointRevoluteTpl<double,0,0> JointRX;
  typedef JointDataRevoluteTpl<double,0,0> JointDataRX;
  typedef JointModelRevoluteTpl<double,0,0> JointModelRX;
746

747
748
749
  typedef JointRevoluteTpl<double,0,1> JointRY;
  typedef JointDataRevoluteTpl<double,0,1> JointDataRY;
  typedef JointModelRevoluteTpl<double,0,1> JointModelRY;
750

751
752
753
  typedef JointRevoluteTpl<double,0,2> JointRZ;
  typedef JointDataRevoluteTpl<double,0,2> JointDataRZ;
  typedef JointModelRevoluteTpl<double,0,2> JointModelRZ;
754

755
} //namespace pinocchio
756

757
758
759
760
761
#include <boost/type_traits.hpp>

namespace boost
{
  template<typename Scalar, int Options, int axis>
762
  struct has_nothrow_constructor< ::pinocchio::JointModelRevoluteTpl<Scalar,Options,axis> >
763
764
765
  : public integral_constant<bool,true> {};
  
  template<typename Scalar, int Options, int axis>
766
  struct has_nothrow_copy< ::pinocchio::JointModelRevoluteTpl<Scalar,Options,axis> >
767
768
769
  : public integral_constant<bool,true> {};
  
  template<typename Scalar, int Options, int axis>
770
  struct has_nothrow_constructor< ::pinocchio::JointDataRevoluteTpl<Scalar,Options,axis> >
771
772
773
  : public integral_constant<bool,true> {};
  
  template<typename Scalar, int Options, int axis>
774
  struct has_nothrow_copy< ::pinocchio::JointDataRevoluteTpl<Scalar,Options,axis> >
775
776
777
  : public integral_constant<bool,true> {};
}

778
#endif // ifndef __pinocchio_joint_revolute_hpp__