joint-translation.hpp 17.5 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_translation_hpp__
#define __pinocchio_joint_translation_hpp__
8

9
#include "pinocchio/macros.hpp"
10
11
12
13
14
#include "pinocchio/multibody/joint/joint-base.hpp"
#include "pinocchio/multibody/constraint.hpp"
#include "pinocchio/spatial/inertia.hpp"
#include "pinocchio/spatial/skew.hpp"

15
namespace pinocchio
16
17
{

18
19
20
  template<typename Scalar, int Options=0> struct MotionTranslationTpl;
  typedef MotionTranslationTpl<double> MotionTranslation;
  
21
22
  template<typename Scalar, int Options>
  struct SE3GroupAction< MotionTranslationTpl<Scalar,Options> >
23
  {
24
25
    typedef MotionTpl<Scalar,Options> ReturnType;
  };
26
  
27
28
29
30
31
32
  template<typename Scalar, int Options, typename MotionDerived>
  struct MotionAlgebraAction< MotionTranslationTpl<Scalar,Options>, MotionDerived>
  {
    typedef MotionTpl<Scalar,Options> ReturnType;
  };

33
34
  template<typename _Scalar, int _Options>
  struct traits< MotionTranslationTpl<_Scalar,_Options> >
35
  {
36
37
38
39
40
    typedef _Scalar Scalar;
    enum { Options = _Options };
    typedef Eigen::Matrix<Scalar,3,1,Options> Vector3;
    typedef Eigen::Matrix<Scalar,6,1,Options> Vector6;
    typedef Eigen::Matrix<Scalar,6,6,Options> Matrix6;
gabrielebndn's avatar
gabrielebndn committed
41
42
    typedef typename PINOCCHIO_EIGEN_REF_CONST_TYPE(Vector6) ToVectorConstReturnType;
    typedef typename PINOCCHIO_EIGEN_REF_TYPE(Vector6) ToVectorReturnType;
43
44
45
46
47
    typedef Vector3 AngularType;
    typedef Vector3 LinearType;
    typedef const Vector3 ConstAngularType;
    typedef const Vector3 ConstLinearType;
    typedef Matrix6 ActionMatrixType;
48
    typedef MotionTpl<Scalar,Options> MotionPlain;
49
    typedef MotionPlain PlainReturnType;
50
51
52
53
    enum {
      LINEAR = 0,
      ANGULAR = 3
    };
54
  }; // traits MotionTranslationTpl
55

56
  template<typename _Scalar, int _Options>
57
58
  struct MotionTranslationTpl
  : MotionBase< MotionTranslationTpl<_Scalar,_Options> >
59
  {
60
61
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
    
62
    MOTION_TYPEDEF_TPL(MotionTranslationTpl);
63

64
    MotionTranslationTpl() {}
65
    
66
    template<typename Vector3Like>
67
68
69
    MotionTranslationTpl(const Eigen::MatrixBase<Vector3Like> & v)
    : rate(v)
    {}
70
    
71
72
73
    MotionTranslationTpl(const MotionTranslationTpl & other)
    : rate(other.rate)
    {}
74
 
75
76
    Vector3 & operator()() { return rate; }
    const Vector3 & operator()() const { return rate; }
77
    
78
79
80
81
    inline PlainReturnType plain() const
    {
      return PlainReturnType(rate,PlainReturnType::Vector3::Zero());
    }
82
    
83
    MotionTranslationTpl & operator=(const MotionTranslationTpl & other)
84
    {
85
      rate = other.rate;
86
87
      return *this;
    }
88
89
    
    template<typename Derived>
90
    void addTo(MotionDense<Derived> & other) const
91
    {
92
93
94
95
96
97
98
99
      other.linear() += rate;
    }
    
    template<typename Derived>
    void setTo(MotionDense<Derived> & other) const
    {
      other.linear() = rate;
      other.angular().setZero();
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
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
    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() = m.rotation() * rate; // TODO: check efficiency
    }
    
    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() = m.rotation().transpose() * rate;
      
      // 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(rate);
      
      // Angular
      mout.angular().setZero();
    }
    
    template<typename M1>
    MotionPlain motionAction(const MotionDense<M1> & v) const
    {
      MotionPlain res;
      motionAction(v,res);
      return res;
    }
    
153
    // data
154
    Vector3 rate;
155
156
    
  }; // struct MotionTranslationTpl
157
  
