joint-revolute.hpp 24.2 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
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
      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(1) = m.rotation().col(1);
          res.rotation().col(2).noalias() = m_cos * m.rotation().col(2) + m_sin * m.rotation().col(0);
          res.rotation().col(0).noalias() = res.rotation().col(1).cross(res.rotation().col(2));
          break;
        }
        case 2:
        {
          res.rotation().col(2) = m.rotation().col(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));
          break;
        }
        default:
        {
          assert(false && "must nerver happened");
          break;
        }
      }
      res.translation() = m.translation();
      return res;
    }
    
    const Scalar & sin() const { return m_sin; }
    const Scalar & cos() const { return m_cos; }
    
148
149
    template<typename OtherScalar>
    void setValues(const OtherScalar & sin, const OtherScalar & cos)
150
    { m_sin = sin; m_cos = cos; }
151

152
    LinearType translation() const { return LinearType::PlainObject::Zero(3); };
153
154
155
156
157
    AngularType rotation() const {
      AngularType m(AngularType::Identity(3));
      _setRotation (m);
      return m;
    }
158
159
160
161
    
  protected:
    
    Scalar m_sin, m_cos;
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
    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;
        }
      }
    }
191
  };
192

193
  template<typename _Scalar, int _Options, int axis>
194
195
  struct MotionRevoluteTpl
  : MotionBase< MotionRevoluteTpl<_Scalar,_Options,axis> >
Valenza Florian's avatar
Valenza Florian committed
196
  {
197
198
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
    
199
200
    MOTION_TYPEDEF_TPL(MotionRevoluteTpl);
    typedef SpatialAxis<axis+ANGULAR> Axis;
201
    typedef typename Axis::CartesianAxis3 CartesianAxis3;
202

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

280
      // Angular
281
      CartesianAxis3::alphaCross(-w,v.angular(),mout.angular());
282
283
284
285
286
287
288
289
290
291
    }
    
    template<typename M1>
    MotionPlain motionAction(const MotionDense<M1> & v) const
    {
      MotionPlain res;
      motionAction(v,res);
      return res;
    }
    
292
293
294
295
296
297
298
299
    // data
    Scalar w;
  }; // 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
300
  {
301
302
303
    typename MotionDerived::MotionPlain res(m2);
    res += m1;
    return res;
Valenza Florian's avatar
Valenza Florian committed
304
  }
305
306
307
308
309
310
311
312
  
  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
313

314
  template<typename Scalar, int Options, int axis> struct ConstraintRevoluteTpl;
315
  
316
317
318
319
320
321
322
  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; };
323
324
325
    
  template<typename Scalar, int Options, int axis, typename ForceDerived>
  struct ConstraintForceOp< ConstraintRevoluteTpl<Scalar,Options,axis>, ForceDerived>
326
  { typedef typename ForceDense<ForceDerived>::ConstAngularType::template ConstFixedSegmentReturnType<1>::Type ReturnType; };
327
328
329
330
  
  template<typename Scalar, int Options, int axis, typename ForceSet>
  struct ConstraintForceSetOp< ConstraintRevoluteTpl<Scalar,Options,axis>, ForceSet>
  { typedef typename Eigen::MatrixBase<ForceSet>::ConstRowXpr ReturnType; };
331

332
333
  template<typename _Scalar, int _Options, int axis>
  struct traits< ConstraintRevoluteTpl<_Scalar,_Options,axis> >
Valenza Florian's avatar
Valenza Florian committed
334
  {
335
336
    typedef _Scalar Scalar;
    enum { Options = _Options };
Valenza Florian's avatar
Valenza Florian committed
337
338
339
    enum {
      LINEAR = 0,
      ANGULAR = 3
340
    };
341
    typedef MotionRevoluteTpl<Scalar,Options,axis> JointMotion;
342
343
    typedef Eigen::Matrix<Scalar,1,1,Options> JointForce;
    typedef Eigen::Matrix<Scalar,6,1,Options> DenseBase;
344
345
    typedef DenseBase MatrixReturnType;
    typedef const DenseBase ConstMatrixReturnType;
346
  }; // traits ConstraintRevoluteTpl
