joint-prismatic.hpp 21.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_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
69
    MotionPrismaticTpl() {}
    MotionPrismaticTpl(const Scalar & v) : rate(v) {}
70

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

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
    //data
146
    Scalar rate;
147
  }; // struct MotionPrismaticTpl
148

149
  template<typename Scalar, int Options, int axis, typename MotionDerived>
150
  typename MotionDerived::MotionPlain
151
  operator+(const MotionPrismaticTpl<Scalar,Options,axis> & m1,
152
            const MotionDense<MotionDerived> & m2)
153
  {
154
155
156
    typename MotionDerived::MotionPlain res(m2);
    res += m1;
    return res;
157
  }
158
  
159
160
161
162
163
164
165
166
  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);
  }
  
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
  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;
182
183
184
185
186
187
    typedef typename Matrix3::IdentityReturnType AngularType;
    typedef AngularType AngularRef;
    typedef AngularType ConstAngularRef;
    typedef Vector3 LinearType;
    typedef const Vector3 LinearRef;
    typedef const Vector3 ConstLinearRef;
188
189
190
191
    typedef typename traits<PlainType>::ActionMatrixType ActionMatrixType;
    typedef typename traits<PlainType>::HomogeneousMatrixType HomogeneousMatrixType;
  }; // traits TransformPrismaticTpl
  
192
193
194
195
  template<typename Scalar, int Options, int axis>
  struct SE3GroupAction< TransformPrismaticTpl<Scalar,Options,axis> >
  { typedef typename traits <TransformPrismaticTpl<Scalar,Options,axis> >::PlainType ReturnType; };

196
197
198
199
  template<typename _Scalar, int _Options, int axis>
  struct TransformPrismaticTpl : SE3Base< TransformPrismaticTpl<_Scalar,_Options,axis> >
  {
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
gabrielebndn's avatar
gabrielebndn committed
200
    PINOCCHIO_SE3_TYPEDEF_TPL(TransformPrismaticTpl);
201
202
203
204
205
206
207
208
209
210
211
212
    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
    {
213
      PlainType res(PlainType::Identity());
214
215
216
217
218
219
220
221
222
      res.rotation().setIdentity();
      res.translation()[axis] = m_displacement;
      
      return res;
    }
    
    operator PlainType() const { return plain(); }
    
    template<typename S2, int O2>
223
    typename SE3GroupAction<TransformPrismaticTpl>::ReturnType
224
225
    se3action(const SE3Tpl<S2,O2> & m) const
    {
226
      typedef typename SE3GroupAction<TransformPrismaticTpl>::ReturnType ReturnType;
227
228
229
230
231
232
233
234
      ReturnType res(m);
      res.translation()[axis] += m_displacement;
      
      return res;
    }
    
    const Scalar & displacement() const { return m_displacement; }
    Scalar & displacement() { return m_displacement; }
235
236
237
    
    ConstLinearRef translation() const { return CartesianAxis3()*displacement(); };
    AngularType rotation() const { return AngularType(3,3); }
238
239
240
241
242

  protected:
    
    Scalar m_displacement;
  };
243

244
  template<typename Scalar, int Options, int axis> struct ConstraintPrismaticTpl;
245
246
  
  template<typename _Scalar, int _Options, int axis>
247
  struct traits< ConstraintPrismaticTpl<_Scalar,_Options,axis> >
248
  {
249
    typedef _Scalar Scalar;
250
    enum { Options = _Options };
251
252
253
254
    enum {
      LINEAR = 0,
      ANGULAR = 3
    };
255
    typedef MotionPrismaticTpl<Scalar,Options,axis> JointMotion;
256
257
    typedef Eigen::Matrix<Scalar,1,1,Options> JointForce;
    typedef Eigen::Matrix<Scalar,6,1,Options> DenseBase;
258
259
    typedef DenseBase MatrixReturnType;
    typedef const DenseBase ConstMatrixReturnType;
260
  }; // traits ConstraintRevolute
261
262
  
  template<typename Scalar, int Options, int axis>
Justin Carpentier's avatar
Justin Carpentier committed
263
  struct SE3GroupAction< ConstraintPrismaticTpl<Scalar,Options,axis> >
264
265
266
  { typedef Eigen::Matrix<Scalar,6,1,Options> ReturnType; };
  
  template<typename Scalar, int Options, int axis, typename MotionDerived>
