joint-prismatic.hpp 21.8 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_prismatic_hpp__
#define __pinocchio_joint_prismatic_hpp__
8

9
#include "pinocchio/macros.hpp"
10
11
12
#include "pinocchio/multibody/joint/joint-base.hpp"
#include "pinocchio/multibody/constraint.hpp"
#include "pinocchio/spatial/inertia.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 MotionPrismaticTpl;
20
  
21
22
  template<typename Scalar, int Options, int axis>
  struct SE3GroupAction< MotionPrismaticTpl<Scalar,Options,axis> >
23
  {
24
25
    typedef MotionTpl<Scalar,Options> ReturnType;
  };
26
  
27
28
29
30
31
32
  template<typename Scalar, int Options, int axis, typename MotionDerived>
  struct MotionAlgebraAction< MotionPrismaticTpl<Scalar,Options,axis>, MotionDerived>
  {
    typedef MotionTpl<Scalar,Options> ReturnType;
  };

33
  template<typename _Scalar, int _Options, int _axis>
34
  struct traits < MotionPrismaticTpl<_Scalar,_Options,_axis> >
35
  {
36
    typedef _Scalar Scalar;
37
38
39
40
    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
    enum {
      LINEAR = 0,
      ANGULAR = 3
53
    };
54
  }; // struct traits MotionPrismaticTpl
55

56
  template<typename _Scalar, int _Options, int _axis>
57
58
  struct MotionPrismaticTpl
  : MotionBase < MotionPrismaticTpl<_Scalar,_Options,_axis> >
59
  {
60
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
61
    MOTION_TYPEDEF_TPL(MotionPrismaticTpl);
62
63
64
    
    enum { axis = _axis };
    
65
    typedef SpatialAxis<_axis+LINEAR> Axis;
66
    typedef typename Axis::CartesianAxis3 CartesianAxis3;
67

68
    MotionPrismaticTpl() {}
69
    MotionPrismaticTpl(const Scalar & v) : m_v(v) {}
70

71
    inline PlainReturnType plain() const { return Axis() * m_v; }
72
73
74
75
    
    template<typename OtherScalar>
    MotionPrismaticTpl __mult__(const OtherScalar & alpha) const
    {
76
      return MotionPrismaticTpl(alpha*m_v);
77
    }
78
79
    
    template<typename Derived>
80
    void addTo(MotionDense<Derived> & other) const
81
    {
82
      typedef typename MotionDense<Derived>::Scalar OtherScalar;
83
      other.linear()[_axis] += (OtherScalar) m_v;
84
85
86
87
88
89
    }
    
    template<typename MotionDerived>
    void setTo(MotionDense<MotionDerived> & other) const
    {
      for(Eigen::DenseIndex k = 0; k < 3; ++k)
90
        other.linear()[k] = k == axis ? m_v : (Scalar)0;
91
      other.angular().setZero();
92
93
94
95
96
97
    }
    
    template<typename S2, int O2, typename D2>
    void se3Action_impl(const SE3Tpl<S2,O2> & m, MotionDense<D2> & v) const
    {
      v.angular().setZero();
98
      v.linear().noalias() = m_v * (m.rotation().col(axis));
99
100
101
102
103
104
105
106
107
108
109
110
111
112
    }
    
    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>
    void se3ActionInverse_impl(const SE3Tpl<S2,O2> & m, MotionDense<D2> & v) const
    {
      // Linear
113
      v.linear().noalias() = m_v * (m.rotation().transpose().col(axis));
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
      
      // Angular
      v.angular().setZero();
    }
    
    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>
    void motionAction(const MotionDense<M1> & v, MotionDense<M2> & mout) const
    {
      // Linear
131
      CartesianAxis3::alphaCross(-m_v,v.angular(),mout.linear());
132

133
134
      // Angular
      mout.angular().setZero();
135
    }
136
137
138
139
140
141
142
143
144
    
    template<typename M1>
    MotionPlain motionAction(const MotionDense<M1> & v) const
    {
      MotionPlain res;
      motionAction(v,res);
      return res;
    }
    
145
146
147
148
149
150
151
152
153
154
155
    Scalar & linearRate() { return m_v; }
    const Scalar & linearRate() const { return m_v; }
    
    bool isEqual_impl(const MotionPrismaticTpl & other) const
    {
      return m_v == other.m_v;
    }
    
  protected:
    
    Scalar m_v;
156
  }; // struct MotionPrismaticTpl
