joint-prismatic-unaligned.hpp 19.4 KB
Newer Older
jcarpent's avatar
jcarpent committed
1
//
2
// Copyright (c) 2015-2019 CNRS INRIA
3
// Copyright (c) 2016 Wandercraft, 86 rue de Paris 91400 Orsay, France.
jcarpent's avatar
jcarpent committed
4
5
//

6
7
#ifndef __pinocchio_joint_prismatic_unaligned_hpp__
#define __pinocchio_joint_prismatic_unaligned_hpp__
jcarpent's avatar
jcarpent committed
8

9
#include "pinocchio/macros.hpp"
jcarpent's avatar
jcarpent committed
10
#include "pinocchio/multibody/joint/joint-base.hpp"
11
#include "pinocchio/multibody/joint/joint-translation.hpp"
jcarpent's avatar
jcarpent committed
12
13
#include "pinocchio/multibody/constraint.hpp"
#include "pinocchio/spatial/inertia.hpp"
14
#include "pinocchio/math/matrix.hpp"
jcarpent's avatar
jcarpent committed
15

16
namespace pinocchio
jcarpent's avatar
jcarpent committed
17
18
{

19
20
21
  template<typename Scalar, int Options=0> struct MotionPrismaticUnalignedTpl;
  typedef MotionPrismaticUnalignedTpl<double> MotionPrismaticUnaligned;
  
22
23
  template<typename Scalar, int Options>
  struct SE3GroupAction< MotionPrismaticUnalignedTpl<Scalar,Options> >
24
  {
25
26
    typedef MotionTpl<Scalar,Options> ReturnType;
  };
jcarpent's avatar
jcarpent committed
27
  
28
29
30
31
32
33
  template<typename Scalar, int Options, typename MotionDerived>
  struct MotionAlgebraAction< MotionPrismaticUnalignedTpl<Scalar,Options>, MotionDerived>
  {
    typedef MotionTpl<Scalar,Options> ReturnType;
  };

34
  template<typename _Scalar, int _Options>
35
  struct traits< MotionPrismaticUnalignedTpl<_Scalar,_Options> >
jcarpent's avatar
jcarpent committed
36
  {
37
    typedef _Scalar Scalar;
38
39
40
41
    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
42
43
    typedef typename PINOCCHIO_EIGEN_REF_CONST_TYPE(Vector6) ToVectorConstReturnType;
    typedef typename PINOCCHIO_EIGEN_REF_TYPE(Vector6) ToVectorReturnType;
44
45
46
47
48
    typedef Vector3 AngularType;
    typedef Vector3 LinearType;
    typedef const Vector3 ConstAngularType;
    typedef const Vector3 ConstLinearType;
    typedef Matrix6 ActionMatrixType;
49
    typedef MotionTpl<Scalar,Options> MotionPlain;
50
    typedef MotionPlain PlainReturnType;
jcarpent's avatar
jcarpent committed
51
52
53
54
    enum {
      LINEAR = 0,
      ANGULAR = 3
    };
55
  }; // traits MotionPrismaticUnalignedTpl
jcarpent's avatar
jcarpent committed
56

57
  template<typename _Scalar, int _Options>
58
59
  struct MotionPrismaticUnalignedTpl
  : MotionBase < MotionPrismaticUnalignedTpl<_Scalar,_Options> >
jcarpent's avatar
jcarpent committed
60
  {
61
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
62
    MOTION_TYPEDEF_TPL(MotionPrismaticUnalignedTpl);
jcarpent's avatar
jcarpent committed
63

64
    MotionPrismaticUnalignedTpl() {}
65
66
    
    template<typename Vector3Like, typename S2>
67
    MotionPrismaticUnalignedTpl(const Eigen::MatrixBase<Vector3Like> & axis,
68
                                const S2 & rate)
69
70
    : axis(axis), rate(rate)
    { EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Vector3Like,3); }
jcarpent's avatar
jcarpent committed
71

72
    inline PlainReturnType plain() const
73
    {
74
75
      return PlainReturnType(axis*rate,
                             PlainReturnType::Vector3::Zero());
76
77
78
79
80
81
82
    }
    
    template<typename OtherScalar>
    MotionPrismaticUnalignedTpl __mult__(const OtherScalar & alpha) const
    {
      return MotionPrismaticUnalignedTpl(axis,alpha*rate);
    }
83
84
    
    template<typename Derived>
85
    void addTo(MotionDense<Derived> & other) const
86
    {
87
      other.linear() += axis * rate;
88
    }
89
    
90
91
92
93
94
95
96
    template<typename Derived>
    void setTo(MotionDense<Derived> & other) const
    {
      other.linear().noalias() = axis*rate;
      other.angular().setZero();
    }

97
98
99
100
    template<typename S2, int O2, typename D2>
    void se3Action_impl(const SE3Tpl<S2,O2> & m, MotionDense<D2> & v) const
    {
      v.linear().noalias() = rate * (m.rotation() * axis); // TODO: check efficiency
101
      v.angular().setZero();
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
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
    }
    
    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() * 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
      mout.linear().noalias() = v.angular().cross(axis);
      mout.linear() *= rate;
      
      // Angular
      mout.angular().setZero();
    }
    
    template<typename M1>
    MotionPlain motionAction(const MotionDense<M1> & v) const
    {
      MotionPlain res;
      motionAction(v,res);
      return res;
    }
    
