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