347

348
  template<typename _Scalar, int _Options, int axis>
349
350
  struct ConstraintRevoluteTpl
  : ConstraintBase< ConstraintRevoluteTpl<_Scalar,_Options,axis> >
351
352
  {
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
353
    
354
    PINOCCHIO_CONSTRAINT_TYPEDEF_TPL(ConstraintRevoluteTpl)
355
    enum { NV = 1 };
356
    
357
    typedef SpatialAxis<ANGULAR+axis> Axis;
358
359
    
    ConstraintRevoluteTpl() {}
360
361

    template<typename Vector1Like>
362
363
    JointMotion __mult__(const Eigen::MatrixBase<Vector1Like> & v) const
    { return JointMotion(v[0]); }
364
    
365
    template<typename S1, int O1>
366
    typename SE3GroupAction<ConstraintRevoluteTpl>::ReturnType
367
    se3Action(const SE3Tpl<S1,O1> & m) const
368
    {
369
      typedef typename SE3GroupAction<ConstraintRevoluteTpl>::ReturnType ReturnType;
370
      ReturnType res;
371
372
      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
373
374
      return res;
    }
375
376
377
378
379
380
381
382
383
384
385
386
    
    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
387

388
389
    int nv_impl() const { return NV; }
    
Valenza Florian's avatar
Valenza Florian committed
390
391
    struct TransposeConst
    {
392
393
      const ConstraintRevoluteTpl & ref;
      TransposeConst(const ConstraintRevoluteTpl & ref) : ref(ref) {}
394

395
396
397
      template<typename ForceDerived>
      typename ConstraintForceOp<ConstraintRevoluteTpl,ForceDerived>::ReturnType
      operator*(const ForceDense<ForceDerived> & f) const
Valenza Florian's avatar
Valenza Florian committed
398
      { return f.angular().template segment<1>(axis); }
399

400
401
402
403
      /// [CRBA]  MatrixBase operator* (Constraint::Transpose S, ForceSet::Block)
      template<typename Derived>
      typename ConstraintForceSetOp<ConstraintRevoluteTpl,Derived>::ReturnType
      operator*(const Eigen::MatrixBase<Derived> & F) const
404
      {
Valenza Florian's avatar
Valenza Florian committed
405
        assert(F.rows()==6);
406
        return F.row(ANGULAR + axis);
407
      }
Valenza Florian's avatar
Valenza Florian committed
408
409
410
411
412
413
414
415
416
417
    }; // 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)
     */
418
419
420
421
422
    DenseBase matrix_impl() const
    {
      DenseBase S;
      MotionRef<DenseBase> v(S);
      v << Axis();
423
      return S;
Valenza Florian's avatar
Valenza Florian committed
424
    }
425
    
426
    template<typename MotionDerived>
427
    typename MotionAlgebraAction<ConstraintRevoluteTpl,MotionDerived>::ReturnType
428
    motionAction(const MotionDense<MotionDerived> & m) const
429
    {
430
      typedef typename MotionAlgebraAction<ConstraintRevoluteTpl,MotionDerived>::ReturnType ReturnType;
431
432
      ReturnType res;
      MotionRef<ReturnType> v(res);
433
      v = m.cross(Axis());
434
435
      return res;
    }
436
  }; // struct ConstraintRevoluteTpl
437

438
  template<typename _Scalar, int _Options, int _axis>
439
  struct JointRevoluteTpl
Valenza Florian's avatar
Valenza Florian committed
440
  {
441
442
443
444
445
446
447
    typedef _Scalar Scalar;
    
    enum
    {
      Options = _Options,
      axis = _axis
    };
448
  };
449
450
451
452
453
454
  
  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;
  };