158
  template<typename S1, int O1, typename MotionDerived>
159
160
161
  inline typename MotionDerived::MotionPlain
  operator+(const MotionTranslationTpl<S1,O1> & m1,
            const MotionDense<MotionDerived> & m2)
162
  {
163
    return typename MotionDerived::MotionPlain(m2.linear() + m1.rate, m2.angular());
164
  }
165
  
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
  template<typename Scalar, int Options> struct TransformTranslationTpl;
  
  template<typename _Scalar, int _Options>
  struct traits< TransformTranslationTpl<_Scalar,_Options> >
  {
    enum {
      Options = _Options,
      LINEAR = 0,
      ANGULAR = 3
    };
    typedef _Scalar Scalar;
    typedef SE3Tpl<Scalar,Options> PlainType;
    typedef Eigen::Matrix<Scalar,3,1,Options> Vector3;
    typedef Eigen::Matrix<Scalar,3,3,Options> Matrix3;
    typedef typename Matrix3::IdentityReturnType AngularType;
    typedef AngularType AngularRef;
    typedef AngularType ConstAngularRef;
    typedef Vector3 LinearType;
    typedef LinearType & LinearRef;
    typedef const LinearType & ConstLinearRef;
    typedef typename traits<PlainType>::ActionMatrixType ActionMatrixType;
    typedef typename traits<PlainType>::HomogeneousMatrixType HomogeneousMatrixType;
  }; // traits TransformTranslationTpl
  
190
191
192
193
  template<typename Scalar, int Options>
  struct SE3GroupAction< TransformTranslationTpl<Scalar,Options> >
  { typedef typename traits <TransformTranslationTpl<Scalar,Options> >::PlainType ReturnType; };

194
195
196
197
198
  template<typename _Scalar, int _Options>
  struct TransformTranslationTpl
  : SE3Base< TransformTranslationTpl<_Scalar,_Options> >
  {
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
gabrielebndn's avatar
gabrielebndn committed
199
    PINOCCHIO_SE3_TYPEDEF_TPL(TransformTranslationTpl);
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
    typedef typename traits<TransformTranslationTpl>::PlainType PlainType;
    
    TransformTranslationTpl() {}
    
    template<typename Vector3Like>
    TransformTranslationTpl(const Eigen::MatrixBase<Vector3Like> & translation)
    : m_translation(translation)
    {}
    
    PlainType plain() const
    {
      PlainType res(PlainType::Identity());
      res.rotation().setIdentity();
      res.translation() = translation();
      
      return res;
    }
    
    operator PlainType() const { return plain(); }
    
    template<typename S2, int O2>
221
    typename SE3GroupAction<TransformTranslationTpl>::ReturnType
222
223
    se3action(const SE3Tpl<S2,O2> & m) const
    {
224
      typedef typename SE3GroupAction<TransformTranslationTpl>::ReturnType ReturnType;
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
      ReturnType res(m);
      res.translation() += translation();
      
      return res;
    }
    
    ConstLinearRef translation() const { return m_translation; }
    LinearRef translation() { return m_translation; }
    
    AngularType rotation() const { return AngularType(3,3); }
    
  protected:
    
    LinearType m_translation;
  };
  
241
242
243
244
  template<typename Scalar, int Options> struct ConstraintTranslationTpl;
  
  template<typename _Scalar, int _Options>
  struct traits< ConstraintTranslationTpl<_Scalar,_Options> >
245
  {
246
    typedef _Scalar Scalar;
247
    
248
    enum { Options = _Options };
249
250
    enum { LINEAR = 0, ANGULAR = 3 };
    
251
    typedef MotionTranslationTpl<Scalar,Options> JointMotion;
252
253
    typedef Eigen::Matrix<Scalar,3,1,Options> JointForce;
    typedef Eigen::Matrix<Scalar,6,3,Options> DenseBase;
254
    
255
256
    typedef DenseBase MatrixReturnType;
    typedef const DenseBase ConstMatrixReturnType;
257
  }; // traits ConstraintTranslationTpl
258
  
259
  template<typename _Scalar, int _Options>
260
261
  struct ConstraintTranslationTpl
  : ConstraintBase< ConstraintTranslationTpl<_Scalar,_Options> >
