GCC Code Coverage Report
Directory: ./ Exec Total Coverage
File: include/pinocchio/multibody/joint/joint-prismatic.hpp Lines: 152 159 95.6 %
Date: 2024-01-23 21:41:47 Branches: 84 162 51.9 %

Line Branch Exec Source
1
//
2
// Copyright (c) 2015-2019 CNRS INRIA
3
// Copyright (c) 2015-2016 Wandercraft, 86 rue de Paris 91400 Orsay, France.
4
//
5
6
#ifndef __pinocchio_joint_prismatic_hpp__
7
#define __pinocchio_joint_prismatic_hpp__
8
9
#include "pinocchio/macros.hpp"
10
#include "pinocchio/multibody/joint/joint-base.hpp"
11
#include "pinocchio/multibody/constraint.hpp"
12
#include "pinocchio/spatial/inertia.hpp"
13
#include "pinocchio/spatial/spatial-axis.hpp"
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
  template<typename Scalar, int Options, int axis>
22
  struct SE3GroupAction< MotionPrismaticTpl<Scalar,Options,axis> >
23
  {
24
    typedef MotionTpl<Scalar,Options> ReturnType;
25
  };
26
27
  template<typename Scalar, int Options, int axis, typename MotionDerived>
28
  struct MotionAlgebraAction< MotionPrismaticTpl<Scalar,Options,axis>, MotionDerived>
29
  {
30
    typedef MotionTpl<Scalar,Options> ReturnType;
31
  };
32
33
  template<typename _Scalar, int _Options, int _axis>
34
  struct traits < MotionPrismaticTpl<_Scalar,_Options,_axis> >
35
  {
36
    typedef _Scalar Scalar;
37
    enum { Options = _Options };
38
    typedef Eigen::Matrix<Scalar,3,1,Options> Vector3;
39
    typedef Eigen::Matrix<Scalar,6,1,Options> Vector6;
40
    typedef Eigen::Matrix<Scalar,4,4,Options> Matrix4;
41
    typedef Eigen::Matrix<Scalar,6,6,Options> Matrix6;
42
    typedef typename PINOCCHIO_EIGEN_REF_CONST_TYPE(Vector6) ToVectorConstReturnType;
43
    typedef typename PINOCCHIO_EIGEN_REF_TYPE(Vector6) ToVectorReturnType;
44
    typedef Vector3 AngularType;
45
    typedef Vector3 LinearType;
46
    typedef const Vector3 ConstAngularType;
47
    typedef const Vector3 ConstLinearType;
48
    typedef Matrix6 ActionMatrixType;
49
    typedef Matrix4 HomogeneousMatrixType;
50
    typedef MotionTpl<Scalar,Options> MotionPlain;
51
    typedef MotionPlain PlainReturnType;
52
    enum {
53
      LINEAR = 0,
54
      ANGULAR = 3
55
    };
56
  }; // struct traits MotionPrismaticTpl
57
58
  template<typename _Scalar, int _Options, int _axis>
59
  struct MotionPrismaticTpl
60
  : MotionBase < MotionPrismaticTpl<_Scalar,_Options,_axis> >
