joint-spherical.hpp 17.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_spherical_hpp__
#define __pinocchio_joint_spherical_hpp__
8

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

16
namespace pinocchio
17
18
{

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

34
  template<typename _Scalar, int _Options>
35
  struct traits< MotionSphericalTpl<_Scalar,_Options> >
36
  {
37
38
39
40
41
    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
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;
51
52
53
54
    enum {
      LINEAR = 0,
      ANGULAR = 3
    };
55
  }; // traits MotionSphericalTpl
56

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

65
    MotionSphericalTpl() {}
66
67
68
69
70
    
    template<typename Vector3Like>
    MotionSphericalTpl(const Eigen::MatrixBase<Vector3Like> & w)
    : w(w)
    {}
71

72
73
    Vector3 & operator() () { return w; }
    const Vector3 & operator() () const { return w; }
74

75
76
77
78
    inline PlainReturnType plain() const
    {
      return PlainReturnType(PlainReturnType::Vector3::Zero(), w);
    }
79
80
81
    
    template<typename MotionDerived>
    void addTo(MotionDense<MotionDerived> & other) const
82
    {
83
      other.angular() += w;
84
    }
85
    
86
87
    template<typename Derived>
    void setTo(MotionDense<Derived> & other) const
88
    {
89
90
      other.linear().setZero();
      other.angular() = w;
91
    }
92
    
93
94
95
96
97
    MotionSphericalTpl __plus__(const MotionSphericalTpl & other) const
    {
      return MotionSphericalTpl(w + other.w);
    }
    
98
99
100
101
102
103
104
105
106
107
108
    bool isEqual_impl(const MotionSphericalTpl & other) const
    {
      return w == other.w;
    }
    
    template<typename MotionDerived>
    bool isEqual_impl(const MotionDense<MotionDerived> & other) const
    {
      return other.angular() == w && other.linear().isZero(0);
    }
    
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
153
154
155
156
157
158
159
160
161
162
163
164
165
166
    template<typename S2, int O2, typename D2>
    void se3Action_impl(const SE3Tpl<S2,O2> & m, MotionDense<D2> & v) const
    {
      // Angular
      v.angular().noalias() = m.rotation() * w;
      
      // Linear
      v.linear().noalias() = m.translation().cross(v.angular());
    }
    
    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
      // TODO: use v.angular() as temporary variable
      Vector3 v3_tmp;
      v3_tmp.noalias() = w.cross(m.translation());
      v.linear().noalias() = m.rotation().transpose() * v3_tmp;
      
      // Angular
      v.angular().noalias() = m.rotation().transpose() * w;
    }
    
    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.linear().cross(w);
      
      // Angular
      mout.angular().noalias() = v.angular().cross(w);
    }
    
    template<typename M1>
    MotionPlain motionAction(const MotionDense<M1> & v) const
    {
      MotionPlain res;
      motionAction(v,res);
      return res;
    }
   
    // data
167
    Vector3 w;
168
  }; // struct MotionSphericalTpl
169

170
171
  template<typename S1, int O1, typename MotionDerived>
  inline typename MotionDerived::MotionPlain
172
  operator+(const MotionSphericalTpl<S1,O1> & m1, const MotionDense<MotionDerived> & m2)
173
  {
174
    return typename MotionDerived::MotionPlain(m2.linear(),m2.angular() + m1.w);
175
  }
176

177
  template<typename Scalar, int Options> struct ConstraintSphericalTpl;
178
179
  
  template<typename _Scalar, int _Options>
180
  struct traits< ConstraintSphericalTpl<_Scalar,_Options> >
181
  {
182
183
    typedef _Scalar Scalar;
    enum { Options = _Options };
184
185
186
187
    enum {
      LINEAR = 0,
      ANGULAR = 3
    };
188
    typedef MotionSphericalTpl<Scalar,Options> JointMotion;
189
190
    typedef Eigen::Matrix<Scalar,3,1,Options> JointForce;
    typedef Eigen::Matrix<Scalar,6,3,Options> DenseBase;
191
192
    typedef DenseBase MatrixReturnType;
    typedef const DenseBase ConstMatrixReturnType;
193
  }; // struct traits struct ConstraintSphericalTpl