Justin Carpentier's avatar
Justin Carpentier committed
267
  struct MotionAlgebraAction< ConstraintPrismaticTpl<Scalar,Options,axis>, MotionDerived >
268
269
270
  { typedef Eigen::Matrix<Scalar,6,1,Options> ReturnType; };

  template<typename Scalar, int Options, int axis, typename ForceDerived>
Justin Carpentier's avatar
Justin Carpentier committed
271
  struct ConstraintForceOp< ConstraintPrismaticTpl<Scalar,Options,axis>, ForceDerived>
272
  { typedef typename ForceDense<ForceDerived>::ConstLinearType::template ConstFixedSegmentReturnType<1>::Type ReturnType; };
273
274
  
  template<typename Scalar, int Options, int axis, typename ForceSet>
Justin Carpentier's avatar
Justin Carpentier committed
275
  struct ConstraintForceSetOp< ConstraintPrismaticTpl<Scalar,Options,axis>, ForceSet>
276
  { typedef typename Eigen::MatrixBase<ForceSet>::ConstRowXpr ReturnType; };
277

278
  template<typename _Scalar, int _Options, int axis>
279
280
  struct ConstraintPrismaticTpl
  : ConstraintBase < ConstraintPrismaticTpl <_Scalar,_Options,axis> >
281
  {
282
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
283
    PINOCCHIO_CONSTRAINT_TYPEDEF_TPL(ConstraintPrismaticTpl)
284
    enum { NV = 1 };
285
    
286
    typedef SpatialAxis<LINEAR+axis> Axis;
287
288
    
    ConstraintPrismaticTpl() {};
289

290
291
    template<typename Vector1Like>
    JointMotion __mult__(const Eigen::MatrixBase<Vector1Like> & v) const
292
    {
293
294
295
      EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Vector1Like,1);
      assert(v.size() == 1);
      return JointMotion(v[0]);
296
    }
297

298
    template<typename S2, int O2>
Justin Carpentier's avatar
Justin Carpentier committed
299
    typename SE3GroupAction<ConstraintPrismaticTpl>::ReturnType
300
    se3Action(const SE3Tpl<S2,O2> & m) const
301
    { 
Justin Carpentier's avatar
Justin Carpentier committed
302
      typename SE3GroupAction<ConstraintPrismaticTpl>::ReturnType res;
303
304
305
      MotionRef<DenseBase> v(res);
      v.linear() = m.rotation().col(axis);
      v.angular().setZero();
306
307
308
309
310
311
312
313
314
315
316
      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();
317
      return res;
318
319
    }

320
321
    int nv_impl() const { return NV; }

322
323
    struct TransposeConst
    {
324
325
      const ConstraintPrismaticTpl & ref; 
      TransposeConst(const ConstraintPrismaticTpl & ref) : ref(ref) {}
326

327
      template<typename ForceDerived>
Justin Carpentier's avatar
Justin Carpentier committed
328
      typename ConstraintForceOp<ConstraintPrismaticTpl,ForceDerived>::ReturnType
329
      operator* (const ForceDense<ForceDerived> & f) const
330
331
332
      { return f.linear().template segment<1>(axis); }

      /* [CRBA]  MatrixBase operator* (Constraint::Transpose S, ForceSet::Block) */
333
      template<typename Derived>
Justin Carpentier's avatar
Justin Carpentier committed
334
      typename ConstraintForceSetOp<ConstraintPrismaticTpl,Derived>::ReturnType
335
      operator*(const Eigen::MatrixBase<Derived> & F )
336
337
      {
        assert(F.rows()==6);
338
        return F.row(LINEAR+axis);
339
340
341
342
      }

    }; // struct TransposeConst
    TransposeConst transpose() const { return TransposeConst(*this); }
343
344
345
346
347
348
349

    /* CRBA joint operators
     *   - ForceSet::Block = ForceSet
     *   - ForceSet operator* (Inertia Y,Constraint S)
     *   - MatrixBase operator* (Constraint::Transpose S, ForceSet::Block)
     *   - SE3::act(ForceSet::Block)
     */
350
    DenseBase matrix_impl() const
351
    {
352
      DenseBase S;
353
354
      MotionRef<DenseBase> v(S);
      v << Axis();
355
      return S;
356
    }
357
    
358
    template<typename MotionDerived>
Justin Carpentier's avatar
Justin Carpentier committed
359
    typename MotionAlgebraAction<ConstraintPrismaticTpl,MotionDerived>::ReturnType