61
  {
62
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
63
    MOTION_TYPEDEF_TPL(MotionPrismaticTpl);
64
65
    enum { axis = _axis };
66
67
    typedef SpatialAxis<_axis+LINEAR> Axis;
68
    typedef typename Axis::CartesianAxis3 CartesianAxis3;
69
70
42
    MotionPrismaticTpl() {}
71
18778
    MotionPrismaticTpl(const Scalar & v) : m_v(v) {}
72
73
12
    inline PlainReturnType plain() const { return Axis() * m_v; }
74
75
    template<typename OtherScalar>
76
6
    MotionPrismaticTpl __mult__(const OtherScalar & alpha) const
77
    {
78
6
      return MotionPrismaticTpl(alpha*m_v);
79
    }
80
81
    template<typename Derived>
82
10
    void addTo(MotionDense<Derived> & other) const
83
    {
84
      typedef typename MotionDense<Derived>::Scalar OtherScalar;
85
10
      other.linear()[_axis] += (OtherScalar) m_v;
86
    }
87
88
    template<typename MotionDerived>
89
6208
    void setTo(MotionDense<MotionDerived> & other) const
90
    {
91
24832
      for(Eigen::DenseIndex k = 0; k < 3; ++k)
92

18624
        other.linear()[k] = k == axis ? m_v : (Scalar)0;
93
6208
      other.angular().setZero();
94
6208
    }
95
96
    template<typename S2, int O2, typename D2>
97
6
    void se3Action_impl(const SE3Tpl<S2,O2> & m, MotionDense<D2> & v) const
98
    {
99
6
      v.angular().setZero();
100


6
      v.linear().noalias() = m_v * (m.rotation().col(axis));
101
6
    }
102
103
    template<typename S2, int O2>
104
6
    MotionPlain se3Action_impl(const SE3Tpl<S2,O2> & m) const
105
    {
106
6
      MotionPlain res;
107
6
      se3Action_impl(m,res);
108
6
      return res;
109
    }
110
111
    template<typename S2, int O2, typename D2>
112
22
    void se3ActionInverse_impl(const SE3Tpl<S2,O2> & m, MotionDense<D2> & v) const
113
    {
114
      // Linear
115


22
      v.linear().noalias() = m_v * (m.rotation().transpose().col(axis));
116
117
      // Angular
118
22
      v.angular().setZero();
119
22
    }
120
121
    template<typename S2, int O2>
122
22
    MotionPlain se3ActionInverse_impl(const SE3Tpl<S2,O2> & m) const
123
    {
124
22
      MotionPlain res;
125
22
      se3ActionInverse_impl(m,res);
126
22
      return res;
127
    }
128
129
    template<typename M1, typename M2>
130
18
    void motionAction(const MotionDense<M1> & v, MotionDense<M2> & mout) const
131
    {
132
      // Linear
133

18
      CartesianAxis3::alphaCross(-m_v,v.angular(),mout.linear());
134
135
      // Angular
136
18
      mout.angular().setZero();
137
18
    }
138
139
    template<typename M1>
140
18
    MotionPlain motionAction(const MotionDense<M1> & v) const
141
    {
142
18
      MotionPlain res;
143
18
      motionAction(v,res);
144
18
      return res;
145
    }
146
147
6392
    Scalar & linearRate() { return m_v; }
148
    const Scalar & linearRate() const { return m_v; }
149
150
102
    bool isEqual_impl(const MotionPrismaticTpl & other) const
151
    {
152
102
      return m_v == other.m_v;
153
    }
154
155
  protected:
156
157
    Scalar m_v;
158
  }; // struct MotionPrismaticTpl
159
160
  template<typename Scalar, int Options, int axis, typename MotionDerived>
161
  typename MotionDerived::MotionPlain
162
  operator+(const MotionPrismaticTpl<Scalar,Options,axis> & m1,
163
            const MotionDense<MotionDerived> & m2)
164
  {
165
    typename MotionDerived::MotionPlain res(m2);
166
    res += m1;
167
    return res;
168
  }
169
170
  template<typename MotionDerived, typename S2, int O2, int axis>
171
  EIGEN_STRONG_INLINE
172
  typename MotionDerived::MotionPlain
173
12
  operator^(const MotionDense<MotionDerived> & m1, const MotionPrismaticTpl<S2,O2,axis> & m2)
174
  {
175
12
    return m2.motionAction(m1);
176
  }
177
178
  template<typename Scalar, int Options, int axis> struct TransformPrismaticTpl;
179
180
  template<typename _Scalar, int _Options, int _axis>
181
  struct traits< TransformPrismaticTpl<_Scalar,_Options,_axis> >
182
  {
183
    enum {
184
      axis = _axis,
185
      Options = _Options,
186
      LINEAR = 0,
187
      ANGULAR = 3
188
    };
189
    typedef _Scalar Scalar;
190
    typedef SE3Tpl<Scalar,Options> PlainType;
191
    typedef Eigen::Matrix<Scalar,3,1,Options> Vector3;
192
    typedef Eigen::Matrix<Scalar,3,3,Options> Matrix3;
193
    typedef typename Matrix3::IdentityReturnType AngularType;
194
    typedef AngularType AngularRef;
195
    typedef AngularType ConstAngularRef;
196
    typedef Vector3 LinearType;
197
    typedef const Vector3 LinearRef;
198
    typedef const Vector3 ConstLinearRef;
199
    typedef typename traits<PlainType>::ActionMatrixType ActionMatrixType;
200
    typedef typename traits<PlainType>::HomogeneousMatrixType HomogeneousMatrixType;
201
  }; // traits TransformPrismaticTpl
202
203
  template<typename Scalar, int Options, int axis>