194

195
  template<typename _Scalar, int _Options>
196
197
  struct ConstraintSphericalTpl
  : public ConstraintBase< ConstraintSphericalTpl<_Scalar,_Options> >
198
  {
199
200
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
    
201
    PINOCCHIO_CONSTRAINT_TYPEDEF_TPL(ConstraintSphericalTpl)
202
    
203
204
    ConstraintSphericalTpl() {}
    
205
    enum { NV = 3 };
206
    
207
208
    int nv_impl() const { return NV; }
    
209
210
211
212
213
214
215
    template<typename Vector3Like>
    JointMotion __mult__(const Eigen::MatrixBase<Vector3Like> & w) const
    {
      EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Vector3Like,3);
      return JointMotion(w);
    }
    
216
    struct TransposeConst
217
    {
218
219
220
221
      template<typename Derived>
      typename ForceDense<Derived>::ConstAngularType
      operator* (const ForceDense<Derived> & phi)
      {  return phi.angular();  }
222

223
      /* [CRBA]  MatrixBase operator* (Constraint::Transpose S, ForceSet::Block) */
224
225
      template<typename MatrixDerived>
      const typename SizeDepType<3>::RowsReturn<MatrixDerived>::ConstType
jcarpent's avatar
jcarpent committed
226
      operator*(const Eigen::MatrixBase<MatrixDerived> & F) const
227
      {
228
        assert(F.rows()==6);
229
        return F.derived().template middleRows<3>(Inertia::ANGULAR);
230
      }
231
    };
232

233
    TransposeConst transpose() const { return TransposeConst(); }
234

235
    DenseBase matrix_impl() const
236
    {
237
      DenseBase S;
238
239
      S.template block <3,3>(LINEAR,0).setZero();
      S.template block <3,3>(ANGULAR,0).setIdentity();
240
      return S;
241
    }
242

243
244
    template<typename S1, int O1>
    Eigen::Matrix<S1,6,3,O1> se3Action(const SE3Tpl<S1,O1> & m) const
245
    {
246
      Eigen::Matrix<S1,6,3,O1> X_subspace;
247
248
      cross(m.translation(),m.rotation(),X_subspace.template middleRows<3>(LINEAR));
      X_subspace.template middleRows<3>(ANGULAR) = m.rotation();
249
250

      return X_subspace;
251
    }
252
    
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
    template<typename S1, int O1>
    Eigen::Matrix<S1,6,3,O1> se3ActionInverse(const SE3Tpl<S1,O1> & m) const
    {
      Eigen::Matrix<S1,6,3,O1> X_subspace;
      AxisX::cross(m.translation(),X_subspace.template middleRows<3>(ANGULAR).col(0));
      AxisY::cross(m.translation(),X_subspace.template middleRows<3>(ANGULAR).col(1));
      AxisZ::cross(m.translation(),X_subspace.template middleRows<3>(ANGULAR).col(2));
      
      X_subspace.template middleRows<3>(LINEAR).noalias()
      = m.rotation().transpose() * X_subspace.template middleRows<3>(ANGULAR);
      X_subspace.template middleRows<3>(ANGULAR) = m.rotation().transpose();
      
      return X_subspace;
    }
    
268
269
    template<typename MotionDerived>
    DenseBase motionAction(const MotionDense<MotionDerived> & m) const
270
    {
271
272
      const typename MotionDerived::ConstLinearType v = m.linear();
      const typename MotionDerived::ConstAngularType w = m.angular();
273
274
      
      DenseBase res;
275
276
      skew(v,res.template middleRows<3>(LINEAR));
      skew(w,res.template middleRows<3>(ANGULAR));
277
278
279
      
      return res;
    }
280

281
  }; // struct ConstraintSphericalTpl
282

283
284
285
  template<typename MotionDerived, typename S2, int O2>
  inline typename MotionDerived::MotionPlain
  operator^(const MotionDense<MotionDerived> & m1,
286
            const MotionSphericalTpl<S2,O2> & m2)