149
    // data
150
151
    Vector3 axis;
    Scalar rate;
152
  }; // struct MotionPrismaticUnalignedTpl
jcarpent's avatar
jcarpent committed
153

154
155
  template<typename Scalar, int Options, typename MotionDerived>
  inline typename MotionDerived::MotionPlain
156
  operator+(const MotionPrismaticUnalignedTpl<Scalar,Options> & m1, const MotionDense<MotionDerived> & m2)
jcarpent's avatar
jcarpent committed
157
  {
158
159
    typedef typename MotionDerived::MotionPlain ReturnType;
    return ReturnType(m1.rate*m1.axis + m2.linear(), m2.angular());
jcarpent's avatar
jcarpent committed
160
  }
161
162
163
164
165
166
167
168
  
  template<typename MotionDerived, typename S2, int O2>
  inline typename MotionDerived::MotionPlain
  operator^(const MotionDense<MotionDerived> & m1,
            const MotionPrismaticUnalignedTpl<S2,O2> & m2)
  {
    return m2.motionAction(m1);
  }
jcarpent's avatar
jcarpent committed
169

170
  template<typename Scalar, int Options> struct ConstraintPrismaticUnalignedTpl;
171
  
172
  template<typename _Scalar, int _Options>
173
  struct traits< ConstraintPrismaticUnalignedTpl<_Scalar,_Options> >
jcarpent's avatar
jcarpent committed
174
  {
175
    typedef _Scalar Scalar;
176
    enum { Options = _Options };
jcarpent's avatar
jcarpent committed
177
178
179
180
    enum {
      LINEAR = 0,
      ANGULAR = 3
    };
181
    typedef MotionPrismaticUnalignedTpl<Scalar,Options> JointMotion;
182
183
    typedef Eigen::Matrix<Scalar,1,1,Options> JointForce;
    typedef Eigen::Matrix<Scalar,6,1,Options> DenseBase;
184
185
    typedef DenseBase MatrixReturnType;
    typedef const DenseBase ConstMatrixReturnType;
186
187
    
    typedef Eigen::Matrix<Scalar,3,1,Options> Vector3;
188
  }; // traits ConstraintPrismaticUnalignedTpl
189
190
  
  template<typename Scalar, int Options>
Justin Carpentier's avatar
Justin Carpentier committed
191
  struct SE3GroupAction< ConstraintPrismaticUnalignedTpl<Scalar,Options> >
192
193
194
  { typedef Eigen::Matrix<Scalar,6,1,Options> ReturnType; };
  
  template<typename Scalar, int Options, typename MotionDerived>