455

456
  /* [CRBA] ForceSet operator* (Inertia Y,Constraint S) */
457
  namespace impl
458
  {
459
460
461
462
463
464
465
466
467
468
469
470
471
472
473
474
475
476
477
478
479
480
481
482
483
484
485
486
487
488
    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;
      }
    };
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
    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> >
555
  {
556
557
    typedef typename M6Like::ConstColXpr ReturnType;
  };
558
  
559
560
  /* [ABA] operator* (Inertia Y,Constraint S) */
  namespace impl
561
  {
562
563
564
565
566
567
568
569
570
571
572
573
574
    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
575

576
577
  template<typename _Scalar, int _Options, int axis>
  struct traits< JointRevoluteTpl<_Scalar,_Options,axis> >
578
  {
579
580
581
582
    enum {
      NQ = 1,
      NV = 1
    };
583
584
585
586
587
    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;
588
    typedef TransformRevoluteTpl<Scalar,Options,axis> Transformation_t;
589
    typedef MotionRevoluteTpl<Scalar,Options,axis> Motion_t;
590
    typedef MotionZeroTpl<Scalar,Options> Bias_t;
Justin Carpentier's avatar
Justin Carpentier committed
591

592
    // [ABA]
593
594
595
    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;
596
    
Gabriele Buondonno's avatar
Gabriele Buondonno committed
597
    PINOCCHIO_JOINT_DATA_BASE_ACCESSOR_DEFAULT_RETURN_TYPE
598

599
600
    typedef Eigen::Matrix<Scalar,NQ,1,Options> ConfigVector_t;
    typedef Eigen::Matrix<Scalar,NV,1,Options> TangentVector_t;
601
602
  };

603
604
605
606
607
608
609
  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; };
610

611
612
  template<typename _Scalar, int _Options, int axis>
  struct JointDataRevoluteTpl : public JointDataBase< JointDataRevoluteTpl<_Scalar,_Options,axis> >
613
  {
614
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
615
    typedef JointRevoluteTpl<_Scalar,_Options,axis> JointDerived;
616
    PINOCCHIO_JOINT_DATA_TYPEDEF_TEMPLATE(JointDerived);
Gabriele Buondonno's avatar
Gabriele Buondonno committed
617
    PINOCCHIO_JOINT_DATA_BASE_DEFAULT_ACCESSOR
618
619
620
621
622

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

624
625
626
627
628
    // [ABA] specific data
    U_t U;
    D_t Dinv;
    UD_t UDinv;

629
    JointDataRevoluteTpl() {}
630

631
632
633
    static std::string classname() { return std::string("JointDataRevolute"); }
    std::string shortname() const { return classname(); }
    
634
  }; // struct JointDataRevoluteTpl
635
636
637
638
639
640
  
  template<typename NewScalar, typename Scalar, int Options, int axis>
  struct CastType< NewScalar, JointModelRevoluteTpl<Scalar,Options,axis> >
  {
    typedef JointModelRevoluteTpl<NewScalar,Options,axis> type;
  };
641

642
  template<typename _Scalar, int _Options, int axis>
643
644
  struct JointModelRevoluteTpl
  : public JointModelBase< JointModelRevoluteTpl<_Scalar,_Options,axis> >