204
  struct SE3GroupAction< TransformPrismaticTpl<Scalar,Options,axis> >
205
  { typedef typename traits <TransformPrismaticTpl<Scalar,Options,axis> >::PlainType ReturnType; };
206
207
  template<typename _Scalar, int _Options, int axis>
208
  struct TransformPrismaticTpl : SE3Base< TransformPrismaticTpl<_Scalar,_Options,axis> >
209
  {
210
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
211
    PINOCCHIO_SE3_TYPEDEF_TPL(TransformPrismaticTpl);
212
213
    typedef SpatialAxis<axis+LINEAR> Axis;
214
    typedef typename Axis::CartesianAxis3 CartesianAxis3;
215
216
42
    TransformPrismaticTpl() {}
217
18744
    TransformPrismaticTpl(const Scalar & displacement)
218
18774
    : m_displacement(displacement)
219
18774
    {}
220
221
36920
    PlainType plain() const
222
    {
223
36920
      PlainType res(PlainType::Identity());
224
36920
      res.rotation().setIdentity();
225
36920
      res.translation()[axis] = m_displacement;
226
227
36920
      return res;
228
    }
229
230
36920
    operator PlainType() const { return plain(); }
231
232
    template<typename S2, int O2>
233
    typename SE3GroupAction<TransformPrismaticTpl>::ReturnType
234
    se3action(const SE3Tpl<S2,O2> & m) const
235
    {
236
      typedef typename SE3GroupAction<TransformPrismaticTpl>::ReturnType ReturnType;
237
      ReturnType res(m);
238
      res.translation()[axis] += m_displacement;
239
240
      return res;
241
    }
242
243
2
    const Scalar & displacement() const { return m_displacement; }
244
37062
    Scalar & displacement() { return m_displacement; }
245
246
2
    ConstLinearRef translation() const { return CartesianAxis3()*displacement(); };
247
2
    AngularType rotation() const { return AngularType(3,3); }
248
249
102
    bool isEqual(const TransformPrismaticTpl & other) const
250
    {
251
102
      return m_displacement == other.m_displacement;
252
    }
253
254
  protected:
255
256
    Scalar m_displacement;
257
  };
258
259
  template<typename Scalar, int Options, int axis> struct ConstraintPrismaticTpl;
260
261
  template<typename _Scalar, int _Options, int axis>
262
  struct traits< ConstraintPrismaticTpl<_Scalar,_Options,axis> >
263
  {
264
    typedef _Scalar Scalar;
265
    enum { Options = _Options };
266
    enum {
267
      LINEAR = 0,
268
      ANGULAR = 3
269
    };
270
    typedef MotionPrismaticTpl<Scalar,Options,axis> JointMotion;
271
    typedef Eigen::Matrix<Scalar,1,1,Options> JointForce;
272
    typedef Eigen::Matrix<Scalar,6,1,Options> DenseBase;
273
    typedef DenseBase MatrixReturnType;
274
    typedef const DenseBase ConstMatrixReturnType;
275
  }; // traits ConstraintRevolute
276
277
  template<typename Scalar, int Options, int axis>
278
  struct SE3GroupAction< ConstraintPrismaticTpl<Scalar,Options,axis> >
279
  { typedef Eigen::Matrix<Scalar,6,1,Options> ReturnType; };
280
281
  template<typename Scalar, int Options, int axis, typename MotionDerived>
282
  struct MotionAlgebraAction< ConstraintPrismaticTpl<Scalar,Options,axis>, MotionDerived >
283
  { typedef Eigen::Matrix<Scalar,6,1,Options> ReturnType; };
284
285
  template<typename Scalar, int Options, int axis, typename ForceDerived>
286
  struct ConstraintForceOp< ConstraintPrismaticTpl<Scalar,Options,axis>, ForceDerived>
287
  { typedef typename ForceDense<ForceDerived>::ConstLinearType::template ConstFixedSegmentReturnType<1>::Type ReturnType; };
288
289
  template<typename Scalar, int Options, int axis, typename ForceSet>
290
  struct ConstraintForceSetOp< ConstraintPrismaticTpl<Scalar,Options,axis>, ForceSet>
291
  { typedef typename Eigen::MatrixBase<ForceSet>::ConstRowXpr ReturnType; };
292
293
  template<typename _Scalar, int _Options, int axis>
294
  struct ConstraintPrismaticTpl
295
  : ConstraintBase < ConstraintPrismaticTpl <_Scalar,_Options,axis> >