Justin Carpentier's avatar
Justin Carpentier committed
195
  struct MotionAlgebraAction< ConstraintPrismaticUnalignedTpl<Scalar,Options>,MotionDerived >
196
197
198
  { typedef Eigen::Matrix<Scalar,6,1,Options> ReturnType; };

  template<typename Scalar, int Options, typename ForceDerived>
Justin Carpentier's avatar
Justin Carpentier committed
199
  struct ConstraintForceOp< ConstraintPrismaticUnalignedTpl<Scalar,Options>, ForceDerived>
200
  {
Justin Carpentier's avatar
Justin Carpentier committed
201
    typedef typename traits< ConstraintPrismaticUnalignedTpl<Scalar,Options> >::Vector3 Vector3;
202
203
204
205
    typedef Eigen::Matrix<typename PINOCCHIO_EIGEN_DOT_PRODUCT_RETURN_TYPE(Vector3,typename ForceDense<ForceDerived>::ConstAngularType),1,1,Options> ReturnType;
  };
  
  template<typename Scalar, int Options, typename ForceSet>
Justin Carpentier's avatar
Justin Carpentier committed
206
  struct ConstraintForceSetOp< ConstraintPrismaticUnalignedTpl<Scalar,Options>, ForceSet>
207
  {
Justin Carpentier's avatar
Justin Carpentier committed
208
    typedef typename traits< ConstraintPrismaticUnalignedTpl<Scalar,Options> >::Vector3 Vector3;
209
210
211
212
    typedef typename MatrixMatrixProduct<Eigen::Transpose<const Vector3>,
    typename Eigen::MatrixBase<const ForceSet>::template NRowsBlockXpr<3>::Type
    >::type ReturnType;
  };
jcarpent's avatar
jcarpent committed
213

214
  template<typename _Scalar, int _Options>
215
216
  struct ConstraintPrismaticUnalignedTpl
  : ConstraintBase< ConstraintPrismaticUnalignedTpl<_Scalar,_Options> >
217
  {
218
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
219
    PINOCCHIO_CONSTRAINT_TYPEDEF_TPL(ConstraintPrismaticUnalignedTpl)
220
    
221
    enum { NV = 1 };
222
    
223
    typedef typename traits<ConstraintPrismaticUnalignedTpl>::Vector3 Vector3;
224

225
    ConstraintPrismaticUnalignedTpl() {}
226
227
    
    template<typename Vector3Like>
228
    ConstraintPrismaticUnalignedTpl(const Eigen::MatrixBase<Vector3Like> & axis)
229
230
231
    : axis(axis)
    { EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Vector3Like,3); }

232
233
    template<typename Vector1Like>
    JointMotion __mult__(const Eigen::MatrixBase<Vector1Like> & v) const
234
    {
235
236
      EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Vector1Like,1);
      return JointMotion(axis,v[0]);
237
238
239
    }
    
    template<typename S1, int O1>
240
241
    typename SE3GroupAction<ConstraintPrismaticUnalignedTpl>::ReturnType
    se3Action(const SE3Tpl<S1,O1> & m) const
242
    {
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
      typename SE3GroupAction<ConstraintPrismaticUnalignedTpl>::ReturnType res;
      MotionRef<DenseBase> v(res);
      v.linear().noalias() = m.rotation()*axis;
      v.angular().setZero();
      return res;
    }
    
    template<typename S1, int O1>
    typename SE3GroupAction<ConstraintPrismaticUnalignedTpl>::ReturnType
    se3ActionInverse(const SE3Tpl<S1,O1> & m) const
    {
      typename SE3GroupAction<ConstraintPrismaticUnalignedTpl>::ReturnType res;
      MotionRef<DenseBase> v(res);
      v.linear().noalias() = m.rotation().transpose()*axis;
      v.angular().setZero();
258
259
260
261
262
263
      return res;
    }
    
    int nv_impl() const { return NV; }
    
    struct TransposeConst
