pinocchio  2.7.1
A fast and flexible implementation of Rigid Body Dynamics algorithms and their analytical derivatives
joint-revolute.hpp
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_revolute_hpp__
7 #define __pinocchio_joint_revolute_hpp__
8 
9 #include "pinocchio/math/sincos.hpp"
10 #include "pinocchio/spatial/inertia.hpp"
11 #include "pinocchio/multibody/constraint.hpp"
12 #include "pinocchio/multibody/joint/joint-base.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 MotionRevoluteTpl;
20 
21  template<typename Scalar, int Options, int axis>
22  struct SE3GroupAction< MotionRevoluteTpl<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< MotionRevoluteTpl<Scalar,Options,axis>, MotionDerived>
29  {
30  typedef MotionTpl<Scalar,Options> ReturnType;
31  };
32 
33  template<typename _Scalar, int _Options, int axis>
34  struct traits< MotionRevoluteTpl<_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  }; // traits MotionRevoluteTpl
57 
58  template<typename Scalar, int Options, int axis> struct TransformRevoluteTpl;
59 
60  template<typename _Scalar, int _Options, int _axis>
61  struct traits< TransformRevoluteTpl<_Scalar,_Options,_axis> >
62  {
63  enum {
64  axis = _axis,
65  Options = _Options,
66  LINEAR = 0,
67  ANGULAR = 3
68  };
69  typedef _Scalar Scalar;
70  typedef SE3Tpl<Scalar,Options> PlainType;
71  typedef Eigen::Matrix<Scalar,3,1,Options> Vector3;
72  typedef Eigen::Matrix<Scalar,3,3,Options> Matrix3;
73  typedef Matrix3 AngularType;
74  typedef Matrix3 AngularRef;
75  typedef Matrix3 ConstAngularRef;
76  typedef typename Vector3::ConstantReturnType LinearType;
77  typedef typename Vector3::ConstantReturnType LinearRef;
78  typedef const typename Vector3::ConstantReturnType ConstLinearRef;
79  typedef typename traits<PlainType>::ActionMatrixType ActionMatrixType;
80  typedef typename traits<PlainType>::HomogeneousMatrixType HomogeneousMatrixType;
81  }; // traits TransformRevoluteTpl
82 
83  template<typename Scalar, int Options, int axis>
84  struct SE3GroupAction< TransformRevoluteTpl<Scalar,Options,axis> >
85  { typedef typename traits <TransformRevoluteTpl<Scalar,Options,axis> >::PlainType ReturnType; };
86 
87  template<typename _Scalar, int _Options, int axis>
88  struct TransformRevoluteTpl : SE3Base< TransformRevoluteTpl<_Scalar,_Options,axis> >
89  {
90  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
91  PINOCCHIO_SE3_TYPEDEF_TPL(TransformRevoluteTpl);
92 
93  TransformRevoluteTpl() {}
94  TransformRevoluteTpl(const Scalar & sin, const Scalar & cos)
95  : m_sin(sin), m_cos(cos)
96  {}
97 
98  PlainType plain() const
99  {
100  PlainType res(PlainType::Identity());
101  _setRotation (res.rotation());
102  return res;
103  }
104 
105  operator PlainType() const { return plain(); }
106 
107  template<typename S2, int O2>
108  typename SE3GroupAction<TransformRevoluteTpl>::ReturnType
109  se3action(const SE3Tpl<S2,O2> & m) const
110  {
111  typedef typename SE3GroupAction<TransformRevoluteTpl>::ReturnType ReturnType;
112  ReturnType res;
113  switch(axis)
114  {
115  case 0:
116  {
117  res.rotation().col(0) = m.rotation().col(0);
118  res.rotation().col(1).noalias() = m_cos * m.rotation().col(1) + m_sin * m.rotation().col(2);
119  res.rotation().col(2).noalias() = res.rotation().col(0).cross(res.rotation().col(1));
120  break;
121  }
122  case 1:
123  {
124  res.rotation().col(2).noalias() = m_cos * m.rotation().col(2) + m_sin * m.rotation().col(0);
125  res.rotation().col(1) = m.rotation().col(1);
126  res.rotation().col(0).noalias() = res.rotation().col(1).cross(res.rotation().col(2));
127  break;
128  }
129  case 2:
130  {
131  res.rotation().col(0).noalias() = m_cos * m.rotation().col(0) + m_sin * m.rotation().col(1);
132  res.rotation().col(1).noalias() = res.rotation().col(2).cross(res.rotation().col(0));
133  res.rotation().col(2) = m.rotation().col(2);
134  break;
135  }
136  default:
137  {
138  assert(false && "must nerver happened");
139  break;
140  }
141  }
142  res.translation() = m.translation();
143  return res;
144  }
145 
146  const Scalar & sin() const { return m_sin; }
147  Scalar & sin() { return m_sin; }
148 
149  const Scalar & cos() const { return m_cos; }
150  Scalar & cos() { return m_cos; }
151 
152  template<typename OtherScalar>
153  void setValues(const OtherScalar & sin, const OtherScalar & cos)
154  { m_sin = sin; m_cos = cos; }
155 
156  LinearType translation() const { return LinearType::PlainObject::Zero(3); };
157  AngularType rotation() const {
158  AngularType m(AngularType::Identity(3));
159  _setRotation (m);
160  return m;
161  }
162 
163  bool isEqual(const TransformRevoluteTpl & other) const
164  {
165  return m_cos == other.m_cos && m_sin == other.m_sin;
166  }
167 
168  protected:
169 
170  Scalar m_sin, m_cos;
171  inline void _setRotation (typename PlainType::AngularRef& rot) const
172  {
173  switch(axis)
174  {
175  case 0:
176  {
177  rot.coeffRef(1,1) = m_cos; rot.coeffRef(1,2) = -m_sin;
178  rot.coeffRef(2,1) = m_sin; rot.coeffRef(2,2) = m_cos;
179  break;
180  }
181  case 1:
182  {
183  rot.coeffRef(0,0) = m_cos; rot.coeffRef(0,2) = m_sin;
184  rot.coeffRef(2,0) = -m_sin; rot.coeffRef(2,2) = m_cos;
185  break;
186  }
187  case 2:
188  {
189  rot.coeffRef(0,0) = m_cos; rot.coeffRef(0,1) = -m_sin;
190  rot.coeffRef(1,0) = m_sin; rot.coeffRef(1,1) = m_cos;
191  break;
192  }
193  default:
194  {
195  assert(false && "must nerver happened");
196  break;
197  }
198  }
199  }
200  };
201 
202  template<typename _Scalar, int _Options, int axis>
203  struct MotionRevoluteTpl
204  : MotionBase< MotionRevoluteTpl<_Scalar,_Options,axis> >
205  {
206  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
207 
208  MOTION_TYPEDEF_TPL(MotionRevoluteTpl);
209  typedef SpatialAxis<axis+ANGULAR> Axis;
210  typedef typename Axis::CartesianAxis3 CartesianAxis3;
211 
212  MotionRevoluteTpl() {}
213 
214  MotionRevoluteTpl(const Scalar & w) : m_w(w) {}
215 
216  template<typename Vector1Like>
217  MotionRevoluteTpl(const Eigen::MatrixBase<Vector1Like> & v)
218  : m_w(v[0])
219  {
220  using namespace Eigen;
221  EIGEN_STATIC_ASSERT_SIZE_1x1(Vector1Like);
222  }
223 
224  inline PlainReturnType plain() const { return Axis() * m_w; }
225 
226  template<typename OtherScalar>
227  MotionRevoluteTpl __mult__(const OtherScalar & alpha) const
228  {
229  return MotionRevoluteTpl(alpha*m_w);
230  }
231 
232  template<typename MotionDerived>
233  void setTo(MotionDense<MotionDerived> & m) const
234  {
235  m.linear().setZero();
236  for(Eigen::DenseIndex k = 0; k < 3; ++k)
237  m.angular()[k] = k == axis ? m_w : (Scalar)0;
238  }
239 
240  template<typename MotionDerived>
241  inline void addTo(MotionDense<MotionDerived> & v) const
242  {
243  typedef typename MotionDense<MotionDerived>::Scalar OtherScalar;
244  v.angular()[axis] += (OtherScalar)m_w;
245  }
246 
247  template<typename S2, int O2, typename D2>
248  inline void se3Action_impl(const SE3Tpl<S2,O2> & m, MotionDense<D2> & v) const
249  {
250  v.angular().noalias() = m.rotation().col(axis) * m_w;
251  v.linear().noalias() = m.translation().cross(v.angular());
252  }
253 
254  template<typename S2, int O2>
255  MotionPlain se3Action_impl(const SE3Tpl<S2,O2> & m) const
256  {
257  MotionPlain res;
258  se3Action_impl(m,res);
259  return res;
260  }
261 
262  template<typename S2, int O2, typename D2>
263  void se3ActionInverse_impl(const SE3Tpl<S2,O2> & m,
264  MotionDense<D2> & v) const
265  {
266  // Linear
267  CartesianAxis3::alphaCross(m_w,m.translation(),v.angular());
268  v.linear().noalias() = m.rotation().transpose() * v.angular();
269 
270  // Angular
271  v.angular().noalias() = m.rotation().transpose().col(axis) * m_w;
272  }
273 
274  template<typename S2, int O2>
275  MotionPlain se3ActionInverse_impl(const SE3Tpl<S2,O2> & m) const
276  {
277  MotionPlain res;
278  se3ActionInverse_impl(m,res);
279  return res;
280  }
281 
282  template<typename M1, typename M2>
283  EIGEN_STRONG_INLINE
284  void motionAction(const MotionDense<M1> & v, MotionDense<M2> & mout) const
285  {
286  // Linear
287  CartesianAxis3::alphaCross(-m_w,v.linear(),mout.linear());
288 
289  // Angular
290  CartesianAxis3::alphaCross(-m_w,v.angular(),mout.angular());
291  }
292 
293  template<typename M1>
294  MotionPlain motionAction(const MotionDense<M1> & v) const
295  {
296  MotionPlain res;
297  motionAction(v,res);
298  return res;
299  }
300 
301  Scalar & angularRate() { return m_w; }
302  const Scalar & angularRate() const { return m_w; }
303 
304  bool isEqual_impl(const MotionRevoluteTpl & other) const
305  {
306  return m_w == other.m_w;
307  }
308 
309  protected:
310 
311  Scalar m_w;
312  }; // struct MotionRevoluteTpl
313 
314  template<typename S1, int O1, int axis, typename MotionDerived>
315  typename MotionDerived::MotionPlain
316  operator+(const MotionRevoluteTpl<S1,O1,axis> & m1,
317  const MotionDense<MotionDerived> & m2)
318  {
319  typename MotionDerived::MotionPlain res(m2);
320  res += m1;
321  return res;
322  }
323 
324  template<typename MotionDerived, typename S2, int O2, int axis>
325  EIGEN_STRONG_INLINE
326  typename MotionDerived::MotionPlain
327  operator^(const MotionDense<MotionDerived> & m1, const MotionRevoluteTpl<S2,O2,axis>& m2)
328  {
329  return m2.motionAction(m1);
330  }
331 
332  template<typename Scalar, int Options, int axis> struct ConstraintRevoluteTpl;
333 
334  template<typename Scalar, int Options, int axis>
335  struct SE3GroupAction< ConstraintRevoluteTpl<Scalar,Options,axis> >
336  { typedef Eigen::Matrix<Scalar,6,1,Options> ReturnType; };
337 
338  template<typename Scalar, int Options, int axis, typename MotionDerived>
339  struct MotionAlgebraAction< ConstraintRevoluteTpl<Scalar,Options,axis>, MotionDerived >
340  { typedef Eigen::Matrix<Scalar,6,1,Options> ReturnType; };
341 
342  template<typename Scalar, int Options, int axis, typename ForceDerived>
343  struct ConstraintForceOp< ConstraintRevoluteTpl<Scalar,Options,axis>, ForceDerived>
344  { typedef typename ForceDense<ForceDerived>::ConstAngularType::template ConstFixedSegmentReturnType<1>::Type ReturnType; };
345 
346  template<typename Scalar, int Options, int axis, typename ForceSet>
347  struct ConstraintForceSetOp< ConstraintRevoluteTpl<Scalar,Options,axis>, ForceSet>
348  { typedef typename Eigen::MatrixBase<ForceSet>::ConstRowXpr ReturnType; };
349 
350  template<typename _Scalar, int _Options, int axis>
351  struct traits< ConstraintRevoluteTpl<_Scalar,_Options,axis> >
352  {
353  typedef _Scalar Scalar;
354  enum { Options = _Options };
355  enum {
356  LINEAR = 0,
357  ANGULAR = 3
358  };
359  typedef MotionRevoluteTpl<Scalar,Options,axis> JointMotion;
360  typedef Eigen::Matrix<Scalar,1,1,Options> JointForce;
361  typedef Eigen::Matrix<Scalar,6,1,Options> DenseBase;
362  typedef DenseBase MatrixReturnType;
363  typedef const DenseBase ConstMatrixReturnType;
364  }; // traits ConstraintRevoluteTpl
365 
366  template<typename _Scalar, int _Options, int axis>
367  struct ConstraintRevoluteTpl
368  : ConstraintBase< ConstraintRevoluteTpl<_Scalar,_Options,axis> >
369  {
370  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
371 
372  PINOCCHIO_CONSTRAINT_TYPEDEF_TPL(ConstraintRevoluteTpl)
373  enum { NV = 1 };
374 
375  typedef SpatialAxis<ANGULAR+axis> Axis;
376 
377  ConstraintRevoluteTpl() {}
378 
379  template<typename Vector1Like>
380  JointMotion __mult__(const Eigen::MatrixBase<Vector1Like> & v) const
381  { return JointMotion(v[0]); }
382 
383  template<typename S1, int O1>
384  typename SE3GroupAction<ConstraintRevoluteTpl>::ReturnType
385  se3Action(const SE3Tpl<S1,O1> & m) const
386  {
387  typedef typename SE3GroupAction<ConstraintRevoluteTpl>::ReturnType ReturnType;
388  ReturnType res;
389  res.template segment<3>(LINEAR) = m.translation().cross(m.rotation().col(axis));
390  res.template segment<3>(ANGULAR) = m.rotation().col(axis);
391  return res;
392  }
393 
394  template<typename S1, int O1>
395  typename SE3GroupAction<ConstraintRevoluteTpl>::ReturnType
396  se3ActionInverse(const SE3Tpl<S1,O1> & m) const
397  {
398  typedef typename SE3GroupAction<ConstraintRevoluteTpl>::ReturnType ReturnType;
399  typedef typename Axis::CartesianAxis3 CartesianAxis3;
400  ReturnType res;
401  res.template segment<3>(LINEAR).noalias() = m.rotation().transpose()*CartesianAxis3::cross(m.translation());
402  res.template segment<3>(ANGULAR) = m.rotation().transpose().col(axis);
403  return res;
404  }
405 
406  int nv_impl() const { return NV; }
407 
408  struct TransposeConst
409  {
410  const ConstraintRevoluteTpl & ref;
411  TransposeConst(const ConstraintRevoluteTpl & ref) : ref(ref) {}
412 
413  template<typename ForceDerived>
414  typename ConstraintForceOp<ConstraintRevoluteTpl,ForceDerived>::ReturnType
415  operator*(const ForceDense<ForceDerived> & f) const
416  { return f.angular().template segment<1>(axis); }
417 
419  template<typename Derived>
420  typename ConstraintForceSetOp<ConstraintRevoluteTpl,Derived>::ReturnType
421  operator*(const Eigen::MatrixBase<Derived> & F) const
422  {
423  assert(F.rows()==6);
424  return F.row(ANGULAR + axis);
425  }
426  }; // struct TransposeConst
427 
428  TransposeConst transpose() const { return TransposeConst(*this); }
429 
430  /* CRBA joint operators
431  * - ForceSet::Block = ForceSet
432  * - ForceSet operator* (Inertia Y,Constraint S)
433  * - MatrixBase operator* (Constraint::Transpose S, ForceSet::Block)
434  * - SE3::act(ForceSet::Block)
435  */
436  DenseBase matrix_impl() const
437  {
438  DenseBase S;
439  MotionRef<DenseBase> v(S);
440  v << Axis();
441  return S;
442  }
443 
444  template<typename MotionDerived>
445  typename MotionAlgebraAction<ConstraintRevoluteTpl,MotionDerived>::ReturnType
446  motionAction(const MotionDense<MotionDerived> & m) const
447  {
448  typedef typename MotionAlgebraAction<ConstraintRevoluteTpl,MotionDerived>::ReturnType ReturnType;
449  ReturnType res;
450  MotionRef<ReturnType> v(res);
451  v = m.cross(Axis());
452  return res;
453  }
454 
455  bool isEqual(const ConstraintRevoluteTpl &) const { return true; }
456 
457  }; // struct ConstraintRevoluteTpl
458 
459  template<typename _Scalar, int _Options, int _axis>
460  struct JointRevoluteTpl
461  {
462  typedef _Scalar Scalar;
463 
464  enum
465  {
466  Options = _Options,
467  axis = _axis
468  };
469  };
470 
471  template<typename S1, int O1,typename S2, int O2, int axis>
472  struct MultiplicationOp<InertiaTpl<S1,O1>, ConstraintRevoluteTpl<S2,O2,axis> >
473  {
474  typedef Eigen::Matrix<S2,6,1,O2> ReturnType;
475  };
476 
477  /* [CRBA] ForceSet operator* (Inertia Y,Constraint S) */
478  namespace impl
479  {
480  template<typename S1, int O1, typename S2, int O2>
481  struct LhsMultiplicationOp<InertiaTpl<S1,O1>, ConstraintRevoluteTpl<S2,O2,0> >
482  {
483  typedef InertiaTpl<S1,O1> Inertia;
484  typedef ConstraintRevoluteTpl<S2,O2,0> Constraint;
485  typedef typename MultiplicationOp<Inertia,Constraint>::ReturnType ReturnType;
486  static inline ReturnType run(const Inertia & Y,
487  const Constraint & /*constraint*/)
488  {
489  ReturnType res;
490 
491  /* Y(:,3) = ( 0,-z, y, I00+yy+zz, I01-xy , I02-xz ) */
492  const S1
493  &m = Y.mass(),
494  &x = Y.lever()[0],
495  &y = Y.lever()[1],
496  &z = Y.lever()[2];
497  const typename Inertia::Symmetric3 & I = Y.inertia();
498 
499  res <<
500  (S2)0,
501  -m*z,
502  m*y,
503  I(0,0)+m*(y*y+z*z),
504  I(0,1)-m*x*y,
505  I(0,2)-m*x*z;
506 
507  return res;
508  }
509  };
510 
511  template<typename S1, int O1, typename S2, int O2>
512  struct LhsMultiplicationOp<InertiaTpl<S1,O1>, ConstraintRevoluteTpl<S2,O2,1> >
513  {
514  typedef InertiaTpl<S1,O1> Inertia;
515  typedef ConstraintRevoluteTpl<S2,O2,1> Constraint;
516  typedef typename MultiplicationOp<Inertia,Constraint>::ReturnType ReturnType;
517  static inline ReturnType run(const Inertia & Y,
518  const Constraint & /*constraint*/)
519  {
520  ReturnType res;
521 
522  /* Y(:,4) = ( z, 0,-x, I10-xy , I11+xx+zz, I12-yz ) */
523  const S1
524  &m = Y.mass(),
525  &x = Y.lever()[0],
526  &y = Y.lever()[1],
527  &z = Y.lever()[2];
528  const typename Inertia::Symmetric3 & I = Y.inertia();
529 
530  res <<
531  m*z,
532  (S2)0,
533  -m*x,
534  I(1,0)-m*x*y,
535  I(1,1)+m*(x*x+z*z),
536  I(1,2)-m*y*z;
537 
538  return res;
539  }
540  };
541 
542  template<typename S1, int O1, typename S2, int O2>
543  struct LhsMultiplicationOp<InertiaTpl<S1,O1>, ConstraintRevoluteTpl<S2,O2,2> >
544  {
545  typedef InertiaTpl<S1,O1> Inertia;
546  typedef ConstraintRevoluteTpl<S2,O2,2> Constraint;
547  typedef typename MultiplicationOp<Inertia,Constraint>::ReturnType ReturnType;
548  static inline ReturnType run(const Inertia & Y,
549  const Constraint & /*constraint*/)
550  {
551  ReturnType res;
552 
553  /* Y(:,5) = (-y, x, 0, I20-xz , I21-yz , I22+xx+yy) */
554  const S1
555  &m = Y.mass(),
556  &x = Y.lever()[0],
557  &y = Y.lever()[1],
558  &z = Y.lever()[2];
559  const typename Inertia::Symmetric3 & I = Y.inertia();
560 
561  res <<
562  -m*y,
563  m*x,
564  (S2)0,
565  I(2,0)-m*x*z,
566  I(2,1)-m*y*z,
567  I(2,2)+m*(x*x+y*y);
568 
569  return res;
570  }
571  };
572  } // namespace impl
573 
574  template<typename M6Like,typename S2, int O2, int axis>
575  struct MultiplicationOp<Eigen::MatrixBase<M6Like>, ConstraintRevoluteTpl<S2,O2,axis> >
576  {
577  typedef typename M6Like::ConstColXpr ReturnType;
578  };
579 
580  /* [ABA] operator* (Inertia Y,Constraint S) */
581  namespace impl
582  {
583  template<typename M6Like, typename Scalar, int Options, int axis>
584  struct LhsMultiplicationOp<Eigen::MatrixBase<M6Like>, ConstraintRevoluteTpl<Scalar,Options,axis> >
585  {
586  typedef ConstraintRevoluteTpl<Scalar,Options,axis> Constraint;
587  typedef typename MultiplicationOp<Eigen::MatrixBase<M6Like>,Constraint>::ReturnType ReturnType;
588  static inline ReturnType run(const Eigen::MatrixBase<M6Like> & Y,
589  const Constraint & /*constraint*/)
590  {
591  EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(M6Like,6,6);
592  return Y.col(Inertia::ANGULAR + axis);
593  }
594  };
595  } // namespace impl
596 
597  template<typename _Scalar, int _Options, int axis>
598  struct traits< JointRevoluteTpl<_Scalar,_Options,axis> >
599  {
600  enum {
601  NQ = 1,
602  NV = 1
603  };
604  typedef _Scalar Scalar;
605  enum { Options = _Options };
606  typedef JointDataRevoluteTpl<Scalar,Options,axis> JointDataDerived;
607  typedef JointModelRevoluteTpl<Scalar,Options,axis> JointModelDerived;
608  typedef ConstraintRevoluteTpl<Scalar,Options,axis> Constraint_t;
609  typedef TransformRevoluteTpl<Scalar,Options,axis> Transformation_t;
610  typedef MotionRevoluteTpl<Scalar,Options,axis> Motion_t;
611  typedef MotionZeroTpl<Scalar,Options> Bias_t;
612 
613  // [ABA]
614  typedef Eigen::Matrix<Scalar,6,NV,Options> U_t;
615  typedef Eigen::Matrix<Scalar,NV,NV,Options> D_t;
616  typedef Eigen::Matrix<Scalar,6,NV,Options> UD_t;
617 
618  PINOCCHIO_JOINT_DATA_BASE_ACCESSOR_DEFAULT_RETURN_TYPE
619 
620  typedef Eigen::Matrix<Scalar,NQ,1,Options> ConfigVector_t;
621  typedef Eigen::Matrix<Scalar,NV,1,Options> TangentVector_t;
622  };
623 
624  template<typename Scalar, int Options, int axis>
625  struct traits< JointDataRevoluteTpl<Scalar,Options,axis> >
626  { typedef JointRevoluteTpl<Scalar,Options,axis> JointDerived; };
627 
628  template<typename Scalar, int Options, int axis>
629  struct traits< JointModelRevoluteTpl<Scalar,Options,axis> >
630  { typedef JointRevoluteTpl<Scalar,Options,axis> JointDerived; };
631 
632  template<typename _Scalar, int _Options, int axis>
633  struct JointDataRevoluteTpl : public JointDataBase< JointDataRevoluteTpl<_Scalar,_Options,axis> >
634  {
635  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
636  typedef JointRevoluteTpl<_Scalar,_Options,axis> JointDerived;
637  PINOCCHIO_JOINT_DATA_TYPEDEF_TEMPLATE(JointDerived);
638  PINOCCHIO_JOINT_DATA_BASE_DEFAULT_ACCESSOR
639 
640  Constraint_t S;
641  Transformation_t M;
642  Motion_t v;
643  Bias_t c;
644 
645  // [ABA] specific data
646  U_t U;
647  D_t Dinv;
648  UD_t UDinv;
649 
650  JointDataRevoluteTpl()
651  : M((Scalar)0,(Scalar)1)
652  , v((Scalar)0)
653  , U(U_t::Zero())
654  , Dinv(D_t::Zero())
655  , UDinv(UD_t::Zero())
656  {}
657 
658  static std::string classname()
659  {
660  return std::string("JointDataR") + axisLabel<axis>();
661  }
662  std::string shortname() const { return classname(); }
663 
664  }; // struct JointDataRevoluteTpl
665 
666  template<typename NewScalar, typename Scalar, int Options, int axis>
667  struct CastType< NewScalar, JointModelRevoluteTpl<Scalar,Options,axis> >
668  {
669  typedef JointModelRevoluteTpl<NewScalar,Options,axis> type;
670  };
671 
672  template<typename _Scalar, int _Options, int axis>
673  struct JointModelRevoluteTpl
674  : public JointModelBase< JointModelRevoluteTpl<_Scalar,_Options,axis> >
675  {
676  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
677  typedef JointRevoluteTpl<_Scalar,_Options,axis> JointDerived;
678  PINOCCHIO_JOINT_TYPEDEF_TEMPLATE(JointDerived);
679 
680  typedef JointModelBase<JointModelRevoluteTpl> Base;
681  using Base::id;
682  using Base::idx_q;
683  using Base::idx_v;
684  using Base::setIndexes;
685 
686  JointDataDerived createData() const { return JointDataDerived(); }
687 
688  JointModelRevoluteTpl() {}
689 
690  const std::vector<bool> hasConfigurationLimit() const
691  {
692  return {true};
693  }
694 
695  const std::vector<bool> hasConfigurationLimitInTangent() const
696  {
697  return {true};
698  }
699 
700  template<typename ConfigVector>
701  EIGEN_DONT_INLINE
702  void calc(JointDataDerived & data,
703  const typename Eigen::MatrixBase<ConfigVector> & qs) const
704  {
705  typedef typename ConfigVector::Scalar OtherScalar;
706 
707  const OtherScalar & q = qs[idx_q()];
708  OtherScalar ca,sa; SINCOS(q,&sa,&ca);
709  data.M.setValues(sa,ca);
710  }
711 
712  template<typename ConfigVector, typename TangentVector>
713  EIGEN_DONT_INLINE
714  void calc(JointDataDerived & data,
715  const typename Eigen::MatrixBase<ConfigVector> & qs,
716  const typename Eigen::MatrixBase<TangentVector> & vs) const
717  {
718  calc(data,qs.derived());
719 
720  data.v.angularRate() = static_cast<Scalar>(vs[idx_v()]);
721  }
722 
723  template<typename Matrix6Like>
724  void calc_aba(JointDataDerived & data,
725  const Eigen::MatrixBase<Matrix6Like> & I,
726  const bool update_I) const
727  {
728  data.U = I.col(Inertia::ANGULAR + axis);
729  data.Dinv[0] = Scalar(1)/I(Inertia::ANGULAR + axis,Inertia::ANGULAR + axis);
730  data.UDinv.noalias() = data.U * data.Dinv[0];
731 
732  if (update_I)
733  PINOCCHIO_EIGEN_CONST_CAST(Matrix6Like,I) -= data.UDinv * data.U.transpose();
734  }
735 
736  static std::string classname()
737  {
738  return std::string("JointModelR") + axisLabel<axis>();
739  }
740  std::string shortname() const { return classname(); }
741 
743  template<typename NewScalar>
744  JointModelRevoluteTpl<NewScalar,Options,axis> cast() const
745  {
746  typedef JointModelRevoluteTpl<NewScalar,Options,axis> ReturnType;
747  ReturnType res;
748  res.setIndexes(id(),idx_q(),idx_v());
749  return res;
750  }
751 
752  }; // struct JointModelRevoluteTpl
753 
754  typedef JointRevoluteTpl<double,0,0> JointRX;
755  typedef JointDataRevoluteTpl<double,0,0> JointDataRX;
756  typedef JointModelRevoluteTpl<double,0,0> JointModelRX;
757 
758  typedef JointRevoluteTpl<double,0,1> JointRY;
759  typedef JointDataRevoluteTpl<double,0,1> JointDataRY;
760  typedef JointModelRevoluteTpl<double,0,1> JointModelRY;
761 
762  typedef JointRevoluteTpl<double,0,2> JointRZ;
763  typedef JointDataRevoluteTpl<double,0,2> JointDataRZ;
764  typedef JointModelRevoluteTpl<double,0,2> JointModelRZ;
765 
766 } //namespace pinocchio
767 
768 #include <boost/type_traits.hpp>
769 
770 namespace boost
771 {
772  template<typename Scalar, int Options, int axis>
773  struct has_nothrow_constructor< ::pinocchio::JointModelRevoluteTpl<Scalar,Options,axis> >
774  : public integral_constant<bool,true> {};
775 
776  template<typename Scalar, int Options, int axis>
777  struct has_nothrow_copy< ::pinocchio::JointModelRevoluteTpl<Scalar,Options,axis> >
778  : public integral_constant<bool,true> {};
779 
780  template<typename Scalar, int Options, int axis>
781  struct has_nothrow_constructor< ::pinocchio::JointDataRevoluteTpl<Scalar,Options,axis> >
782  : public integral_constant<bool,true> {};
783 
784  template<typename Scalar, int Options, int axis>
785  struct has_nothrow_copy< ::pinocchio::JointDataRevoluteTpl<Scalar,Options,axis> >
786  : public integral_constant<bool,true> {};
787 }
788 
789 #endif // ifndef __pinocchio_joint_revolute_hpp__
pinocchio::MultiplicationOp
Forward declaration of the multiplication operation return type. Should be overloaded,...
Definition: binary-op.hpp:15
pinocchio::ConstraintRevoluteTpl::TransposeConst
Definition: joint-revolute.hpp:408
pinocchio::MotionRevoluteTpl
Definition: joint-revolute.hpp:19
pinocchio::operator*
MultiplicationOp< InertiaTpl< Scalar, Options >, ConstraintDerived >::ReturnType operator*(const InertiaTpl< Scalar, Options > &Y, const ConstraintBase< ConstraintDerived > &constraint)
&#160;
Definition: constraint-base.hpp:112
pinocchio::idx_q
int idx_q(const JointModelTpl< Scalar, Options, JointCollectionTpl > &jmodel)
Visit a JointModelTpl through JointIdxQVisitor to get the index in the full model configuration space...
pinocchio::JointModelBase
Definition: joint-model-base.hpp:67
pinocchio::ConstraintForceOp
Return type of the Constraint::Transpose * Force operation.
Definition: constraint-base.hpp:35
pinocchio::idx_v
int idx_v(const JointModelTpl< Scalar, Options, JointCollectionTpl > &jmodel)
Visit a JointModelTpl through JointIdxVVisitor to get the index in the full model tangent space corre...
pinocchio::SE3Tpl< Scalar, Options >
pinocchio::JointDataBase
Definition: joint-data-base.hpp:82
pinocchio::isEqual
bool isEqual(const JointModelTpl< Scalar, Options, JointCollectionTpl > &jmodel_generic, const JointModelBase< JointModelDerived > &jmodel)
Visit a JointModelTpl<Scalar,...> to compare it to JointModelDerived.
pinocchio::SINCOS
void SINCOS(const S1 &a, S2 *sa, S3 *ca)
Computes sin/cos values of a given input scalar.
Definition: sincos.hpp:26
pinocchio::MotionAlgebraAction
Return type of the ation of a Motion onto an object of type D.
Definition: motion.hpp:46
pinocchio::impl::LhsMultiplicationOp
Definition: binary-op.hpp:20
pinocchio::JointRevoluteTpl
Definition: joint-revolute.hpp:460
pinocchio::SE3GroupAction
Definition: se3.hpp:39
pinocchio::MotionZeroTpl
Definition: fwd.hpp:44
pinocchio::hasConfigurationLimitInTangent
const std::vector< bool > hasConfigurationLimitInTangent(const JointModelTpl< Scalar, Options, JointCollectionTpl > &jmodel)
Visit a JointModelTpl through JointConfigurationLimitInTangentVisitor to get the configurations limit...
pinocchio::JointModelRevoluteTpl::cast
JointModelRevoluteTpl< NewScalar, Options, axis > cast() const
Definition: joint-revolute.hpp:744
pinocchio::ConstraintRevoluteTpl::TransposeConst::operator*
ConstraintForceSetOp< ConstraintRevoluteTpl, Derived >::ReturnType operator*(const Eigen::MatrixBase< Derived > &F) const
[CRBA] MatrixBase operator* (Constraint::Transpose S, ForceSet::Block)
Definition: joint-revolute.hpp:421
pinocchio::ConstraintRevoluteTpl
Definition: joint-revolute.hpp:332
pinocchio::shortname
std::string shortname(const JointModelTpl< Scalar, Options, JointCollectionTpl > &jmodel)
Visit a JointModelTpl through JointShortnameVisitor to get the shortname of the derived joint model.
pinocchio::JointDataRevoluteTpl
Definition: fwd.hpp:26
pinocchio::TransformRevoluteTpl
Definition: joint-revolute.hpp:58
pinocchio::hasConfigurationLimit
const std::vector< bool > hasConfigurationLimit(const JointModelTpl< Scalar, Options, JointCollectionTpl > &jmodel)
Visit a JointModelTpl through JointConfigurationLimitVisitor to get the configurations limits.
pinocchio::JointModelRevoluteTpl
Definition: fwd.hpp:25
pinocchio::ForceSetTpl
Definition: force-set.hpp:14
pinocchio::ConstraintForceSetOp
Return type of the Constraint::Transpose * ForceSet operation.
Definition: constraint-base.hpp:42
pinocchio::SE3Base
Base class for rigid transformation.
Definition: se3-base.hpp:30
pinocchio::calc_aba
void calc_aba(const JointModelTpl< Scalar, Options, JointCollectionTpl > &jmodel, JointDataTpl< Scalar, Options, JointCollectionTpl > &jdata, const Eigen::MatrixBase< Matrix6Type > &I, const bool update_I)
Visit a JointModelTpl and the corresponding JointDataTpl through JointCalcAbaVisitor to.
pinocchio::ForceDense
Definition: force-dense.hpp:24
pinocchio::ConstraintBase
Definition: constraint-base.hpp:48
pinocchio::InertiaTpl
Definition: fwd.hpp:52
pinocchio::traits
Common traits structure to fully define base classes for CRTP.
Definition: fwd.hpp:44
pinocchio::createData
JointDataTpl< Scalar, Options, JointCollectionTpl > createData(const JointModelTpl< Scalar, Options, JointCollectionTpl > &jmodel)
Visit a JointModelTpl through CreateData visitor to create a JointDataTpl.
pinocchio::MotionTpl< Scalar, Options >
pinocchio::CastType
Type of the cast of a class C templated by Scalar and Options, to a new NewScalar type....
Definition: fwd.hpp:55
pinocchio::ForceDense::angular
ConstAngularType angular() const
Return the angular part of the force vector.
Definition: force-base.hpp:35
pinocchio
Main pinocchio namespace.
Definition: treeview.dox:11