157

158
  template<typename Scalar, int Options, int axis, typename MotionDerived>
159
  typename MotionDerived::MotionPlain
160
  operator+(const MotionPrismaticTpl<Scalar,Options,axis> & m1,
161
            const MotionDense<MotionDerived> & m2)
162
  {
163
164
165
    typename MotionDerived::MotionPlain res(m2);
    res += m1;
    return res;
166
  }
167
  
168
169
170
171
172
173
174
175
  template<typename MotionDerived, typename S2, int O2, int axis>
  EIGEN_STRONG_INLINE
  typename MotionDerived::MotionPlain
  operator^(const MotionDense<MotionDerived> & m1, const MotionPrismaticTpl<S2,O2,axis> & m2)
  {
    return m2.motionAction(m1);
  }
  
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
  template<typename Scalar, int Options, int axis> struct TransformPrismaticTpl;
  
  template<typename _Scalar, int _Options, int _axis>
  struct traits< TransformPrismaticTpl<_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;
191
192
193
194
195
196
    typedef typename Matrix3::IdentityReturnType AngularType;
    typedef AngularType AngularRef;
    typedef AngularType ConstAngularRef;
    typedef Vector3 LinearType;
    typedef const Vector3 LinearRef;
    typedef const Vector3 ConstLinearRef;
197
198
199
200
    typedef typename traits<PlainType>::ActionMatrixType ActionMatrixType;
    typedef typename traits<PlainType>::HomogeneousMatrixType HomogeneousMatrixType;
  }; // traits TransformPrismaticTpl
  
201
202
203
204
  template<typename Scalar, int Options, int axis>
  struct SE3GroupAction< TransformPrismaticTpl<Scalar,Options,axis> >
  { typedef typename traits <TransformPrismaticTpl<Scalar,Options,axis> >::PlainType ReturnType; };

205
206
207
208
  template<typename _Scalar, int _Options, int axis>
  struct TransformPrismaticTpl : SE3Base< TransformPrismaticTpl<_Scalar,_Options,axis> >
  {
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
gabrielebndn's avatar
gabrielebndn committed
209
    PINOCCHIO_SE3_TYPEDEF_TPL(TransformPrismaticTpl);
210
211
212
213
214
215
216
217
218
219
220
221
    typedef typename traits<TransformPrismaticTpl>::PlainType PlainType;
    
    typedef SpatialAxis<axis+LINEAR> Axis;
    typedef typename Axis::CartesianAxis3 CartesianAxis3;
    
    TransformPrismaticTpl() {}
    TransformPrismaticTpl(const Scalar & displacement)
    : m_displacement(displacement)
    {}
    
    PlainType plain() const
    {
222
      PlainType res(PlainType::Identity());
223
224
225
226
227
228
229
230
231
      res.rotation().setIdentity();
      res.translation()[axis] = m_displacement;
      
      return res;
    }
    
    operator PlainType() const { return plain(); }
    
    template<typename S2, int O2>
232
    typename SE3GroupAction<TransformPrismaticTpl>::ReturnType
233
234
    se3action(const SE3Tpl<S2,O2> & m) const
    {
235
      typedef typename SE3GroupAction<TransformPrismaticTpl>::ReturnType ReturnType;
236
237
238
239
240
241
242
243
      ReturnType res(m);
      res.translation()[axis] += m_displacement;
      
      return res;
    }
    
    const Scalar & displacement() const { return m_displacement; }
    Scalar & displacement() { return m_displacement; }
244
245
246
    
    ConstLinearRef translation() const { return CartesianAxis3()*displacement(); };
    AngularType rotation() const { return AngularType(3,3); }
247
248
249
250
251
    
    bool isEqual(const TransformPrismaticTpl & other) const
    {
      return m_displacement == other.m_displacement;
    }
252
253
254
255
256

  protected:
    
    Scalar m_displacement;
  };