262
  {
263
264
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
    
265
    PINOCCHIO_CONSTRAINT_TYPEDEF_TPL(ConstraintTranslationTpl)
266
    
267
    enum { NV = 3 };
268
269
    
    ConstraintTranslationTpl() {}
270
    
271
272
273
//    template<typename S1, int O1>
//    Motion operator*(const MotionTranslationTpl<S1,O1> & vj) const
//    { return Motion(vj(), Motion::Vector3::Zero()); }
274
    
275
    template<typename Vector3Like>
276
    JointMotion __mult__(const Eigen::MatrixBase<Vector3Like> & v) const
277
278
    {
      EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Vector3Like,3);
279
      return JointMotion(v);
280
281
    }
    
282
    int nv_impl() const { return NV; }
283
    
284
285
    struct ConstraintTranspose
    {
286
287
      const ConstraintTranslationTpl & ref;
      ConstraintTranspose(const ConstraintTranslationTpl & ref) : ref(ref) {}
288
      
289
290
291
      template<typename Derived>
      typename ForceDense<Derived>::ConstLinearType
      operator* (const ForceDense<Derived> & phi)
292
      {
293
        return phi.linear();
294
      }
295
      
296
      /* [CRBA]  MatrixBase operator* (Constraint::Transpose S, ForceSet::Block) */
297
298
299
      template<typename MatrixDerived>
      const typename SizeDepType<3>::RowsReturn<MatrixDerived>::ConstType
      operator*(const Eigen::MatrixBase<MatrixDerived> & F) const
300
301
      {
        assert(F.rows()==6);
302
        return F.derived().template middleRows<3>(LINEAR);
303
      }
304
      
305
    }; // struct ConstraintTranspose
306
    
307
    ConstraintTranspose transpose () const { return ConstraintTranspose(*this); }
308
    
309
    DenseBase matrix_impl() const
310
    {
311
      DenseBase S;
312
313
      S.template middleRows<3>(LINEAR).setIdentity();
      S.template middleRows<3>(ANGULAR).setZero();
314
      return S;
315
    }
316
    
317
318
    template<typename S1, int O1>
    Eigen::Matrix<S1,6,3,O1> se3Action(const SE3Tpl<S1,O1> & m) const
319
    {
320
      Eigen::Matrix<S1,6,3,O1> M;
321
322
323
324
325
326
327
328
329
330
331
332
      M.template middleRows<3>(LINEAR) = m.rotation();
      M.template middleRows<3>(ANGULAR).setZero();
      
      return M;
    }
    
    template<typename S1, int O1>
    Eigen::Matrix<S1,6,3,O1> se3ActionInverse(const SE3Tpl<S1,O1> & m) const
    {
      Eigen::Matrix<S1,6,3,O1> M;
      M.template middleRows<3>(LINEAR) = m.rotation().transpose();
      M.template middleRows<3>(ANGULAR).setZero();
333
      
334
335
      return M;
    }
336
    
337
338
    template<typename MotionDerived>
    DenseBase motionAction(const MotionDense<MotionDerived> & m) const
339
    {
340
      const typename MotionDerived::ConstAngularType w = m.angular();
341
342
      
      DenseBase res;
343
344
      skew(w,res.template middleRows<3>(LINEAR));
      res.template middleRows<3>(ANGULAR).setZero();
345
346
347
      
      return res;
    }
348
    
349
  }; // struct ConstraintTranslationTpl
350
  
351
352
353
354
  template<typename MotionDerived, typename S2, int O2>
  inline typename MotionDerived::MotionPlain
  operator^(const MotionDense<MotionDerived> & m1,
            const MotionTranslationTpl<S2,O2> & m2)
355
  {
356
    return m2.motionAction(m1);
357
  }
358
  
359
  /* [CRBA] ForceSet operator* (Inertia Y,Constraint S) */
360
361
362
363
  template<typename S1, int O1, typename S2, int O2>
  inline Eigen::Matrix<S2,6,3,O2>
  operator*(const InertiaTpl<S1,O1> & Y,
            const ConstraintTranslationTpl<S2,O2> &)
364
  {
365
366
367
368
369
    typedef ConstraintTranslationTpl<S2,O2> Constraint;
    Eigen::Matrix<S2,6,3,O2> M;
    alphaSkew(Y.mass(),Y.lever(),M.template middleRows<3>(Constraint::ANGULAR));
    M.template middleRows<3>(Constraint::LINEAR).setZero();
    M.template middleRows<3>(Constraint::LINEAR).diagonal().fill(Y.mass ());
370
    
371
372
    return M;
  }
373
  