287
  {
288
    return m2.motionAction(m1);
289
290
291
  }

  /* [CRBA] ForceSet operator* (Inertia Y,Constraint S) */
292
293
294
  template<typename S1, int O1, typename S2, int O2>
  inline Eigen::Matrix<S2,6,3,O2>
  operator*(const InertiaTpl<S1,O1> & Y,
295
            const ConstraintSphericalTpl<S2,O2> &)
296
  {
297
    typedef InertiaTpl<S1,O1> Inertia;
Justin Carpentier's avatar
Justin Carpentier committed
298
    typedef typename Inertia::Symmetric3 Symmetric3;
299
    Eigen::Matrix<S2,6,3,O2> M;
300
    //    M.block <3,3> (Inertia::LINEAR, 0) = - Y.mass () * skew(Y.lever ());
301
    M.template block<3,3>(Inertia::LINEAR,0) = alphaSkew(-Y.mass(), Y.lever());
Justin Carpentier's avatar
Justin Carpentier committed
302
    M.template block<3,3>(Inertia::ANGULAR,0) = (Y.inertia() - typename Symmetric3::AlphaSkewSquare(Y.mass(), Y.lever())).matrix();
303
304
305
    return M;
  }

306
  /* [ABA] Y*S operator*/
307
  template<typename M6Like, typename S2, int O2>
308
  inline typename SizeDepType<3>::ColsReturn<M6Like>::ConstType
309
  operator*(const Eigen::MatrixBase<M6Like> & Y,
310
            const ConstraintSphericalTpl<S2,O2> &)
311
  {
312
    typedef ConstraintSphericalTpl<S2,O2> Constraint;
jcarpent's avatar
jcarpent committed
313
    return Y.derived().template middleCols<3>(Constraint::ANGULAR);
314
315
  }
  
316
317
318
319
320
321
322
  template<typename S1, int O1>
  struct SE3GroupAction< ConstraintSphericalTpl<S1,O1> >
  { typedef Eigen::Matrix<S1,6,3,O1> ReturnType; };
  
  template<typename S1, int O1, typename MotionDerived>
  struct MotionAlgebraAction< ConstraintSphericalTpl<S1,O1>,MotionDerived >
  { typedef Eigen::Matrix<S1,6,3,O1> ReturnType; };
323

324
325
326
327
  template<typename Scalar, int Options> struct JointSphericalTpl;
  
  template<typename _Scalar, int _Options>
  struct traits< JointSphericalTpl<_Scalar,_Options> >
328
  {
329
330
331
332
    enum {
      NQ = 4,
      NV = 3
    };
333
334
335
336
    typedef _Scalar Scalar;
    enum { Options = _Options };
    typedef JointDataSphericalTpl<Scalar,Options> JointDataDerived;
    typedef JointModelSphericalTpl<Scalar,Options> JointModelDerived;
337
    typedef ConstraintSphericalTpl<Scalar,Options> Constraint_t;
338
    typedef SE3Tpl<Scalar,Options> Transformation_t;
339
    typedef MotionSphericalTpl<Scalar,Options> Motion_t;
340
    typedef MotionZeroTpl<Scalar,Options> Bias_t;
Justin Carpentier's avatar
Justin Carpentier committed
341

342
    // [ABA]
343
344
345
    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;
346
    
Gabriele Buondonno's avatar
Gabriele Buondonno committed
347
    PINOCCHIO_JOINT_DATA_BASE_ACCESSOR_DEFAULT_RETURN_TYPE
348

349
350
    typedef Eigen::Matrix<Scalar,NQ,1,Options> ConfigVector_t;
    typedef Eigen::Matrix<Scalar,NV,1,Options> TangentVector_t;
351
  };
352
353
354
355
356
357
358
359
  
  template<typename Scalar, int Options>
  struct traits< JointDataSphericalTpl<Scalar,Options> >
  { typedef JointSphericalTpl<Scalar,Options> JointDerived; };
  
  template<typename Scalar, int Options>
  struct traits< JointModelSphericalTpl<Scalar,Options> >
  { typedef JointSphericalTpl<Scalar,Options> JointDerived; };