257

258
  template<typename Scalar, int Options, int axis> struct ConstraintPrismaticTpl;
259
260
  
  template<typename _Scalar, int _Options, int axis>
261
  struct traits< ConstraintPrismaticTpl<_Scalar,_Options,axis> >
262
  {
263
    typedef _Scalar Scalar;
264
    enum { Options = _Options };
265
266
267
268
    enum {
      LINEAR = 0,
      ANGULAR = 3
    };
269
    typedef MotionPrismaticTpl<Scalar,Options,axis> JointMotion;
270
271
    typedef Eigen::Matrix<Scalar,1,1,Options> JointForce;
    typedef Eigen::Matrix<Scalar,6,1,Options> DenseBase;
272
273
    typedef DenseBase MatrixReturnType;
    typedef const DenseBase ConstMatrixReturnType;
274
  }; // traits ConstraintRevolute
275
276
  
  template<typename Scalar, int Options, int axis>
Justin Carpentier's avatar
Justin Carpentier committed
277
  struct SE3GroupAction< ConstraintPrismaticTpl<Scalar,Options,axis> >
278
279
280
  { typedef Eigen::Matrix<Scalar,6,1,Options> ReturnType; };
  
  template<typename Scalar, int Options, int axis, typename MotionDerived>
Justin Carpentier's avatar
Justin Carpentier committed
281
  struct MotionAlgebraAction< ConstraintPrismaticTpl<Scalar,Options,axis>, MotionDerived >
282
283
284
  { typedef Eigen::Matrix<Scalar,6,1,Options> ReturnType; };

  template<typename Scalar, int Options, int axis, typename ForceDerived>
Justin Carpentier's avatar
Justin Carpentier committed
285
  struct ConstraintForceOp< ConstraintPrismaticTpl<Scalar,Options,axis>, ForceDerived>
286
  { typedef typename ForceDense<ForceDerived>::ConstLinearType::template ConstFixedSegmentReturnType<1>::Type ReturnType; };
287
288
  
  template<typename Scalar, int Options, int axis, typename ForceSet>
Justin Carpentier's avatar
Justin Carpentier committed
289
  struct ConstraintForceSetOp< ConstraintPrismaticTpl<Scalar,Options,axis>, ForceSet>
290
  { typedef typename Eigen::MatrixBase<ForceSet>::ConstRowXpr ReturnType; };
291

292
  template<typename _Scalar, int _Options, int axis>
293
294
  struct ConstraintPrismaticTpl
  : ConstraintBase < ConstraintPrismaticTpl <_Scalar,_Options,axis> >
295
  {
296
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
297
    PINOCCHIO_CONSTRAINT_TYPEDEF_TPL(ConstraintPrismaticTpl)
298
    enum { NV = 1 };
299
    
300
    typedef SpatialAxis<LINEAR+axis> Axis;
301
302
    
    ConstraintPrismaticTpl() {};
303

304
305
    template<typename Vector1Like>
    JointMotion __mult__(const Eigen::MatrixBase<Vector1Like> & v) const
306
    {
307
308
309
      EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Vector1Like,1);
      assert(v.size() == 1);
      return JointMotion(v[0]);
310
    }
311

312
    template<typename S2, int O2>
Justin Carpentier's avatar
Justin Carpentier committed
313
    typename SE3GroupAction<ConstraintPrismaticTpl>::ReturnType
314
    se3Action(const SE3Tpl<S2,O2> & m) const