645
  {
646
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
647
    typedef JointRevoluteTpl<_Scalar,_Options,axis> JointDerived;
648
    PINOCCHIO_JOINT_TYPEDEF_TEMPLATE(JointDerived);
649

jcarpent's avatar
jcarpent committed
650
651
652
653
654
    typedef JointModelBase<JointModelRevoluteTpl> Base;
    using Base::id;
    using Base::idx_q;
    using Base::idx_v;
    using Base::setIndexes;
655
    
656
    JointDataDerived createData() const { return JointDataDerived(); }
657
    
658
659
    JointModelRevoluteTpl() {}
    
660
    template<typename ConfigVector>
661
    EIGEN_DONT_INLINE
662
663
    void calc(JointDataDerived & data,
              const typename Eigen::MatrixBase<ConfigVector> & qs) const
664
    {
665
666
667
668
      typedef typename ConfigVector::Scalar OtherScalar;
      
      const OtherScalar & q = qs[idx_q()];
      OtherScalar ca,sa; SINCOS(q,&sa,&ca);
669
      data.M.setValues(sa,ca);
670
671
    }

672
    template<typename ConfigVector, typename TangentVector>
673
    EIGEN_DONT_INLINE
674
675
676
    void calc(JointDataDerived & data,
              const typename Eigen::MatrixBase<ConfigVector> & qs,
              const typename Eigen::MatrixBase<TangentVector> & vs) const
677
    {
678
      calc(data,qs.derived());
679

Qiang Qiu's avatar
Qiang Qiu committed
680
      data.v.w = (Scalar)vs[idx_v()];
681
    }
682
    
683
    template<typename Matrix6Like>
684
685
686
    void calc_aba(JointDataDerived & data,
                  const Eigen::MatrixBase<Matrix6Like> & I,
                  const bool update_I) const
687
688
    {
      data.U = I.col(Inertia::ANGULAR + axis);
689
      data.Dinv[0] = Scalar(1)/I(Inertia::ANGULAR + axis,Inertia::ANGULAR + axis);
690
      data.UDinv.noalias() = data.U * data.Dinv[0];
691
692
      
      if (update_I)
gabrielebndn's avatar
gabrielebndn committed
693
        PINOCCHIO_EIGEN_CONST_CAST(Matrix6Like,I) -= data.UDinv * data.U.transpose();
694
    }
695
    
696
697
698
699
    static std::string classname()
    {
      return std::string("JointModelR") + axisLabel<axis>();
    }
700
    std::string shortname() const { return classname(); }
701
702
703
704
705
706
707
708
709
710
711
    
    /// \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;
    }
    
712
  }; // struct JointModelRevoluteTpl
713

714
715
716
  typedef JointRevoluteTpl<double,0,0> JointRX;
  typedef JointDataRevoluteTpl<double,0,0> JointDataRX;
  typedef JointModelRevoluteTpl<double,0,0> JointModelRX;
717

718
719
720
  typedef JointRevoluteTpl<double,0,1> JointRY;
  typedef JointDataRevoluteTpl<double,0,1> JointDataRY;
  typedef JointModelRevoluteTpl<double,0,1> JointModelRY;
721

722
723
724
  typedef JointRevoluteTpl<double,0,2> JointRZ;
  typedef JointDataRevoluteTpl<double,0,2> JointDataRZ;
  typedef JointModelRevoluteTpl<double,0,2> JointModelRZ;
725

726
} //namespace pinocchio
727

728
729
730
731
732
#include <boost/type_traits.hpp>

namespace boost
{
  template<typename Scalar, int Options, int axis>
733
  struct has_nothrow_constructor< ::pinocchio::JointModelRevoluteTpl<Scalar,Options,axis> >
734
735
736
  : public integral_constant<bool,true> {};
  
  template<typename Scalar, int Options, int axis>
737
  struct has_nothrow_copy< ::pinocchio::JointModelRevoluteTpl<Scalar,Options,axis> >
738
739
740
  : public integral_constant<bool,true> {};
  
  template<typename Scalar, int Options, int axis>
741
  struct has_nothrow_constructor< ::pinocchio::JointDataRevoluteTpl<Scalar,Options,axis> >
742
743
744
  : public integral_constant<bool,true> {};
  
  template<typename Scalar, int Options, int axis>
745
  struct has_nothrow_copy< ::pinocchio::JointDataRevoluteTpl<Scalar,Options,axis> >
746
747
748
  : public integral_constant<bool,true> {};
}

749
#endif // ifndef __pinocchio_joint_revolute_hpp__