296
  {
297
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
298
    PINOCCHIO_CONSTRAINT_TYPEDEF_TPL(ConstraintPrismaticTpl)
299
    enum { NV = 1 };
300
301
    typedef SpatialAxis<LINEAR+axis> Axis;
302
303
18810
    ConstraintPrismaticTpl() {};
304
305
    template<typename Vector1Like>
306
28
    JointMotion __mult__(const Eigen::MatrixBase<Vector1Like> & v) const
307
    {
308
      EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Vector1Like,1);
309
28
      assert(v.size() == 1);
310
28
      return JointMotion(v[0]);
311
    }
312
313
    template<typename S2, int O2>
314
    typename SE3GroupAction<ConstraintPrismaticTpl>::ReturnType
315
34
    se3Action(const SE3Tpl<S2,O2> & m) const
316
    {
317
34
      typename SE3GroupAction<ConstraintPrismaticTpl>::ReturnType res;
318
34
      MotionRef<DenseBase> v(res);
319


34
      v.linear() = m.rotation().col(axis);
320

34
      v.angular().setZero();
321
68
      return res;
322
    }
323
324
    template<typename S2, int O2>
325
    typename SE3GroupAction<ConstraintPrismaticTpl>::ReturnType
326
38
    se3ActionInverse(const SE3Tpl<S2,O2> & m) const
327
    {
328
38
      typename SE3GroupAction<ConstraintPrismaticTpl>::ReturnType res;
329
38
      MotionRef<DenseBase> v(res);
330


38
      v.linear() = m.rotation().transpose().col(axis);
331

38
      v.angular().setZero();
332
76
      return res;
333
    }
334
335
114
    int nv_impl() const { return NV; }
336
337
    struct TransposeConst
338
    {
339
      const ConstraintPrismaticTpl & ref;
340
56
      TransposeConst(const ConstraintPrismaticTpl & ref) : ref(ref) {}
341
342
      template<typename ForceDerived>
343
      typename ConstraintForceOp<ConstraintPrismaticTpl,ForceDerived>::ReturnType
344
30
      operator* (const ForceDense<ForceDerived> & f) const
345
30
      { return f.linear().template segment<1>(axis); }
346
347
      /* [CRBA]  MatrixBase operator* (Constraint::Transpose S, ForceSet::Block) */
348
      template<typename Derived>
349
      typename ConstraintForceSetOp<ConstraintPrismaticTpl,Derived>::ReturnType
350
26
      operator*(const Eigen::MatrixBase<Derived> & F )
351
      {
352
26
        assert(F.rows()==6);
353
26
        return F.row(LINEAR+axis);
354
      }
355
356
    }; // struct TransposeConst
357
56
    TransposeConst transpose() const { return TransposeConst(*this); }
358
359
    /* CRBA joint operators
360
     *   - ForceSet::Block = ForceSet
361
     *   - ForceSet operator* (Inertia Y,Constraint S)
362
     *   - MatrixBase operator* (Constraint::Transpose S, ForceSet::Block)
363
     *   - SE3::act(ForceSet::Block)
364
     */
365
168
    DenseBase matrix_impl() const
366
    {
367
168
      DenseBase S;
368
168
      MotionRef<DenseBase> v(S);
369
168
      v << Axis();
370
336
      return S;
371
    }
372
373
    template<typename MotionDerived>
374
    typename MotionAlgebraAction<ConstraintPrismaticTpl,MotionDerived>::ReturnType
375
24
    motionAction(const MotionDense<MotionDerived> & m) const
376
    {
377
24
      typename MotionAlgebraAction<ConstraintPrismaticTpl,MotionDerived>::ReturnType res;
378
24
      MotionRef<DenseBase> v(res);
379

24
      v = m.cross(Axis());
380
48
      return res;
381
    }
382
383
102
    bool isEqual(const ConstraintPrismaticTpl &) const { return true; }
384
385
  }; // struct ConstraintPrismaticTpl
386
387
  template<typename S1, int O1,typename S2, int O2, int axis>
388
  struct MultiplicationOp<InertiaTpl<S1,O1>, ConstraintPrismaticTpl<S2,O2,axis> >
389
  {
390
    typedef Eigen::Matrix<S2,6,1,O2> ReturnType;
391
  };
392
393
  /* [CRBA] ForceSet operator* (Inertia Y,Constraint S) */
394
  namespace impl