360

361
362
  template<typename _Scalar, int _Options>
  struct JointDataSphericalTpl : public JointDataBase< JointDataSphericalTpl<_Scalar,_Options> >
363
  {
364
365
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
    
366
    typedef JointSphericalTpl<_Scalar,_Options> JointDerived;
367
    PINOCCHIO_JOINT_DATA_TYPEDEF_TEMPLATE(JointDerived);
Gabriele Buondonno's avatar
Gabriele Buondonno committed
368
    PINOCCHIO_JOINT_DATA_BASE_DEFAULT_ACCESSOR
369
370
371
372
373
374

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

375
376
377
378
    // [ABA] specific data
    U_t U;
    D_t Dinv;
    UD_t UDinv;
379

380
    JointDataSphericalTpl () : M(1), U(), Dinv(), UDinv() {}
381

382
383
384
    static std::string classname() { return std::string("JointDataSpherical"); }
    std::string shortname() const { return classname(); }
    
385
  }; // struct JointDataSphericalTpl
386

Gabriele Buondonno's avatar
Gabriele Buondonno committed
387
  PINOCCHIO_JOINT_CAST_TYPE_SPECIALIZATION(JointModelSphericalTpl);
388
  template<typename _Scalar, int _Options>
jcarpent's avatar
jcarpent committed
389
390
  struct JointModelSphericalTpl
  : public JointModelBase< JointModelSphericalTpl<_Scalar,_Options> >