360
    motionAction(const MotionDense<MotionDerived> & m) const
361
    {
Justin Carpentier's avatar
Justin Carpentier committed
362
      typename MotionAlgebraAction<ConstraintPrismaticTpl,MotionDerived>::ReturnType res;
363
364
      MotionRef<DenseBase> v(res);
      v = m.cross(Axis());
365
366
      return res;
    }
367

368
  }; // struct ConstraintPrismaticTpl
369
370
371
372
373
374
375
  
  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;
  };
  
376
  /* [CRBA] ForceSet operator* (Inertia Y,Constraint S) */
377
378
379
380
381
382
383
384
385
386
387
388
389
390
391
392
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
  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;
  };
453
454
  
  /* [ABA] operator* (Inertia Y,Constraint S) */
455
  namespace impl
456
  {
457
458
459
460
461
462
463
464
465
466
467
468
469
470
    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
  
471
472
473
474
475
476
477
478
479
480
481
  template<typename _Scalar, int _Options, int _axis>
  struct JointPrismaticTpl
  {
    typedef _Scalar Scalar;
    
    enum
    {
      Options = _Options,
      axis = _axis
    };
  };
482

483
  template<typename _Scalar, int _Options, int axis>
484
  struct traits< JointPrismaticTpl<_Scalar,_Options,axis> >
485
  {
486
487
488
489
    enum {
      NQ = 1,
      NV = 1
    };
490
    typedef _Scalar Scalar;
491
    enum { Options = _Options };
492
493
    typedef JointDataPrismaticTpl<Scalar,Options,axis> JointDataDerived;
    typedef JointModelPrismaticTpl<Scalar,Options,axis> JointModelDerived;
494
    typedef ConstraintPrismaticTpl<Scalar,Options,axis> Constraint_t;
495
    typedef TransformPrismaticTpl<Scalar,Options,axis> Transformation_t;
496
    typedef MotionPrismaticTpl<Scalar,Options,axis> Motion_t;
497
    typedef MotionZeroTpl<Scalar,Options> Bias_t;
Justin Carpentier's avatar
Justin Carpentier committed
498

499
    // [ABA]
500
501
502
    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;
503
    
Gabriele Buondonno's avatar
Gabriele Buondonno committed
504
    PINOCCHIO_JOINT_DATA_BASE_ACCESSOR_DEFAULT_RETURN_TYPE
505

506
507
    typedef Eigen::Matrix<Scalar,NQ,1,Options> ConfigVector_t;
    typedef Eigen::Matrix<Scalar,NV,1,Options> TangentVector_t;
508
509
  };

510
  template<typename Scalar, int Options, int axis>
511
  struct traits< JointDataPrismaticTpl<Scalar,Options,axis> >
512
  { typedef JointPrismaticTpl<Scalar,Options,axis> JointDerived; };
513
514
  
  template<typename Scalar, int Options, int axis>
515
  struct traits< JointModelPrismaticTpl<Scalar,Options,axis> >
516
  { typedef JointPrismaticTpl<Scalar,Options,axis> JointDerived; };
517

518
  template<typename _Scalar, int _Options, int axis>
519
  struct JointDataPrismaticTpl : public JointDataBase< JointDataPrismaticTpl<_Scalar,_Options,axis> >
520
  {
521
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
522
    typedef JointPrismaticTpl<_Scalar,_Options,axis> JointDerived;
523
    PINOCCHIO_JOINT_DATA_TYPEDEF_TEMPLATE(JointDerived);
Gabriele Buondonno's avatar
Gabriele Buondonno committed
524
    PINOCCHIO_JOINT_DATA_BASE_DEFAULT_ACCESSOR
525
526
527
528
529
530

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

531
532
533
534
    // [ABA] specific data
    U_t U;
    D_t Dinv;
    UD_t UDinv;
535

536
    JointDataPrismaticTpl() {}
537

538
539
540
    static std::string classname() { return std::string("JointDataPrismatic"); }
    std::string shortname() const { return classname(); }

541
  }; // struct JointDataPrismaticTpl
542
543
544
545
546
547
  
  template<typename NewScalar, typename Scalar, int Options, int axis>
  struct CastType< NewScalar, JointModelPrismaticTpl<Scalar,Options,axis> >
  {
    typedef JointModelPrismaticTpl<NewScalar,Options,axis> type;
  };
548

549
  template<typename _Scalar, int _Options, int axis>
jcarpent's avatar
jcarpent committed
550
551
  struct JointModelPrismaticTpl
  : public JointModelBase< JointModelPrismaticTpl<_Scalar,_Options,axis> >
552
  {
553
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
554
    typedef JointPrismaticTpl<_Scalar,_Options,axis> JointDerived;
555
    PINOCCHIO_JOINT_TYPEDEF_TEMPLATE(JointDerived);
jcarpent's avatar
jcarpent committed
556
557
558
559
560
561
    
    typedef JointModelBase<JointModelPrismaticTpl> Base;
    using Base::id;
    using Base::idx_q;
    using Base::idx_v;
    using Base::setIndexes;
562
    
563
    JointDataDerived createData() const { return JointDataDerived(); }
564
565
566
567
    
    template<typename ConfigVector>
    void calc(JointDataDerived & data,
              const typename Eigen::MatrixBase<ConfigVector> & qs) const
568
    {
569
570
      typedef typename ConfigVector::Scalar Scalar;
      const Scalar & q = qs[idx_q()];
571
      data.M.displacement() = q;
572
573
    }

574
575
576
577
    template<typename ConfigVector, typename TangentVector>
    void calc(JointDataDerived & data,
              const typename Eigen::MatrixBase<ConfigVector> & qs,
              const typename Eigen::MatrixBase<TangentVector> & vs) const
578
    {
579
580
581
582
      calc(data,qs.derived());
      
      typedef typename TangentVector::Scalar S2;
      const S2 & v = vs[idx_v()];
583
      data.v.rate = v;
584
    }
585
    
586
587
    template<typename Matrix6Like>
    void calc_aba(JointDataDerived & data, const Eigen::MatrixBase<Matrix6Like> & I, const bool update_I) const
588
589
590
    {
      data.U = I.col(Inertia::LINEAR + axis);
      data.Dinv[0] = 1./I(Inertia::LINEAR + axis, Inertia::LINEAR + axis);
591
      data.UDinv.noalias() = data.U * data.Dinv[0];
592
593
      
      if (update_I)
gabrielebndn's avatar
gabrielebndn committed
594
        PINOCCHIO_EIGEN_CONST_CAST(Matrix6Like,I) -= data.UDinv * data.U.transpose();
595
    }
jcarpent's avatar
jcarpent committed
596
    
597
598
599
600
    static std::string classname()
    {
      return std::string("JointModelP") + axisLabel<axis>();
    }
601
    std::string shortname() const { return classname(); }
602
603
604
605
606
607
608
609
610
611
    
    /// \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;
    }
612

613
  }; // struct JointModelPrismaticTpl