315
    { 
Justin Carpentier's avatar
Justin Carpentier committed
316
      typename SE3GroupAction<ConstraintPrismaticTpl>::ReturnType res;
317
318
319
      MotionRef<DenseBase> v(res);
      v.linear() = m.rotation().col(axis);
      v.angular().setZero();
320
321
322
323
324
325
326
327
328
329
330
      return res;
    }
    
    template<typename S2, int O2>
    typename SE3GroupAction<ConstraintPrismaticTpl>::ReturnType
    se3ActionInverse(const SE3Tpl<S2,O2> & m) const
    {
      typename SE3GroupAction<ConstraintPrismaticTpl>::ReturnType res;
      MotionRef<DenseBase> v(res);
      v.linear() = m.rotation().transpose().col(axis);
      v.angular().setZero();
331
      return res;
332
333
    }

334
335
    int nv_impl() const { return NV; }

336
337
    struct TransposeConst
    {
338
339
      const ConstraintPrismaticTpl & ref; 
      TransposeConst(const ConstraintPrismaticTpl & ref) : ref(ref) {}
340

341
      template<typename ForceDerived>
Justin Carpentier's avatar
Justin Carpentier committed
342
      typename ConstraintForceOp<ConstraintPrismaticTpl,ForceDerived>::ReturnType
343
      operator* (const ForceDense<ForceDerived> & f) const
344
345
346
      { return f.linear().template segment<1>(axis); }

      /* [CRBA]  MatrixBase operator* (Constraint::Transpose S, ForceSet::Block) */
347
      template<typename Derived>
Justin Carpentier's avatar
Justin Carpentier committed
348
      typename ConstraintForceSetOp<ConstraintPrismaticTpl,Derived>::ReturnType
349
      operator*(const Eigen::MatrixBase<Derived> & F )
350
351
      {
        assert(F.rows()==6);
352
        return F.row(LINEAR+axis);
353
354
355
356
      }

    }; // struct TransposeConst
    TransposeConst transpose() const { return TransposeConst(*this); }
357
358
359
360
361
362
363

    /* CRBA joint operators
     *   - ForceSet::Block = ForceSet
     *   - ForceSet operator* (Inertia Y,Constraint S)
     *   - MatrixBase operator* (Constraint::Transpose S, ForceSet::Block)
     *   - SE3::act(ForceSet::Block)
     */
364
    DenseBase matrix_impl() const
365
    {
366
      DenseBase S;
367
368
      MotionRef<DenseBase> v(S);
      v << Axis();
369
      return S;
370
    }
371
    
372
    template<typename MotionDerived>
Justin Carpentier's avatar
Justin Carpentier committed
373
    typename MotionAlgebraAction<ConstraintPrismaticTpl,MotionDerived>::ReturnType
374
    motionAction(const MotionDense<MotionDerived> & m) const
375
    {
Justin Carpentier's avatar
Justin Carpentier committed
376
      typename MotionAlgebraAction<ConstraintPrismaticTpl,MotionDerived>::ReturnType res;
377
378
      MotionRef<DenseBase> v(res);
      v = m.cross(Axis());
379
380
      return res;
    }
381
382
    
    bool isEqual(const ConstraintPrismaticTpl &) const { return true; }
383

384
  }; // struct ConstraintPrismaticTpl
385
386
387
388
389
390
391
  
  template<typename S1, int O1,typename S2, int O2, int axis>
  struct MultiplicationOp<InertiaTpl<S1,O1>, ConstraintPrismaticTpl<S2,O2,axis> >
  {
    typedef Eigen::Matrix<S2,6,1,O2> ReturnType;
  };
  
392
  /* [CRBA] ForceSet operator* (Inertia Y,Constraint S) */