jcarpent's avatar
jcarpent committed
264
    {
265
266
      const ConstraintPrismaticUnalignedTpl & ref;
      TransposeConst(const ConstraintPrismaticUnalignedTpl & ref) : ref(ref) {}
267
      
268
      template<typename ForceDerived>
Justin Carpentier's avatar
Justin Carpentier committed
269
      typename ConstraintForceOp<ConstraintPrismaticUnalignedTpl,ForceDerived>::ReturnType
270
      operator* (const ForceDense<ForceDerived> & f) const
jcarpent's avatar
jcarpent committed
271
      {
Justin Carpentier's avatar
Justin Carpentier committed
272
        typedef typename ConstraintForceOp<ConstraintPrismaticUnalignedTpl,ForceDerived>::ReturnType ReturnType;
273
274
275
        ReturnType res;
        res[0] = ref.axis.dot(f.linear());
        return res;
jcarpent's avatar
jcarpent committed
276
      }
277
278
      
      /* [CRBA]  MatrixBase operator* (Constraint::Transpose S, ForceSet::Block) */
279
      template<typename ForceSet>
Justin Carpentier's avatar
Justin Carpentier committed
280
      typename ConstraintForceSetOp<ConstraintPrismaticUnalignedTpl,ForceSet>::ReturnType
281
      operator*(const Eigen::MatrixBase<ForceSet> & F)
jcarpent's avatar
jcarpent committed
282
      {
283
        EIGEN_STATIC_ASSERT(ForceSet::RowsAtCompileTime==6,THIS_METHOD_IS_ONLY_FOR_MATRICES_OF_A_SPECIFIC_SIZE)
284
        /* Return ax.T * F[1:3,:] */
285
        return ref.axis.transpose() * F.template middleRows<3>(LINEAR);
jcarpent's avatar
jcarpent committed
286
287
      }
      
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
    };
    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)
     */
    DenseBase matrix_impl() const
    {
      DenseBase S;
      S << axis, Vector3::Zero();
      return S;
    }
    
    template<typename MotionDerived>
    DenseBase motionAction(const MotionDense<MotionDerived> & v) const
    {
      DenseBase res;
      res << v.angular().cross(axis), Vector3::Zero();
310
      
311
312
313
      return res;
    }
    
314
    // data
315
316
    Vector3 axis;
    
317
  }; // struct ConstraintPrismaticUnalignedTpl
318
319
320
  
  template<typename S1, int O1,typename S2, int O2>
  struct MultiplicationOp<InertiaTpl<S1,O1>, ConstraintPrismaticUnalignedTpl<S2,O2> >
321
  {
322
323
324
    typedef Eigen::Matrix<S2,6,1,O2> ReturnType;
  };
  
325
  /* [CRBA] ForceSet operator* (Inertia Y,Constraint S) */
326
327
328
329
330
331
332
333
334
335
336
337
338
339
340
341
342
343
344
345
346
347
348
349
350
351
352
  namespace impl
  {
    template<typename S1, int O1, typename S2, int O2>
    struct LhsMultiplicationOp<InertiaTpl<S1,O1>, ConstraintPrismaticUnalignedTpl<S2,O2> >
    {
      typedef InertiaTpl<S1,O1> Inertia;
      typedef ConstraintPrismaticUnalignedTpl<S2,O2> Constraint;
      typedef typename MultiplicationOp<Inertia,Constraint>::ReturnType ReturnType;
      
      static inline ReturnType run(const Inertia & Y,
                                   const Constraint & cpu)
      {
        ReturnType res;
        /* YS = [ m -mcx ; mcx I-mcxcx ] [ 0 ; w ] = [ mcxw ; Iw -mcxcxw ] */
        const S1 & m                             = Y.mass();
        const typename Inertia::Vector3 & c      = Y.lever();
        
        res.template head<3>().noalias() = m*cpu.axis;
        res.template tail<3>().noalias() = c.cross(res.template head<3>());
        
        return res;
      }
    };
  } // namespace impl
  
  template<typename M6Like, typename Scalar, int Options>
  struct MultiplicationOp<Eigen::MatrixBase<M6Like>, ConstraintPrismaticUnalignedTpl<Scalar,Options> >