374
375
376
377
378
379
380
381
382
383
  /* [ABA] Y*S operator*/
  template<typename M6Like, typename S2, int O2>
  inline const typename SizeDepType<3>::ColsReturn<M6Like>::ConstType
  operator*(const Eigen::MatrixBase<M6Like> & Y,
            const ConstraintTranslationTpl<S2,O2> &)
  {
    typedef ConstraintTranslationTpl<S2,O2> Constraint;
    return Y.derived().template middleCols<3>(Constraint::LINEAR);
  }
  
384
385
386
  template<typename S1, int O1>
  struct SE3GroupAction< ConstraintTranslationTpl<S1,O1> >
  { typedef Eigen::Matrix<S1,6,3,O1> ReturnType; };
387
  
388
389
390
391
  template<typename S1, int O1, typename MotionDerived>
  struct MotionAlgebraAction< ConstraintTranslationTpl<S1,O1>,MotionDerived >
  { typedef Eigen::Matrix<S1,6,3,O1> ReturnType; };

392
393
394
395
  template<typename Scalar, int Options> struct JointTranslationTpl;
  
  template<typename _Scalar, int _Options>
  struct traits< JointTranslationTpl<_Scalar,_Options> >
396
  {
397
398
399
400
    enum {
      NQ = 3,
      NV = 3
    };
401
402
403
404
    typedef _Scalar Scalar;
    enum { Options = _Options };
    typedef JointDataTranslationTpl<Scalar,Options> JointDataDerived;
    typedef JointModelTranslationTpl<Scalar,Options> JointModelDerived;
405
    typedef ConstraintTranslationTpl<Scalar,Options> Constraint_t;
406
    typedef TransformTranslationTpl<Scalar,Options> Transformation_t;
407
    typedef MotionTranslationTpl<Scalar,Options> Motion_t;
408
    typedef MotionZeroTpl<Scalar,Options> Bias_t;
Justin Carpentier's avatar
Justin Carpentier committed
409

410
    // [ABA]
411
412
413
    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;
414
    
Gabriele Buondonno's avatar
Gabriele Buondonno committed
415
    PINOCCHIO_JOINT_DATA_BASE_ACCESSOR_DEFAULT_RETURN_TYPE
416

417
418
419
420
421
422
423
    typedef Eigen::Matrix<Scalar,NQ,1,Options> ConfigVector_t;
    typedef Eigen::Matrix<Scalar,NV,1,Options> TangentVector_t;
  }; // traits JointTranslationTpl
  
  template<typename Scalar, int Options>
  struct traits< JointDataTranslationTpl<Scalar,Options> >
  { typedef JointTranslationTpl<Scalar,Options> JointDerived; };
424
  
425
426
427
  template<typename Scalar, int Options>
  struct traits< JointModelTranslationTpl<Scalar,Options> >
  { typedef JointTranslationTpl<Scalar,Options> JointDerived; };
428
  
429
  template<typename _Scalar, int _Options>
430
431
  struct JointDataTranslationTpl
  : public JointDataBase< JointDataTranslationTpl<_Scalar,_Options> >
432
  {
433
434
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
    
435
    typedef JointTranslationTpl<_Scalar,_Options> JointDerived;
436
    PINOCCHIO_JOINT_DATA_TYPEDEF_TEMPLATE(JointDerived);
Gabriele Buondonno's avatar
Gabriele Buondonno committed
437
    PINOCCHIO_JOINT_DATA_BASE_DEFAULT_ACCESSOR
438
439
440
441
442
443

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

444
445
446
447
    // [ABA] specific data
    U_t U;
    D_t Dinv;
    UD_t UDinv;
448

449
    JointDataTranslationTpl() {}
450

451
452
    static std::string classname() { return std::string("JointDataTranslation"); }
    std::string shortname() const { return classname(); }
453
  }; // struct JointDataTranslationTpl
454

Gabriele Buondonno's avatar
Gabriele Buondonno committed
455
  PINOCCHIO_JOINT_CAST_TYPE_SPECIALIZATION(JointModelTranslationTpl);
456
  template<typename _Scalar, int _Options>
jcarpent's avatar
jcarpent committed
457
458
  struct JointModelTranslationTpl
  : public JointModelBase< JointModelTranslationTpl<_Scalar,_Options> >