391
  {
392
393
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
    
394
    typedef JointSphericalTpl<_Scalar,_Options> JointDerived;
395
    PINOCCHIO_JOINT_TYPEDEF_TEMPLATE(JointDerived);
jcarpent's avatar
jcarpent committed
396
397
398
399
400
401
    
    typedef JointModelBase<JointModelSphericalTpl> Base;
    using Base::id;
    using Base::idx_q;
    using Base::idx_v;
    using Base::setIndexes;
402

403
    JointDataDerived createData() const { return JointDataDerived(); }
404

405
406
    template<typename ConfigVectorLike>
    inline void forwardKinematics(Transformation_t & M, const Eigen::MatrixBase<ConfigVectorLike> & q_joint) const
407
    {
408
      EIGEN_STATIC_ASSERT_SAME_VECTOR_SIZE(ConfigVector_t,ConfigVectorLike);
gabrielebndn's avatar
gabrielebndn committed
409
      typedef typename Eigen::Quaternion<typename ConfigVectorLike::Scalar,PINOCCHIO_EIGEN_PLAIN_TYPE(ConfigVectorLike)::Options> Quaternion;
410
      typedef Eigen::Map<const Quaternion> ConstQuaternionMap;
411

412
      ConstQuaternionMap quat(q_joint.derived().data());
413
414
      //assert(math::fabs(quat.coeffs().squaredNorm()-1.) <= sqrt(Eigen::NumTraits<typename V::Scalar>::epsilon())); TODO: check validity of the rhs precision
      assert(math::fabs(quat.coeffs().squaredNorm()-1.) <= 1e-4);
415
      
416
417
      M.rotation(quat.matrix());
      M.translation().setZero();
418
    }
419
420
421
422
423
424
425
426
    
    template<typename QuaternionDerived>
    void calc(JointDataDerived & data,
              const typename Eigen::QuaternionBase<QuaternionDerived> & quat) const
    {
      data.M.rotation(quat.matrix());
    }
    
427
    template<typename ConfigVector>
428
    EIGEN_DONT_INLINE
429
    void calc(JointDataDerived & data,
430
              const typename Eigen::PlainObjectBase<ConfigVector> & qs) const
431
    {
432
433
      typedef typename Eigen::Quaternion<typename ConfigVector::Scalar,ConfigVector::Options> Quaternion;
      typedef Eigen::Map<const Quaternion> ConstQuaternionMap;
434
435
436
437
438
439
440
441
442
443
444
445
446
447

      ConstQuaternionMap quat(qs.template segment<NQ>(idx_q()).data());
      calc(data,quat);
    }

    template<typename ConfigVector>
    EIGEN_DONT_INLINE
    void calc(JointDataDerived & data,
              const typename Eigen::MatrixBase<ConfigVector> & qs) const
    {
      typedef typename Eigen::Quaternion<Scalar,Options> Quaternion;

      const Quaternion quat(qs.template segment<NQ>(idx_q()));
      calc(data,quat);
448
449
    }

450
451
452
453
    template<typename ConfigVector, typename TangentVector>
    void calc(JointDataDerived & data,
              const typename Eigen::MatrixBase<ConfigVector> & qs,
              const typename Eigen::MatrixBase<TangentVector> & vs) const
454
    {
455
      calc(data,qs.derived());
456
      
457
      data.v() = vs.template segment<NV>(idx_v());
458
    }
459
    
460
    template<typename Matrix6Like>
461
462
463
    void calc_aba(JointDataDerived & data,
                  const Eigen::MatrixBase<Matrix6Like> & I,
                  const bool update_I) const
464
    {
465
      data.U = I.template block<6,3>(0,Inertia::ANGULAR);
466
467
      
      // compute inverse
468
469
470
//      data.Dinv.setIdentity();
//      data.U.template middleRows<3>(Inertia::ANGULAR).llt().solveInPlace(data.Dinv);
      internal::PerformStYSInversion<Scalar>::run(data.U.template middleRows<3>(Inertia::ANGULAR),data.Dinv);
471
      
472
      data.UDinv.template middleRows<3>(Inertia::ANGULAR).setIdentity(); // can be put in data constructor
473
      data.UDinv.template middleRows<3>(Inertia::LINEAR).noalias() = data.U.template block<3,3>(Inertia::LINEAR, 0) * data.Dinv;
474
475
476
      
      if (update_I)
      {
gabrielebndn's avatar
gabrielebndn committed
477
        Matrix6Like & I_ = PINOCCHIO_EIGEN_CONST_CAST(Matrix6Like,I);
478
479
480
481
        I_.template block<3,3>(Inertia::LINEAR,Inertia::LINEAR)
        -= data.UDinv.template middleRows<3>(Inertia::LINEAR) * I_.template block<3,3> (Inertia::ANGULAR, Inertia::LINEAR);
        I_.template block<6,3>(0,Inertia::ANGULAR).setZero();
        I_.template block<3,3>(Inertia::ANGULAR,Inertia::LINEAR).setZero();
482
483
      }
    }
484
    
485
486
    static std::string classname() { return std::string("JointModelSpherical"); }
    std::string shortname() const { return classname(); }
487
488
489
490
491
492
493
494
495
496
497
    
    /// \returns An expression of *this with the Scalar type casted to NewScalar.
    template<typename NewScalar>
    JointModelSphericalTpl<NewScalar,Options> cast() const
    {
      typedef JointModelSphericalTpl<NewScalar,Options> ReturnType;
      ReturnType res;
      res.setIndexes(id(),idx_q(),idx_v());
      return res;
    }

498

499
  }; // struct JointModelSphericalTpl
500

501
} // namespace pinocchio
502

503
504
505
506
507
#include <boost/type_traits.hpp>

namespace boost
{
  template<typename Scalar, int Options>
508
  struct has_nothrow_constructor< ::pinocchio::JointModelSphericalTpl<Scalar,Options> >
509
510
511
  : public integral_constant<bool,true> {};
  
  template<typename Scalar, int Options>
512
  struct has_nothrow_copy< ::pinocchio::JointModelSphericalTpl<Scalar,Options> >
513
514
515
  : public integral_constant<bool,true> {};
  
  template<typename Scalar, int Options>
516
  struct has_nothrow_constructor< ::pinocchio::JointDataSphericalTpl<Scalar,Options> >
517
518
519
  : public integral_constant<bool,true> {};
  
  template<typename Scalar, int Options>
520
  struct has_nothrow_copy< ::pinocchio::JointDataSphericalTpl<Scalar,Options> >
521
522
523
  : public integral_constant<bool,true> {};
}

524
#endif // ifndef __pinocchio_joint_spherical_hpp__