395
  {
396
    template<typename S1, int O1, typename S2, int O2>
397
    struct LhsMultiplicationOp<InertiaTpl<S1,O1>, ConstraintPrismaticTpl<S2,O2,0> >
398
    {
399
      typedef InertiaTpl<S1,O1> Inertia;
400
      typedef ConstraintPrismaticTpl<S2,O2,0> Constraint;
401
      typedef typename MultiplicationOp<Inertia,Constraint>::ReturnType ReturnType;
402
8
      static inline ReturnType run(const Inertia & Y,
403
                                   const Constraint & /*constraint*/)
404
      {
405
8
        ReturnType res;
406
407
        /* Y(:,0) = ( 1,0, 0, 0 , z , -y ) */
408
        const S1
409
8
        &m = Y.mass(),
410
8
        &y = Y.lever()[1],
411
8
        &z = Y.lever()[2];
412


8
        res << m, S1(0), S1(0), S1(0), m*z, -m*y;
413
414
8
        return res;
415
      }
416
    };
417
418
    template<typename S1, int O1, typename S2, int O2>
419
    struct LhsMultiplicationOp<InertiaTpl<S1,O1>, ConstraintPrismaticTpl<S2,O2,1> >
420
    {
421
      typedef InertiaTpl<S1,O1> Inertia;
422
      typedef ConstraintPrismaticTpl<S2,O2,1> Constraint;
423
      typedef typename MultiplicationOp<Inertia,Constraint>::ReturnType ReturnType;
424
4
      static inline ReturnType run(const Inertia & Y,
425
                                   const Constraint & /*constraint*/)
426
      {
427
4
        ReturnType res;
428
429
        /* Y(:,1) = ( 0,1, 0, -z , 0 , x) */
430
        const S1
431
4
        &m = Y.mass(),
432
4
        &x = Y.lever()[0],
433
4
        &z = Y.lever()[2];
434
435



4
        res << S1(0), m, S1(0), -m*z, S1(0), m*x;
436
437
4
        return res;
438
      }
439
    };
440
441
    template<typename S1, int O1, typename S2, int O2>
442
    struct LhsMultiplicationOp<InertiaTpl<S1,O1>, ConstraintPrismaticTpl<S2,O2,2> >
443
    {
444
      typedef InertiaTpl<S1,O1> Inertia;
445
      typedef ConstraintPrismaticTpl<S2,O2,2> Constraint;
446
      typedef typename MultiplicationOp<Inertia,Constraint>::ReturnType ReturnType;
447
4
      static inline ReturnType run(const Inertia & Y,
448
                                   const Constraint & /*constraint*/)
449
      {
450
4
        ReturnType res;
451
452
        /* Y(:,2) = ( 0,0, 1, y , -x , 0) */
453
        const S1
454
4
        &m = Y.mass(),
455
4
        &x = Y.lever()[0],
456
4
        &y = Y.lever()[1];
457
458



4
        res << S1(0), S1(0), m, m*y, -m*x, S1(0);
459
460
4
        return res;
461
      }
462
    };
463
  } // namespace impl
464
465
  template<typename M6Like,typename S2, int O2, int axis>
466
  struct MultiplicationOp<Eigen::MatrixBase<M6Like>, ConstraintPrismaticTpl<S2,O2,axis> >
467
  {
468
    typedef typename M6Like::ConstColXpr ReturnType;
469
  };
470
471
  /* [ABA] operator* (Inertia Y,Constraint S) */
472
  namespace impl
473
  {
474
    template<typename M6Like, typename Scalar, int Options, int axis>
475
    struct LhsMultiplicationOp<Eigen::MatrixBase<M6Like>, ConstraintPrismaticTpl<Scalar,Options,axis> >
476
    {
477
      typedef ConstraintPrismaticTpl<Scalar,Options,axis> Constraint;
478
      typedef typename MultiplicationOp<Eigen::MatrixBase<M6Like>,Constraint>::ReturnType ReturnType;
479
12
      static inline ReturnType run(const Eigen::MatrixBase<M6Like> & Y,
480
                             const Constraint & /*constraint*/)
481
      {
482
        EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(M6Like,6,6);
483
12
        return Y.derived().col(Inertia::LINEAR + axis);
484
      }
485
    };
486
  } // namespace impl
487
488
  template<typename _Scalar, int _Options, int _axis>
489
  struct JointPrismaticTpl