393
394
395
396
397
398
399
400
401
402
403
404
405
406
407
408
409
410
411
412
413
414
415
416
417
418
419
420
421
422
423
424
425
426
427
428
429
430
431
432
433
434
435
436
437
438
439
440
441
442
443
444
445
446
447
448
449
450
451
452
453
454
455
456
457
458
459
460
461
462
463
464
465
466
467
468
  namespace impl
  {
    template<typename S1, int O1, typename S2, int O2>
    struct LhsMultiplicationOp<InertiaTpl<S1,O1>, ConstraintPrismaticTpl<S2,O2,0> >
    {
      typedef InertiaTpl<S1,O1> Inertia;
      typedef ConstraintPrismaticTpl<S2,O2,0> Constraint;
      typedef typename MultiplicationOp<Inertia,Constraint>::ReturnType ReturnType;
      static inline ReturnType run(const Inertia & Y,
                                   const Constraint & /*constraint*/)
      {
        ReturnType res;
        
        /* Y(:,0) = ( 1,0, 0, 0 , z , -y ) */
        const S1
        &m = Y.mass(),
        &y = Y.lever()[1],
        &z = Y.lever()[2];
        res << m, S1(0), S1(0), S1(0), m*z, -m*y;
        
        return res;
      }
    };
    
    template<typename S1, int O1, typename S2, int O2>
    struct LhsMultiplicationOp<InertiaTpl<S1,O1>, ConstraintPrismaticTpl<S2,O2,1> >
    {
      typedef InertiaTpl<S1,O1> Inertia;
      typedef ConstraintPrismaticTpl<S2,O2,1> Constraint;
      typedef typename MultiplicationOp<Inertia,Constraint>::ReturnType ReturnType;
      static inline ReturnType run(const Inertia & Y,
                                   const Constraint & /*constraint*/)
      {
        ReturnType res;
        
        /* Y(:,1) = ( 0,1, 0, -z , 0 , x) */
        const S1
        &m = Y.mass(),
        &x = Y.lever()[0],
        &z = Y.lever()[2];
        
        res << S1(0), m, S1(0), -m*z, S1(0), m*x;
        
        return res;
      }
    };
    
    template<typename S1, int O1, typename S2, int O2>
    struct LhsMultiplicationOp<InertiaTpl<S1,O1>, ConstraintPrismaticTpl<S2,O2,2> >
    {
      typedef InertiaTpl<S1,O1> Inertia;
      typedef ConstraintPrismaticTpl<S2,O2,2> Constraint;
      typedef typename MultiplicationOp<Inertia,Constraint>::ReturnType ReturnType;
      static inline ReturnType run(const Inertia & Y,
                                   const Constraint & /*constraint*/)
      {
        ReturnType res;
        
        /* Y(:,2) = ( 0,0, 1, y , -x , 0) */
        const S1
        &m = Y.mass(),
        &x = Y.lever()[0],
        &y = Y.lever()[1];
        
        res << S1(0), S1(0), m, m*y, -m*x, S1(0);
        
        return res;
      }
    };
  } // namespace impl
  
  template<typename M6Like,typename S2, int O2, int axis>
  struct MultiplicationOp<Eigen::MatrixBase<M6Like>, ConstraintPrismaticTpl<S2,O2,axis> >
  {
    typedef typename M6Like::ConstColXpr ReturnType;
  };
469
470
  
  /* [ABA] operator* (Inertia Y,Constraint S) */
471
  namespace impl
472
  {
473
474
475
476
477
478
479
480
481
482
483
484
485
486
    template<typename M6Like, typename Scalar, int Options, int axis>
    struct LhsMultiplicationOp<Eigen::MatrixBase<M6Like>, ConstraintPrismaticTpl<Scalar,Options,axis> >
    {
      typedef ConstraintPrismaticTpl<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.derived().col(Inertia::LINEAR + axis);
      }
    };
  } // namespace impl
  
487
488
489
490
491
492
493
494
495
496
497
  template<typename _Scalar, int _Options, int _axis>
  struct JointPrismaticTpl
  {
    typedef _Scalar Scalar;
    
    enum
    {
      Options = _Options,
      axis = _axis
    };
  };
498

499
  template<typename _Scalar, int _Options, int axis>
500
  struct traits< JointPrismaticTpl<_Scalar,_Options,axis> >