353
  {
354
355
    typedef typename SizeDepType<3>::ColsReturn<M6Like>::ConstType M6LikeCols;
    typedef typename Eigen::internal::remove_const<M6LikeCols>::type M6LikeColsNonConst;
356
    
357
358
359
360
    typedef ConstraintPrismaticUnalignedTpl<Scalar,Options> Constraint;
    typedef typename Constraint::Vector3 Vector3;
    typedef const typename MatrixMatrixProduct<M6LikeColsNonConst,Vector3>::type ReturnType;
  };
jcarpent's avatar
jcarpent committed
361
  
362
363
  /* [ABA] operator* (Inertia Y,Constraint S) */
  namespace impl
364
  {
365
366
367
368
369
370
371
372
373
374
375
376
377
378
    template<typename M6Like, typename Scalar, int Options>
    struct LhsMultiplicationOp<Eigen::MatrixBase<M6Like>, ConstraintPrismaticUnalignedTpl<Scalar,Options> >
    {
      typedef ConstraintPrismaticUnalignedTpl<Scalar,Options> Constraint;
      typedef typename MultiplicationOp<Eigen::MatrixBase<M6Like>,Constraint>::ReturnType ReturnType;
      static inline ReturnType run(const Eigen::MatrixBase<M6Like> & Y,
                                   const Constraint & cru)
      {
        EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(M6Like,6,6);
        return Y.derived().template middleCols<3>(Constraint::LINEAR) * cru.axis;
      }
    };
  } // namespace impl
  
379
  template<typename Scalar, int Options> struct JointPrismaticUnalignedTpl;
380
  
381
382
  template<typename _Scalar, int _Options>
  struct traits< JointPrismaticUnalignedTpl<_Scalar,_Options> >
383
384
385
386
  {
    enum {
      NQ = 1,
      NV = 1
jcarpent's avatar
jcarpent committed
387
    };
388
    typedef _Scalar Scalar;
389
390
391
    enum { Options = _Options };
    typedef JointDataPrismaticUnalignedTpl<Scalar,Options> JointDataDerived;
    typedef JointModelPrismaticUnalignedTpl<Scalar,Options> JointModelDerived;
392
    typedef ConstraintPrismaticUnalignedTpl<Scalar,Options> Constraint_t;
393
    typedef TransformTranslationTpl<Scalar,Options> Transformation_t;
394
    typedef MotionPrismaticUnalignedTpl<Scalar,Options> Motion_t;
395
    typedef MotionZeroTpl<Scalar,Options> Bias_t;
Justin Carpentier's avatar
Justin Carpentier committed
396

397
    // [ABA]
398
399
400
    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;
401
    
Gabriele Buondonno's avatar
Gabriele Buondonno committed
402
    PINOCCHIO_JOINT_DATA_BASE_ACCESSOR_DEFAULT_RETURN_TYPE
403
    
404
405
    typedef Eigen::Matrix<Scalar,NQ,1,Options> ConfigVector_t;
    typedef Eigen::Matrix<Scalar,NV,1,Options> TangentVector_t;
406
  };
jcarpent's avatar
jcarpent committed
407

408
409
410
  template<typename Scalar, int Options>
  struct traits< JointDataPrismaticUnalignedTpl<Scalar,Options> >
  { typedef JointPrismaticUnalignedTpl<Scalar,Options> JointDerived; };
jcarpent's avatar
jcarpent committed
411

412
  template<typename _Scalar, int _Options>
413
414
  struct JointDataPrismaticUnalignedTpl
  : public JointDataBase< JointDataPrismaticUnalignedTpl<_Scalar,_Options> >