614

615
  typedef JointPrismaticTpl<double,0,0> JointPX;
616
617
  typedef JointDataPrismaticTpl<double,0,0> JointDataPX;
  typedef JointModelPrismaticTpl<double,0,0> JointModelPX;
618

619
  typedef JointPrismaticTpl<double,0,1> JointPY;
620
621
  typedef JointDataPrismaticTpl<double,0,1> JointDataPY;
  typedef JointModelPrismaticTpl<double,0,1> JointModelPY;
622

623
  typedef JointPrismaticTpl<double,0,2> JointPZ;
624
625
  typedef JointDataPrismaticTpl<double,0,2> JointDataPZ;
  typedef JointModelPrismaticTpl<double,0,2> JointModelPZ;
626

627
} //namespace pinocchio
628

629
630
631
632
633
#include <boost/type_traits.hpp>

namespace boost
{
  template<typename Scalar, int Options, int axis>
634
  struct has_nothrow_constructor< ::pinocchio::JointModelPrismaticTpl<Scalar,Options,axis> >
635
636
637
  : public integral_constant<bool,true> {};
  
  template<typename Scalar, int Options, int axis>
638
  struct has_nothrow_copy< ::pinocchio::JointModelPrismaticTpl<Scalar,Options,axis> >
639
640
641
  : public integral_constant<bool,true> {};
  
  template<typename Scalar, int Options, int axis>
642
  struct has_nothrow_constructor< ::pinocchio::JointDataPrismaticTpl<Scalar,Options,axis> >
643
644
645
  : public integral_constant<bool,true> {};
  
  template<typename Scalar, int Options, int axis>
646
  struct has_nothrow_copy< ::pinocchio::JointDataPrismaticTpl<Scalar,Options,axis> >
647
648
649
  : public integral_constant<bool,true> {};
}

650
#endif // ifndef __pinocchio_joint_prismatic_hpp__