501
  {
502
503
504
505
    enum {
      NQ = 1,
      NV = 1
    };
506
    typedef _Scalar Scalar;
507
    enum { Options = _Options };
508
509
    typedef JointDataPrismaticTpl<Scalar,Options,axis> JointDataDerived;
    typedef JointModelPrismaticTpl<Scalar,Options,axis> JointModelDerived;
510
    typedef ConstraintPrismaticTpl<Scalar,Options,axis> Constraint_t;
511
    typedef TransformPrismaticTpl<Scalar,Options,axis> Transformation_t;
512
    typedef MotionPrismaticTpl<Scalar,Options,axis> Motion_t;
513
    typedef MotionZeroTpl<Scalar,Options> Bias_t;
Justin Carpentier's avatar
Justin Carpentier committed
514

515
    // [ABA]
516
517
518
    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;
519
    
Gabriele Buondonno's avatar
Gabriele Buondonno committed
520
    PINOCCHIO_JOINT_DATA_BASE_ACCESSOR_DEFAULT_RETURN_TYPE
521

522
523
    typedef Eigen::Matrix<Scalar,NQ,1,Options> ConfigVector_t;
    typedef Eigen::Matrix<Scalar,NV,1,Options> TangentVector_t;
524
525
  };

526
  template<typename Scalar, int Options, int axis>
527
  struct traits< JointDataPrismaticTpl<Scalar,Options,axis> >
528
  { typedef JointPrismaticTpl<Scalar,Options,axis> JointDerived; };
529
530
  
  template<typename Scalar, int Options, int axis>
531
  struct traits< JointModelPrismaticTpl<Scalar,Options,axis> >
532
  { typedef JointPrismaticTpl<Scalar,Options,axis> JointDerived; };
533

534
  template<typename _Scalar, int _Options, int axis>
535
  struct JointDataPrismaticTpl : public JointDataBase< JointDataPrismaticTpl<_Scalar,_Options,axis> >
536
  {
537
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
538
    typedef JointPrismaticTpl<_Scalar,_Options,axis> JointDerived;
539
    PINOCCHIO_JOINT_DATA_TYPEDEF_TEMPLATE(JointDerived);
Gabriele Buondonno's avatar
Gabriele Buondonno committed
540
    PINOCCHIO_JOINT_DATA_BASE_DEFAULT_ACCESSOR
541
542
543
544
545
546

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

547
548
549
550
    // [ABA] specific data
    U_t U;
    D_t Dinv;
    UD_t UDinv;
551

552
    JointDataPrismaticTpl()
553
554
555
    : M((Scalar)0)
    , v((Scalar)0)
    , U(U_t::Zero())
556
557
558
    , Dinv(D_t::Zero())
    , UDinv(UD_t::Zero())
    {}
559

560
561
562
563
    static std::string classname()
    {
      return std::string("JointDataP") + axisLabel<axis>();
    }
564
565
    std::string shortname() const { return classname(); }

566
  }; // struct JointDataPrismaticTpl
567
568
569
570
571
572
  
  template<typename NewScalar, typename Scalar, int Options, int axis>
  struct CastType< NewScalar, JointModelPrismaticTpl<Scalar,Options,axis> >
  {
    typedef JointModelPrismaticTpl<NewScalar,Options,axis> type;
  };
573

574
  template<typename _Scalar, int _Options, int axis>
jcarpent's avatar
jcarpent committed
575
576
  struct JointModelPrismaticTpl
  : public JointModelBase< JointModelPrismaticTpl<_Scalar,_Options,axis> >