490
  {
491
    typedef _Scalar Scalar;
492
493
    enum
494
    {
495
      Options = _Options,
496
      axis = _axis
497
    };
498
  };
499
500
  template<typename _Scalar, int _Options, int axis>
501
  struct traits< JointPrismaticTpl<_Scalar,_Options,axis> >
502
  {
503
    enum {
504
      NQ = 1,
505
      NV = 1
506
    };
507
    typedef _Scalar Scalar;
508
    enum { Options = _Options };
509
    typedef JointDataPrismaticTpl<Scalar,Options,axis> JointDataDerived;
510
    typedef JointModelPrismaticTpl<Scalar,Options,axis> JointModelDerived;
511
    typedef ConstraintPrismaticTpl<Scalar,Options,axis> Constraint_t;
512
    typedef TransformPrismaticTpl<Scalar,Options,axis> Transformation_t;
513
    typedef MotionPrismaticTpl<Scalar,Options,axis> Motion_t;
514
    typedef MotionZeroTpl<Scalar,Options> Bias_t;
515
516
    // [ABA]
517
    typedef Eigen::Matrix<Scalar,6,NV,Options> U_t;
518
    typedef Eigen::Matrix<Scalar,NV,NV,Options> D_t;
519
    typedef Eigen::Matrix<Scalar,6,NV,Options> UD_t;
520
521
    PINOCCHIO_JOINT_DATA_BASE_ACCESSOR_DEFAULT_RETURN_TYPE
522
523
    typedef Eigen::Matrix<Scalar,NQ,1,Options> ConfigVector_t;
524
    typedef Eigen::Matrix<Scalar,NV,1,Options> TangentVector_t;
525
  };
526
527
  template<typename Scalar, int Options, int axis>
528
  struct traits< JointDataPrismaticTpl<Scalar,Options,axis> >
529
  { typedef JointPrismaticTpl<Scalar,Options,axis> JointDerived; };
530
531
  template<typename Scalar, int Options, int axis>
532
  struct traits< JointModelPrismaticTpl<Scalar,Options,axis> >
533
  { typedef JointPrismaticTpl<Scalar,Options,axis> JointDerived; };
534
535
  template<typename _Scalar, int _Options, int axis>
536
  struct JointDataPrismaticTpl : public JointDataBase< JointDataPrismaticTpl<_Scalar,_Options,axis> >
537
  {
538
180
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
539
    typedef JointPrismaticTpl<_Scalar,_Options,axis> JointDerived;
540
    PINOCCHIO_JOINT_DATA_TYPEDEF_TEMPLATE(JointDerived);
541
2036
    PINOCCHIO_JOINT_DATA_BASE_DEFAULT_ACCESSOR
542
543
    Constraint_t S;
544
    Transformation_t M;
545
    Motion_t v;
546
    Bias_t c;
547
548
    // [ABA] specific data
549
    U_t U;
550
    D_t Dinv;
551
    UD_t UDinv;
552
553
18768
    JointDataPrismaticTpl()
554
    : M((Scalar)0)
555
    , v((Scalar)0)
556
    , U(U_t::Zero())
557
    , Dinv(D_t::Zero())
558

18768
    , UDinv(UD_t::Zero())
559
18768
    {}
560
561
264
    static std::string classname()
562
    {
563

264
      return std::string("JointDataP") + axisLabel<axis>();
564
    }
565
9
    std::string shortname() const { return classname(); }
566
567
  }; // struct JointDataPrismaticTpl
568
569
  template<typename NewScalar, typename Scalar, int Options, int axis>
570
  struct CastType< NewScalar, JointModelPrismaticTpl<Scalar,Options,axis> >
571
  {
572
    typedef JointModelPrismaticTpl<NewScalar,Options,axis> type;
573
  };
574
575
  template<typename _Scalar, int _Options, int axis>
576
  struct JointModelPrismaticTpl
577
  : public JointModelBase< JointModelPrismaticTpl<_Scalar,_Options,axis> >