459
  {
460
461
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
    
462
    typedef JointTranslationTpl<_Scalar,_Options> JointDerived;
463
    PINOCCHIO_JOINT_TYPEDEF_TEMPLATE(JointDerived);
jcarpent's avatar
jcarpent committed
464
465
466
467
468
469
    
    typedef JointModelBase<JointModelTranslationTpl> Base;
    using Base::id;
    using Base::idx_q;
    using Base::idx_v;
    using Base::setIndexes;
470

471
    JointDataDerived createData() const { return JointDataDerived(); }
472

473
474
475
    template<typename ConfigVector>
    void calc(JointDataDerived & data,
              const typename Eigen::MatrixBase<ConfigVector> & qs) const
476
    {
477
      data.M.translation() = this->jointConfigSelector(qs);
478
    }
479
480
481
482
483
    
    template<typename ConfigVector, typename TangentVector>
    void calc(JointDataDerived & data,
              const typename Eigen::MatrixBase<ConfigVector> & qs,
              const typename Eigen::MatrixBase<TangentVector> & vs) const
484
    {
485
486
      calc(data,qs.derived());
      
487
      data.v() = this->jointVelocitySelector(vs);
488
    }
489
    
490
    template<typename Matrix6Like>
491
492
493
    void calc_aba(JointDataDerived & data,
                  const Eigen::MatrixBase<Matrix6Like> & I,
                  const bool update_I) const
494
    {
495
      data.U = I.template middleCols<3>(Inertia::LINEAR);
496
497
      
      // compute inverse
498
499
500
//      data.Dinv.setIdentity();
//      data.U.template middleRows<3>(Inertia::LINEAR).llt().solveInPlace(data.Dinv);
      internal::PerformStYSInversion<Scalar>::run(data.U.template middleRows<3>(Inertia::LINEAR),data.Dinv);
501
      
502
503
      data.UDinv.template middleRows<3>(Inertia::LINEAR).setIdentity(); // can be put in data constructor
      data.UDinv.template middleRows<3>(Inertia::ANGULAR).noalias() = data.U.template middleRows<3>(Inertia::ANGULAR) * data.Dinv;
504
505
506
      
      if (update_I)
      {
gabrielebndn's avatar
gabrielebndn committed
507
        Matrix6Like & I_ = PINOCCHIO_EIGEN_CONST_CAST(Matrix6Like,I);
508
509
510
511
        I_.template block<3,3>(Inertia::ANGULAR,Inertia::ANGULAR)
        -= data.UDinv.template middleRows<3>(Inertia::ANGULAR) * I_.template block<3,3>(Inertia::LINEAR, Inertia::ANGULAR);
        I_.template middleCols<3>(Inertia::LINEAR).setZero();
        I_.template block<3,3>(Inertia::LINEAR,Inertia::ANGULAR).setZero();
512
513
      }
    }
514
    
515
516
    static std::string classname() { return std::string("JointModelTranslation"); }
    std::string shortname() const { return classname(); }
517
518
519
520
521
522
523
524
525
526
    
    /// \returns An expression of *this with the Scalar type casted to NewScalar.
    template<typename NewScalar>
    JointModelTranslationTpl<NewScalar,Options> cast() const
    {
      typedef JointModelTranslationTpl<NewScalar,Options> ReturnType;
      ReturnType res;
      res.setIndexes(id(),idx_q(),idx_v());
      return res;
    }
527

528
  }; // struct JointModelTranslationTpl
529
  
530
} // namespace pinocchio
531

532
533
534
535
536
#include <boost/type_traits.hpp>

namespace boost
{
  template<typename Scalar, int Options>
537
  struct has_nothrow_constructor< ::pinocchio::JointModelTranslationTpl<Scalar,Options> >
538
539
540
  : public integral_constant<bool,true> {};
  
  template<typename Scalar, int Options>
541
  struct has_nothrow_copy< ::pinocchio::JointModelTranslationTpl<Scalar,Options> >
542
543
544
  : public integral_constant<bool,true> {};
  
  template<typename Scalar, int Options>
545
  struct has_nothrow_constructor< ::pinocchio::JointDataTranslationTpl<Scalar,Options> >
546
547
548
  : public integral_constant<bool,true> {};
  
  template<typename Scalar, int Options>
549
  struct has_nothrow_copy< ::pinocchio::JointDataTranslationTpl<Scalar,Options> >
550
551
552
  : public integral_constant<bool,true> {};
}

553
#endif // ifndef __pinocchio_joint_translation_hpp__