577
  {
578
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
579
    typedef JointPrismaticTpl<_Scalar,_Options,axis> JointDerived;
580
    PINOCCHIO_JOINT_TYPEDEF_TEMPLATE(JointDerived);
jcarpent's avatar
jcarpent committed
581
582
583
584
585
586
    
    typedef JointModelBase<JointModelPrismaticTpl> Base;
    using Base::id;
    using Base::idx_q;
    using Base::idx_v;
    using Base::setIndexes;
587
    
588
    JointDataDerived createData() const { return JointDataDerived(); }
589
590
591
592
    
    template<typename ConfigVector>
    void calc(JointDataDerived & data,
              const typename Eigen::MatrixBase<ConfigVector> & qs) const
593
    {
594
595
      typedef typename ConfigVector::Scalar Scalar;
      const Scalar & q = qs[idx_q()];
596
      data.M.displacement() = q;
597
598
    }

599
600
601
602
    template<typename ConfigVector, typename TangentVector>
    void calc(JointDataDerived & data,
              const typename Eigen::MatrixBase<ConfigVector> & qs,
              const typename Eigen::MatrixBase<TangentVector> & vs) const
603
    {
604
605
606
607
      calc(data,qs.derived());
      
      typedef typename TangentVector::Scalar S2;
      const S2 & v = vs[idx_v()];
608
      data.v.linearRate() = v;
609
    }
610
    
611
612
    template<typename Matrix6Like>
    void calc_aba(JointDataDerived & data, const Eigen::MatrixBase<Matrix6Like> & I, const bool update_I) const
613
614
615
    {
      data.U = I.col(Inertia::LINEAR + axis);
      data.Dinv[0] = 1./I(Inertia::LINEAR + axis, Inertia::LINEAR + axis);
616
      data.UDinv.noalias() = data.U * data.Dinv[0];
617
618
      
      if (update_I)
gabrielebndn's avatar
gabrielebndn committed
619
        PINOCCHIO_EIGEN_CONST_CAST(Matrix6Like,I) -= data.UDinv * data.U.transpose();
620
    }
jcarpent's avatar
jcarpent committed
621
    
622
623
624
625
    static std::string classname()
    {
      return std::string("JointModelP") + axisLabel<axis>();
    }
626
    std::string shortname() const { return classname(); }
627
628
629
630
631
632
633
634
635
636
    
    /// \returns An expression of *this with the Scalar type casted to NewScalar.
    template<typename NewScalar>
    JointModelPrismaticTpl<NewScalar,Options,axis> cast() const
    {
      typedef JointModelPrismaticTpl<NewScalar,Options,axis> ReturnType;
      ReturnType res;
      res.setIndexes(id(),idx_q(),idx_v());
      return res;
    }
637

638
  }; // struct JointModelPrismaticTpl
639

640
  typedef JointPrismaticTpl<double,0,0> JointPX;
641
642
  typedef JointDataPrismaticTpl<double,0,0> JointDataPX;
  typedef JointModelPrismaticTpl<double,0,0> JointModelPX;
643

644
  typedef JointPrismaticTpl<double,0,1> JointPY;
645
646
  typedef JointDataPrismaticTpl<double,0,1> JointDataPY;
  typedef JointModelPrismaticTpl<double,0,1> JointModelPY;
647

648
  typedef JointPrismaticTpl<double,0,2> JointPZ;
649
650
  typedef JointDataPrismaticTpl<double,0,2> JointDataPZ;
  typedef JointModelPrismaticTpl<double,0,2> JointModelPZ;
651

652
} //namespace pinocchio
653

654
655
656
657
658
#include <boost/type_traits.hpp>

namespace boost
{
  template<typename Scalar, int Options, int axis>
659
  struct has_nothrow_constructor< ::pinocchio::JointModelPrismaticTpl<Scalar,Options,axis> >
660
661
662
  : public integral_constant<bool,true> {};
  
  template<typename Scalar, int Options, int axis>
663
  struct has_nothrow_copy< ::pinocchio::JointModelPrismaticTpl<Scalar,Options,axis> >
664
665
666
  : public integral_constant<bool,true> {};
  
  template<typename Scalar, int Options, int axis>
667
  struct has_nothrow_constructor< ::pinocchio::JointDataPrismaticTpl<Scalar,Options,axis> >
668
669
670
  : public integral_constant<bool,true> {};
  
  template<typename Scalar, int Options, int axis>
671
  struct has_nothrow_copy< ::pinocchio::JointDataPrismaticTpl<Scalar,Options,axis> >
672
673
674
  : public integral_constant<bool,true> {};
}

675
#endif // ifndef __pinocchio_joint_prismatic_hpp__