pinocchio  2.2.1-dirty
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  typedef typename traits<TransformRevoluteTpl>::PlainType PlainType;
91 
93  TransformRevoluteTpl(const Scalar & sin, const Scalar & cos)
94  : m_sin(sin), m_cos(cos)
95  {}
96 
97  PlainType plain() const
98  {
99  PlainType res(PlainType::Identity());
100  _setRotation (res.rotation());
101  return res;
102  }
103 
104  operator PlainType() const { return plain(); }
105 
106  template<typename S2, int O2>
108  se3action(const SE3Tpl<S2,O2> & m) const
109  {
110  typedef typename SE3GroupAction<TransformRevoluteTpl>::ReturnType ReturnType;
111  ReturnType res;
112  switch(axis)
113  {
114  case 0:
115  {
116  res.rotation().col(0) = m.rotation().col(0);
117  res.rotation().col(1).noalias() = m_cos * m.rotation().col(1) + m_sin * m.rotation().col(2);
118  res.rotation().col(2).noalias() = res.rotation().col(0).cross(res.rotation().col(1));
119  break;
120  }
121  case 1:
122  {
123  res.rotation().col(1) = m.rotation().col(1);
124  res.rotation().col(2).noalias() = m_cos * m.rotation().col(2) + m_sin * m.rotation().col(0);
125  res.rotation().col(0).noalias() = res.rotation().col(1).cross(res.rotation().col(2));
126  break;
127  }
128  case 2:
129  {
130  res.rotation().col(2) = m.rotation().col(2);
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  break;
134  }
135  default:
136  {
137  assert(false && "must nerver happened");
138  break;
139  }
140  }
141  res.translation() = m.translation();
142  return res;
143  }
144 
145  const Scalar & sin() const { return m_sin; }
146  const Scalar & cos() const { return m_cos; }
147 
148  template<typename OtherScalar>
149  void setValues(const OtherScalar & sin, const OtherScalar & cos)
150  { m_sin = sin; m_cos = cos; }
151 
152  LinearType translation() const { return LinearType::PlainObject::Zero(3); };
153  AngularType rotation() const {
154  AngularType m(AngularType::Identity(3));
155  _setRotation (m);
156  return m;
157  }
158 
159  protected:
160 
161  Scalar m_sin, m_cos;
162  inline void _setRotation (typename PlainType::AngularRef& rot) const
163  {
164  switch(axis)
165  {
166  case 0:
167  {
168  rot.coeffRef(1,1) = m_cos; rot.coeffRef(1,2) = -m_sin;
169  rot.coeffRef(2,1) = m_sin; rot.coeffRef(2,2) = m_cos;
170  break;
171  }
172  case 1:
173  {
174  rot.coeffRef(0,0) = m_cos; rot.coeffRef(0,2) = m_sin;
175  rot.coeffRef(2,0) = -m_sin; rot.coeffRef(2,2) = m_cos;
176  break;
177  }
178  case 2:
179  {
180  rot.coeffRef(0,0) = m_cos; rot.coeffRef(0,1) = -m_sin;
181  rot.coeffRef(1,0) = m_sin; rot.coeffRef(1,1) = m_cos;
182  break;
183  }
184  default:
185  {
186  assert(false && "must nerver happened");
187  break;
188  }
189  }
190  }
191  };
192 
193  template<typename _Scalar, int _Options, int axis>
194  struct MotionRevoluteTpl
195  : MotionBase< MotionRevoluteTpl<_Scalar,_Options,axis> >
196  {
197  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
198 
199  MOTION_TYPEDEF_TPL(MotionRevoluteTpl);
200  typedef SpatialAxis<axis+ANGULAR> Axis;
201  typedef typename Axis::CartesianAxis3 CartesianAxis3;
202 
203  MotionRevoluteTpl() {}
204 
205  MotionRevoluteTpl(const Scalar & w) : w(w) {}
206 
207  template<typename Vector1Like>
208  MotionRevoluteTpl(const Eigen::MatrixBase<Vector1Like> & v)
209  : w(v[0])
210  {
211  using namespace Eigen;
212  EIGEN_STATIC_ASSERT_SIZE_1x1(Vector1Like);
213  }
214 
215  inline PlainReturnType plain() const { return Axis() * w; }
216 
217  template<typename OtherScalar>
218  MotionRevoluteTpl __mult__(const OtherScalar & alpha) const
219  {
220  return MotionRevoluteTpl(alpha*w);
221  }
222 
223  template<typename MotionDerived>
224  void setTo(MotionDense<MotionDerived> & m) const
225  {
226  m.linear().setZero();
227  for(Eigen::DenseIndex k = 0; k < 3; ++k)
228  m.angular()[k] = k == axis ? w : (Scalar)0;
229  }
230 
231  template<typename MotionDerived>
232  inline void addTo(MotionDense<MotionDerived> & v) const
233  {
234  typedef typename MotionDense<MotionDerived>::Scalar OtherScalar;
235  v.angular()[axis] += (OtherScalar)w;
236  }
237 
238  template<typename S2, int O2, typename D2>
239  inline void se3Action_impl(const SE3Tpl<S2,O2> & m, MotionDense<D2> & v) const
240  {
241  v.angular().noalias() = m.rotation().col(axis) * w;
242  v.linear().noalias() = m.translation().cross(v.angular());
243  }
244 
245  template<typename S2, int O2>
246  MotionPlain se3Action_impl(const SE3Tpl<S2,O2> & m) const
247  {
248  MotionPlain res;
249  se3Action_impl(m,res);
250  return res;
251  }
252 
253  template<typename S2, int O2, typename D2>
254  void se3ActionInverse_impl(const SE3Tpl<S2,O2> & m,
255  MotionDense<D2> & v) const
256  {
257  // Linear
258  CartesianAxis3::alphaCross(w,m.translation(),v.angular());
259  v.linear().noalias() = m.rotation().transpose() * v.angular();
260 
261  // Angular
262  v.angular().noalias() = m.rotation().transpose().col(axis) * w;
263  }
264 
265  template<typename S2, int O2>
266  MotionPlain se3ActionInverse_impl(const SE3Tpl<S2,O2> & m) const
267  {
268  MotionPlain res;
269  se3ActionInverse_impl(m,res);
270  return res;
271  }
272 
273  template<typename M1, typename M2>
274  EIGEN_STRONG_INLINE
275  void motionAction(const MotionDense<M1> & v, MotionDense<M2> & mout) const
276  {
277  // Linear
278  CartesianAxis3::alphaCross(-w,v.linear(),mout.linear());
279 
280  // Angular
281  CartesianAxis3::alphaCross(-w,v.angular(),mout.angular());
282  }
283 
284  template<typename M1>
285  MotionPlain motionAction(const MotionDense<M1> & v) const
286  {
287  MotionPlain res;
288  motionAction(v,res);
289  return res;
290  }
291 
292  // data
293  Scalar w;
294  }; // struct MotionRevoluteTpl
295 
296  template<typename S1, int O1, int axis, typename MotionDerived>
297  typename MotionDerived::MotionPlain
298  operator+(const MotionRevoluteTpl<S1,O1,axis> & m1,
299  const MotionDense<MotionDerived> & m2)
300  {
301  typename MotionDerived::MotionPlain res(m2);
302  res += m1;
303  return res;
304  }
305 
306  template<typename MotionDerived, typename S2, int O2, int axis>
307  EIGEN_STRONG_INLINE
308  typename MotionDerived::MotionPlain
309  operator^(const MotionDense<MotionDerived> & m1, const MotionRevoluteTpl<S2,O2,axis>& m2)
310  {
311  return m2.motionAction(m1);
312  }
313 
314  template<typename Scalar, int Options, int axis> struct ConstraintRevoluteTpl;
315 
316  template<typename Scalar, int Options, int axis>
317  struct SE3GroupAction< ConstraintRevoluteTpl<Scalar,Options,axis> >
318  { typedef Eigen::Matrix<Scalar,6,1,Options> ReturnType; };
319 
320  template<typename Scalar, int Options, int axis, typename MotionDerived>
321  struct MotionAlgebraAction< ConstraintRevoluteTpl<Scalar,Options,axis>, MotionDerived >
322  { typedef Eigen::Matrix<Scalar,6,1,Options> ReturnType; };
323 
324  template<typename Scalar, int Options, int axis, typename ForceDerived>
325  struct ConstraintForceOp< ConstraintRevoluteTpl<Scalar,Options,axis>, ForceDerived>
326  { typedef typename ForceDense<ForceDerived>::ConstAngularType::template ConstFixedSegmentReturnType<1>::Type ReturnType; };
327 
328  template<typename Scalar, int Options, int axis, typename ForceSet>
329  struct ConstraintForceSetOp< ConstraintRevoluteTpl<Scalar,Options,axis>, ForceSet>
330  { typedef typename Eigen::MatrixBase<ForceSet>::ConstRowXpr ReturnType; };
331 
332  template<typename _Scalar, int _Options, int axis>
333  struct traits< ConstraintRevoluteTpl<_Scalar,_Options,axis> >
334  {
335  typedef _Scalar Scalar;
336  enum { Options = _Options };
337  enum {
338  LINEAR = 0,
339  ANGULAR = 3
340  };
342  typedef Eigen::Matrix<Scalar,1,1,Options> JointForce;
343  typedef Eigen::Matrix<Scalar,6,1,Options> DenseBase;
344  typedef DenseBase MatrixReturnType;
345  typedef const DenseBase ConstMatrixReturnType;
346  }; // traits ConstraintRevoluteTpl
347 
348  template<typename _Scalar, int _Options, int axis>
349  struct ConstraintRevoluteTpl
350  : ConstraintBase< ConstraintRevoluteTpl<_Scalar,_Options,axis> >
351  {
352  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
353 
354  PINOCCHIO_CONSTRAINT_TYPEDEF_TPL(ConstraintRevoluteTpl)
355  enum { NV = 1 };
356 
357  typedef SpatialAxis<ANGULAR+axis> Axis;
358 
360 
361  template<typename Vector1Like>
362  JointMotion __mult__(const Eigen::MatrixBase<Vector1Like> & v) const
363  { return JointMotion(v[0]); }
364 
365  template<typename S1, int O1>
367  se3Action(const SE3Tpl<S1,O1> & m) const
368  {
369  typedef typename SE3GroupAction<ConstraintRevoluteTpl>::ReturnType ReturnType;
370  ReturnType res;
371  res.template segment<3>(LINEAR) = m.translation().cross(m.rotation().col(axis));
372  res.template segment<3>(ANGULAR) = m.rotation().col(axis);
373  return res;
374  }
375 
376  template<typename S1, int O1>
378  se3ActionInverse(const SE3Tpl<S1,O1> & m) const
379  {
380  typedef typename SE3GroupAction<ConstraintRevoluteTpl>::ReturnType ReturnType;
381  typedef typename Axis::CartesianAxis3 CartesianAxis3;
382  ReturnType res;
383  res.template segment<3>(LINEAR).noalias() = m.rotation().transpose()*CartesianAxis3::cross(m.translation());
384  res.template segment<3>(ANGULAR) = m.rotation().transpose().col(axis);
385  return res;
386  }
387 
388  int nv_impl() const { return NV; }
389 
391  {
392  const ConstraintRevoluteTpl & ref;
393  TransposeConst(const ConstraintRevoluteTpl & ref) : ref(ref) {}
394 
395  template<typename ForceDerived>
396  typename ConstraintForceOp<ConstraintRevoluteTpl,ForceDerived>::ReturnType
397  operator*(const ForceDense<ForceDerived> & f) const
398  { return f.angular().template segment<1>(axis); }
399 
401  template<typename Derived>
402  typename ConstraintForceSetOp<ConstraintRevoluteTpl,Derived>::ReturnType
403  operator*(const Eigen::MatrixBase<Derived> & F) const
404  {
405  assert(F.rows()==6);
406  return F.row(ANGULAR + axis);
407  }
408  }; // struct TransposeConst
409 
410  TransposeConst transpose() const { return TransposeConst(*this); }
411 
412  /* CRBA joint operators
413  * - ForceSet::Block = ForceSet
414  * - ForceSet operator* (Inertia Y,Constraint S)
415  * - MatrixBase operator* (Constraint::Transpose S, ForceSet::Block)
416  * - SE3::act(ForceSet::Block)
417  */
418  DenseBase matrix_impl() const
419  {
420  DenseBase S;
422  v << Axis();
423  return S;
424  }
425 
426  template<typename MotionDerived>
428  motionAction(const MotionDense<MotionDerived> & m) const
429  {
431  ReturnType res;
432  MotionRef<ReturnType> v(res);
433  v = m.cross(Axis());
434  return res;
435  }
436  }; // struct ConstraintRevoluteTpl
437 
438  template<typename _Scalar, int _Options, int _axis>
440  {
441  typedef _Scalar Scalar;
442 
443  enum
444  {
445  Options = _Options,
446  axis = _axis
447  };
448  };
449 
450  template<typename S1, int O1,typename S2, int O2, int axis>
451  struct MultiplicationOp<InertiaTpl<S1,O1>, ConstraintRevoluteTpl<S2,O2,axis> >
452  {
453  typedef Eigen::Matrix<S2,6,1,O2> ReturnType;
454  };
455 
456  /* [CRBA] ForceSet operator* (Inertia Y,Constraint S) */
457  namespace impl
458  {
459  template<typename S1, int O1, typename S2, int O2>
461  {
462  typedef InertiaTpl<S1,O1> Inertia;
464  typedef typename MultiplicationOp<Inertia,Constraint>::ReturnType ReturnType;
465  static inline ReturnType run(const Inertia & Y,
466  const Constraint & /*constraint*/)
467  {
468  ReturnType res;
469 
470  /* Y(:,3) = ( 0,-z, y, I00+yy+zz, I01-xy , I02-xz ) */
471  const S1
472  &m = Y.mass(),
473  &x = Y.lever()[0],
474  &y = Y.lever()[1],
475  &z = Y.lever()[2];
476  const typename Inertia::Symmetric3 & I = Y.inertia();
477 
478  res <<
479  (S2)0,
480  -m*z,
481  m*y,
482  I(0,0)+m*(y*y+z*z),
483  I(0,1)-m*x*y,
484  I(0,2)-m*x*z;
485 
486  return res;
487  }
488  };
489 
490  template<typename S1, int O1, typename S2, int O2>
492  {
493  typedef InertiaTpl<S1,O1> Inertia;
495  typedef typename MultiplicationOp<Inertia,Constraint>::ReturnType ReturnType;
496  static inline ReturnType run(const Inertia & Y,
497  const Constraint & /*constraint*/)
498  {
499  ReturnType res;
500 
501  /* Y(:,4) = ( z, 0,-x, I10-xy , I11+xx+zz, I12-yz ) */
502  const S1
503  &m = Y.mass(),
504  &x = Y.lever()[0],
505  &y = Y.lever()[1],
506  &z = Y.lever()[2];
507  const typename Inertia::Symmetric3 & I = Y.inertia();
508 
509  res <<
510  m*z,
511  (S2)0,
512  -m*x,
513  I(1,0)-m*x*y,
514  I(1,1)+m*(x*x+z*z),
515  I(1,2)-m*y*z;
516 
517  return res;
518  }
519  };
520 
521  template<typename S1, int O1, typename S2, int O2>
523  {
524  typedef InertiaTpl<S1,O1> Inertia;
526  typedef typename MultiplicationOp<Inertia,Constraint>::ReturnType ReturnType;
527  static inline ReturnType run(const Inertia & Y,
528  const Constraint & /*constraint*/)
529  {
530  ReturnType res;
531 
532  /* Y(:,5) = (-y, x, 0, I20-xz , I21-yz , I22+xx+yy) */
533  const S1
534  &m = Y.mass(),
535  &x = Y.lever()[0],
536  &y = Y.lever()[1],
537  &z = Y.lever()[2];
538  const typename Inertia::Symmetric3 & I = Y.inertia();
539 
540  res <<
541  -m*y,
542  m*x,
543  (S2)0,
544  I(2,0)-m*x*z,
545  I(2,1)-m*y*z,
546  I(2,2)+m*(x*x+y*y);
547 
548  return res;
549  }
550  };
551  } // namespace impl
552 
553  template<typename M6Like,typename S2, int O2, int axis>
554  struct MultiplicationOp<Eigen::MatrixBase<M6Like>, ConstraintRevoluteTpl<S2,O2,axis> >
555  {
556  typedef typename M6Like::ConstColXpr ReturnType;
557  };
558 
559  /* [ABA] operator* (Inertia Y,Constraint S) */
560  namespace impl
561  {
562  template<typename M6Like, typename Scalar, int Options, int axis>
563  struct LhsMultiplicationOp<Eigen::MatrixBase<M6Like>, ConstraintRevoluteTpl<Scalar,Options,axis> >
564  {
566  typedef typename MultiplicationOp<Eigen::MatrixBase<M6Like>,Constraint>::ReturnType ReturnType;
567  static inline ReturnType run(const Eigen::MatrixBase<M6Like> & Y,
568  const Constraint & /*constraint*/)
569  {
570  EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(M6Like,6,6);
571  return Y.col(Inertia::ANGULAR + axis);
572  }
573  };
574  } // namespace impl
575 
576  template<typename _Scalar, int _Options, int axis>
577  struct traits< JointRevoluteTpl<_Scalar,_Options,axis> >
578  {
579  enum {
580  NQ = 1,
581  NV = 1
582  };
583  typedef _Scalar Scalar;
584  enum { Options = _Options };
591 
592  // [ABA]
593  typedef Eigen::Matrix<Scalar,6,NV,Options> U_t;
594  typedef Eigen::Matrix<Scalar,NV,NV,Options> D_t;
595  typedef Eigen::Matrix<Scalar,6,NV,Options> UD_t;
596 
597  PINOCCHIO_JOINT_DATA_BASE_ACCESSOR_DEFAULT_RETURN_TYPE
598 
599  typedef Eigen::Matrix<Scalar,NQ,1,Options> ConfigVector_t;
600  typedef Eigen::Matrix<Scalar,NV,1,Options> TangentVector_t;
601  };
602 
603  template<typename Scalar, int Options, int axis>
604  struct traits< JointDataRevoluteTpl<Scalar,Options,axis> >
606 
607  template<typename Scalar, int Options, int axis>
608  struct traits< JointModelRevoluteTpl<Scalar,Options,axis> >
610 
611  template<typename _Scalar, int _Options, int axis>
612  struct JointDataRevoluteTpl : public JointDataBase< JointDataRevoluteTpl<_Scalar,_Options,axis> >
613  {
614  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
616  PINOCCHIO_JOINT_DATA_TYPEDEF_TEMPLATE(JointDerived);
617  PINOCCHIO_JOINT_DATA_BASE_DEFAULT_ACCESSOR
618 
619  Constraint_t S;
620  Transformation_t M;
621  Motion_t v;
622  Bias_t c;
623 
624  // [ABA] specific data
625  U_t U;
626  D_t Dinv;
627  UD_t UDinv;
628 
630 
631  static std::string classname() { return std::string("JointDataRevolute"); }
632  std::string shortname() const { return classname(); }
633 
634  }; // struct JointDataRevoluteTpl
635 
636  template<typename NewScalar, typename Scalar, int Options, int axis>
637  struct CastType< NewScalar, JointModelRevoluteTpl<Scalar,Options,axis> >
638  {
640  };
641 
642  template<typename _Scalar, int _Options, int axis>
643  struct JointModelRevoluteTpl
644  : public JointModelBase< JointModelRevoluteTpl<_Scalar,_Options,axis> >
645  {
646  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
647  typedef JointRevoluteTpl<_Scalar,_Options,axis> JointDerived;
648  PINOCCHIO_JOINT_TYPEDEF_TEMPLATE(JointDerived);
649 
651  using Base::id;
652  using Base::idx_q;
653  using Base::idx_v;
654  using Base::setIndexes;
655 
656  JointDataDerived createData() const { return JointDataDerived(); }
657 
659 
660  template<typename ConfigVector>
661  EIGEN_DONT_INLINE
662  void calc(JointDataDerived & data,
663  const typename Eigen::MatrixBase<ConfigVector> & qs) const
664  {
665  typedef typename ConfigVector::Scalar OtherScalar;
666 
667  const OtherScalar & q = qs[idx_q()];
668  OtherScalar ca,sa; SINCOS(q,&sa,&ca);
669  data.M.setValues(sa,ca);
670  }
671 
672  template<typename ConfigVector, typename TangentVector>
673  EIGEN_DONT_INLINE
674  void calc(JointDataDerived & data,
675  const typename Eigen::MatrixBase<ConfigVector> & qs,
676  const typename Eigen::MatrixBase<TangentVector> & vs) const
677  {
678  calc(data,qs.derived());
679 
680  data.v.w = (Scalar)vs[idx_v()];
681  }
682 
683  template<typename Matrix6Like>
684  void calc_aba(JointDataDerived & data,
685  const Eigen::MatrixBase<Matrix6Like> & I,
686  const bool update_I) const
687  {
688  data.U = I.col(Inertia::ANGULAR + axis);
689  data.Dinv[0] = Scalar(1)/I(Inertia::ANGULAR + axis,Inertia::ANGULAR + axis);
690  data.UDinv.noalias() = data.U * data.Dinv[0];
691 
692  if (update_I)
693  PINOCCHIO_EIGEN_CONST_CAST(Matrix6Like,I) -= data.UDinv * data.U.transpose();
694  }
695 
696  static std::string classname()
697  {
698  return std::string("JointModelR") + axisLabel<axis>();
699  }
700  std::string shortname() const { return classname(); }
701 
703  template<typename NewScalar>
705  {
707  ReturnType res;
708  res.setIndexes(id(),idx_q(),idx_v());
709  return res;
710  }
711 
712  }; // struct JointModelRevoluteTpl
713 
717 
721 
725 
726 } //namespace pinocchio
727 
728 #include <boost/type_traits.hpp>
729 
730 namespace boost
731 {
732  template<typename Scalar, int Options, int axis>
733  struct has_nothrow_constructor< ::pinocchio::JointModelRevoluteTpl<Scalar,Options,axis> >
734  : public integral_constant<bool,true> {};
735 
736  template<typename Scalar, int Options, int axis>
737  struct has_nothrow_copy< ::pinocchio::JointModelRevoluteTpl<Scalar,Options,axis> >
738  : public integral_constant<bool,true> {};
739 
740  template<typename Scalar, int Options, int axis>
741  struct has_nothrow_constructor< ::pinocchio::JointDataRevoluteTpl<Scalar,Options,axis> >
742  : public integral_constant<bool,true> {};
743 
744  template<typename Scalar, int Options, int axis>
745  struct has_nothrow_copy< ::pinocchio::JointDataRevoluteTpl<Scalar,Options,axis> >
746  : public integral_constant<bool,true> {};
747 }
748 
749 #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...
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.
JointDataTpl< Scalar, Options, JointCollectionTpl > createData(const JointModelTpl< Scalar, Options, JointCollectionTpl > &jmodel)
Visit a JointModelTpl through CreateData visitor to create a JointDataTpl.
Source from #include <cppad/example/cppad_eigen.hpp>
void SINCOS(const Scalar &a, Scalar *sa, Scalar *ca)
Computes sin/cos values of a given input scalar.
Definition: sincos.hpp:27
ConstAngularType angular() const
Return the angular part of the force vector.
Definition: force-base.hpp:40
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)
Main pinocchio namespace.
Definition: treeview.dox:24
Common traits structure to fully define base classes for CRTP.
Definition: spatial/fwd.hpp:32
Type of the cast of a class C templated by Scalar and Options, to a new NewScalar type...
Definition: spatial/fwd.hpp:43
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)