jcarpent's avatar
jcarpent committed
415
  {
416
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
417
    typedef JointPrismaticUnalignedTpl<_Scalar,_Options> JointDerived;
418
    PINOCCHIO_JOINT_DATA_TYPEDEF_TEMPLATE(JointDerived);
Gabriele Buondonno's avatar
Gabriele Buondonno committed
419
    PINOCCHIO_JOINT_DATA_BASE_DEFAULT_ACCESSOR
420
    
jcarpent's avatar
jcarpent committed
421
422
423
424
425
    Transformation_t M;
    Constraint_t S;
    Motion_t v;
    Bias_t c;

426
427
428
429
    // [ABA] specific data
    U_t U;
    D_t Dinv;
    UD_t UDinv;
jcarpent's avatar
jcarpent committed
430

431
    JointDataPrismaticUnalignedTpl() {}
jcarpent's avatar
jcarpent committed
432
    
433
434
    template<typename Vector3Like>
    JointDataPrismaticUnalignedTpl(const Eigen::MatrixBase<Vector3Like> & axis)
435
    : M()
436
    , S(axis)
437
    , v(axis,(Scalar)NAN)
438
    , U(), Dinv(), UDinv()
jcarpent's avatar
jcarpent committed
439
440
    {}

441
442
443
    static std::string classname() { return std::string("JointDataPrismaticUnaligned"); }
    std::string shortname() const { return classname(); }
    
444
445
  }; // struct JointDataPrismaticUnalignedTpl
  
446
447
448
  template<typename Scalar, int Options>
  struct traits< JointModelPrismaticUnalignedTpl<Scalar,Options> >
  { typedef JointPrismaticUnalignedTpl<Scalar,Options> JointDerived; };
jcarpent's avatar
jcarpent committed
449

Gabriele Buondonno's avatar
Gabriele Buondonno committed
450
  PINOCCHIO_JOINT_CAST_TYPE_SPECIALIZATION(JointModelPrismaticUnalignedTpl);
451
  template<typename _Scalar, int _Options>
jcarpent's avatar
jcarpent committed
452
453
  struct JointModelPrismaticUnalignedTpl
  : public JointModelBase< JointModelPrismaticUnalignedTpl<_Scalar,_Options> >