578
  {
579
308
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
580
    typedef JointPrismaticTpl<_Scalar,_Options,axis> JointDerived;
581
    PINOCCHIO_JOINT_TYPEDEF_TEMPLATE(JointDerived);
582
583
    typedef JointModelBase<JointModelPrismaticTpl> Base;
584
    using Base::id;
585
    using Base::idx_q;
586
    using Base::idx_v;
587
    using Base::setIndexes;
588
589
18610
    JointDataDerived createData() const { return JointDataDerived(); }
590
591
2
    const std::vector<bool> hasConfigurationLimit() const
592
    {
593
2
      return {true};
594
    }
595
596
2
    const std::vector<bool> hasConfigurationLimitInTangent() const
597
    {
598
2
      return {true};
599
    }
600
601
    template<typename ConfigVector>
602
36894
    void calc(JointDataDerived & data,
603
              const typename Eigen::MatrixBase<ConfigVector> & qs) const
604
    {
605
      typedef typename ConfigVector::Scalar Scalar;
606
36894
      const Scalar & q = qs[idx_q()];
607
36894
      data.M.displacement() = q;
608
36894
    }
609
610
    template<typename ConfigVector, typename TangentVector>
611
6224
    void calc(JointDataDerived & data,
612
              const typename Eigen::MatrixBase<ConfigVector> & qs,
613
              const typename Eigen::MatrixBase<TangentVector> & vs) const
614
    {
615
6224
      calc(data,qs.derived());
616
617
      typedef typename TangentVector::Scalar S2;
618
6224
      const S2 & v = vs[idx_v()];
619
6224
      data.v.linearRate() = v;
620
6224
    }
621
622
    template<typename Matrix6Like>
623
44
    void calc_aba(JointDataDerived & data, const Eigen::MatrixBase<Matrix6Like> & I, const bool update_I) const
624
    {
625
44
      data.U = I.col(Inertia::LINEAR + axis);
626
44
      data.Dinv[0] = Scalar(1)/I(Inertia::LINEAR + axis, Inertia::LINEAR + axis);
627

44
      data.UDinv.noalias() = data.U * data.Dinv[0];
628
629
44
      if (update_I)
630

12
        PINOCCHIO_EIGEN_CONST_CAST(Matrix6Like,I) -= data.UDinv * data.U.transpose();
631
44
    }
632
633
110571
    static std::string classname()
634
    {
635

110571
      return std::string("JointModelP") + axisLabel<axis>();
636
    }
637
55161
    std::string shortname() const { return classname(); }
638
639
    /// \returns An expression of *this with the Scalar type casted to NewScalar.
640
    template<typename NewScalar>
641
12
    JointModelPrismaticTpl<NewScalar,Options,axis> cast() const
642
    {
643
      typedef JointModelPrismaticTpl<NewScalar,Options,axis> ReturnType;
644
12
      ReturnType res;
645
12
      res.setIndexes(id(),idx_q(),idx_v());
646
12
      return res;
647
    }
648
649
  }; // struct JointModelPrismaticTpl
650
651
  typedef JointPrismaticTpl<double,0,0> JointPX;
652
  typedef JointDataPrismaticTpl<double,0,0> JointDataPX;
653
  typedef JointModelPrismaticTpl<double,0,0> JointModelPX;
654
655
  typedef JointPrismaticTpl<double,0,1> JointPY;
656
  typedef JointDataPrismaticTpl<double,0,1> JointDataPY;
657
  typedef JointModelPrismaticTpl<double,0,1> JointModelPY;
658
659
  typedef JointPrismaticTpl<double,0,2> JointPZ;
660
  typedef JointDataPrismaticTpl<double,0,2> JointDataPZ;
661
  typedef JointModelPrismaticTpl<double,0,2> JointModelPZ;
662
663
} //namespace pinocchio
664
665
#include <boost/type_traits.hpp>
666
667
namespace boost
668
{
669
  template<typename Scalar, int Options, int axis>
670
  struct has_nothrow_constructor< ::pinocchio::JointModelPrismaticTpl<Scalar,Options,axis> >
671
  : public integral_constant<bool,true> {};
672
673
  template<typename Scalar, int Options, int axis>
674
  struct has_nothrow_copy< ::pinocchio::JointModelPrismaticTpl<Scalar,Options,axis> >
675
  : public integral_constant<bool,true> {};
676
677
  template<typename Scalar, int Options, int axis>
678
  struct has_nothrow_constructor< ::pinocchio::JointDataPrismaticTpl<Scalar,Options,axis> >
679
  : public integral_constant<bool,true> {};
680
681
  template<typename Scalar, int Options, int axis>
682
  struct has_nothrow_copy< ::pinocchio::JointDataPrismaticTpl<Scalar,Options,axis> >
683
  : public integral_constant<bool,true> {};
684
}
685
686
#endif // ifndef __pinocchio_joint_prismatic_hpp__