jcarpent's avatar
jcarpent committed
454
  {
455
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
456
    typedef JointPrismaticUnalignedTpl<_Scalar,_Options> JointDerived;
457
    PINOCCHIO_JOINT_TYPEDEF_TEMPLATE(JointDerived);
jcarpent's avatar
jcarpent committed
458
459
460
461
462
463
464
465
    
    typedef JointModelBase<JointModelPrismaticUnalignedTpl> Base;
    using Base::id;
    using Base::idx_q;
    using Base::idx_v;
    using Base::setIndexes;
    
    typedef Eigen::Matrix<Scalar,3,1,_Options> Vector3;
jcarpent's avatar
jcarpent committed
466
    
467
    JointModelPrismaticUnalignedTpl() {}
468
469
470
    JointModelPrismaticUnalignedTpl(const Scalar & x,
                                    const Scalar & y,
                                    const Scalar & z)
471
    : axis(x,y,z)
jcarpent's avatar
jcarpent committed
472
473
    {
      axis.normalize();
474
      assert(isUnitary(axis) && "Translation axis is not unitary");
jcarpent's avatar
jcarpent committed
475
476
    }
    
477
    template<typename Vector3Like>
478
479
    JointModelPrismaticUnalignedTpl(const Eigen::MatrixBase<Vector3Like> & axis)
    : axis(axis)
jcarpent's avatar
jcarpent committed
480
    {
481
      EIGEN_STATIC_ASSERT_VECTOR_ONLY(Vector3Like);
482
      assert(isUnitary(axis) && "Translation axis is not unitary");
jcarpent's avatar
jcarpent committed
483
484
    }

485
    JointDataDerived createData() const { return JointDataDerived(axis); }
jcarpent's avatar
jcarpent committed
486
    
487
488
489
490
491
492
    using Base::isEqual;
    bool isEqual(const JointModelPrismaticUnalignedTpl & other) const
    {
      return Base::isEqual(other) && axis == other.axis;
    }
    
493
494
495
    template<typename ConfigVector>
    void calc(JointDataDerived & data,
              const typename Eigen::MatrixBase<ConfigVector> & qs) const
jcarpent's avatar
jcarpent committed
496
    {
497
498
      typedef typename ConfigVector::Scalar Scalar;
      const Scalar & q = qs[idx_q()];
jcarpent's avatar
jcarpent committed
499

500
      data.M.translation().noalias() = axis * q;
jcarpent's avatar
jcarpent committed
501
502
    }

503
    template<typename ConfigVector, typename TangentVector>
504
    void calc(JointDataDerived & data,
505
506
              const typename Eigen::MatrixBase<ConfigVector> & qs,
              const typename Eigen::MatrixBase<TangentVector> & vs) const
jcarpent's avatar
jcarpent committed
507
    {
508
509
510
511
      calc(data,qs.derived());
      
      typedef typename TangentVector::Scalar S2;
      const S2 & v = vs[idx_v()];
512
      data.v.rate = v;
jcarpent's avatar
jcarpent committed
513
    }
514
    
515
516
    template<typename Matrix6Like>
    void calc_aba(JointDataDerived & data, const Eigen::MatrixBase<Matrix6Like> & I, const bool update_I) const
517
    {
518
      data.U.noalias() = I.template block<6,3> (0,Inertia::LINEAR) * axis;
519
      data.Dinv[0] = Scalar(1)/axis.dot(data.U.template segment <3> (Inertia::LINEAR));
520
      data.UDinv.noalias() = data.U * data.Dinv;
521
522
      
      if (update_I)
gabrielebndn's avatar
gabrielebndn committed
523
        PINOCCHIO_EIGEN_CONST_CAST(Matrix6Like,I) -= data.UDinv * data.U.transpose();
524
    }
525
    
526
    static std::string classname() { return std::string("JointModelPrismaticUnaligned"); }
527
    std::string shortname() const { return classname(); }
528
529
530
531
532
533
534
535
536
537
    
    /// \returns An expression of *this with the Scalar type casted to NewScalar.
    template<typename NewScalar>
    JointModelPrismaticUnalignedTpl<NewScalar,Options> cast() const
    {
      typedef JointModelPrismaticUnalignedTpl<NewScalar,Options> ReturnType;
      ReturnType res(axis.template cast<NewScalar>());
      res.setIndexes(id(),idx_q(),idx_v());
      return res;
    }
538
539
540
541
542
543
    
    // data
    
    ///
    /// \brief 3d main axis of the joint.
    ///
544
    Vector3 axis;
545
546
  }; // struct JointModelPrismaticUnalignedTpl
  
547
} //namespace pinocchio
jcarpent's avatar
jcarpent committed
548

549
550
551
552
553
#include <boost/type_traits.hpp>

namespace boost
{
  template<typename Scalar, int Options>
554
  struct has_nothrow_constructor< ::pinocchio::JointModelPrismaticUnalignedTpl<Scalar,Options> >
555
556
557
  : public integral_constant<bool,true> {};
  
  template<typename Scalar, int Options>
558
  struct has_nothrow_copy< ::pinocchio::JointModelPrismaticUnalignedTpl<Scalar,Options> >
559
560
561
  : public integral_constant<bool,true> {};
  
  template<typename Scalar, int Options>
562
  struct has_nothrow_constructor< ::pinocchio::JointDataPrismaticUnalignedTpl<Scalar,Options> >
563
564
565
  : public integral_constant<bool,true> {};
  
  template<typename Scalar, int Options>
566
  struct has_nothrow_copy< ::pinocchio::JointDataPrismaticUnalignedTpl<Scalar,Options> >
567
568
569
  : public integral_constant<bool,true> {};
}

jcarpent's avatar
jcarpent committed
570

571
#endif // ifndef __pinocchio_joint_prismatic_unaligned_hpp__