pinocchio  2.3.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(2).noalias() = m_cos * m.rotation().col(2) + m_sin * m.rotation().col(0);
124  res.rotation().col(1) = m.rotation().col(1);
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(0).noalias() = m_cos * m.rotation().col(0) + m_sin * m.rotation().col(1);
131  res.rotation().col(1).noalias() = res.rotation().col(2).cross(res.rotation().col(0));
132  res.rotation().col(2) = m.rotation().col(2);
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  Scalar & sin() { return m_sin; }
147 
148  const Scalar & cos() const { return m_cos; }
149  Scalar & cos() { return m_cos; }
150 
151  template<typename OtherScalar>
152  void setValues(const OtherScalar & sin, const OtherScalar & cos)
153  { m_sin = sin; m_cos = cos; }
154 
155  LinearType translation() const { return LinearType::PlainObject::Zero(3); };
156  AngularType rotation() const {
157  AngularType m(AngularType::Identity(3));
158  _setRotation (m);
159  return m;
160  }
161 
162  bool isEqual(const TransformRevoluteTpl & other) const
163  {
164  return m_cos == other.m_cos && m_sin == other.m_sin;
165  }
166 
167  protected:
168 
169  Scalar m_sin, m_cos;
170  inline void _setRotation (typename PlainType::AngularRef& rot) const
171  {
172  switch(axis)
173  {
174  case 0:
175  {
176  rot.coeffRef(1,1) = m_cos; rot.coeffRef(1,2) = -m_sin;
177  rot.coeffRef(2,1) = m_sin; rot.coeffRef(2,2) = m_cos;
178  break;
179  }
180  case 1:
181  {
182  rot.coeffRef(0,0) = m_cos; rot.coeffRef(0,2) = m_sin;
183  rot.coeffRef(2,0) = -m_sin; rot.coeffRef(2,2) = m_cos;
184  break;
185  }
186  case 2:
187  {
188  rot.coeffRef(0,0) = m_cos; rot.coeffRef(0,1) = -m_sin;
189  rot.coeffRef(1,0) = m_sin; rot.coeffRef(1,1) = m_cos;
190  break;
191  }
192  default:
193  {
194  assert(false && "must nerver happened");
195  break;
196  }
197  }
198  }
199  };
200 
201  template<typename _Scalar, int _Options, int axis>
202  struct MotionRevoluteTpl
203  : MotionBase< MotionRevoluteTpl<_Scalar,_Options,axis> >
204  {
205  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
206 
207  MOTION_TYPEDEF_TPL(MotionRevoluteTpl);
208  typedef SpatialAxis<axis+ANGULAR> Axis;
209  typedef typename Axis::CartesianAxis3 CartesianAxis3;
210 
211  MotionRevoluteTpl() {}
212 
213  MotionRevoluteTpl(const Scalar & w) : m_w(w) {}
214 
215  template<typename Vector1Like>
216  MotionRevoluteTpl(const Eigen::MatrixBase<Vector1Like> & v)
217  : m_w(v[0])
218  {
219  using namespace Eigen;
220  EIGEN_STATIC_ASSERT_SIZE_1x1(Vector1Like);
221  }
222 
223  inline PlainReturnType plain() const { return Axis() * m_w; }
224 
225  template<typename OtherScalar>
226  MotionRevoluteTpl __mult__(const OtherScalar & alpha) const
227  {
228  return MotionRevoluteTpl(alpha*m_w);
229  }
230 
231  template<typename MotionDerived>
232  void setTo(MotionDense<MotionDerived> & m) const
233  {
234  m.linear().setZero();
235  for(Eigen::DenseIndex k = 0; k < 3; ++k)
236  m.angular()[k] = k == axis ? m_w : (Scalar)0;
237  }
238 
239  template<typename MotionDerived>
240  inline void addTo(MotionDense<MotionDerived> & v) const
241  {
242  typedef typename MotionDense<MotionDerived>::Scalar OtherScalar;
243  v.angular()[axis] += (OtherScalar)m_w;
244  }
245 
246  template<typename S2, int O2, typename D2>
247  inline void se3Action_impl(const SE3Tpl<S2,O2> & m, MotionDense<D2> & v) const
248  {
249  v.angular().noalias() = m.rotation().col(axis) * m_w;
250  v.linear().noalias() = m.translation().cross(v.angular());
251  }
252 
253  template<typename S2, int O2>
254  MotionPlain se3Action_impl(const SE3Tpl<S2,O2> & m) const
255  {
256  MotionPlain res;
257  se3Action_impl(m,res);
258  return res;
259  }
260 
261  template<typename S2, int O2, typename D2>
262  void se3ActionInverse_impl(const SE3Tpl<S2,O2> & m,
263  MotionDense<D2> & v) const
264  {
265  // Linear
266  CartesianAxis3::alphaCross(m_w,m.translation(),v.angular());
267  v.linear().noalias() = m.rotation().transpose() * v.angular();
268 
269  // Angular
270  v.angular().noalias() = m.rotation().transpose().col(axis) * m_w;
271  }
272 
273  template<typename S2, int O2>
274  MotionPlain se3ActionInverse_impl(const SE3Tpl<S2,O2> & m) const
275  {
276  MotionPlain res;
277  se3ActionInverse_impl(m,res);
278  return res;
279  }
280 
281  template<typename M1, typename M2>
282  EIGEN_STRONG_INLINE
283  void motionAction(const MotionDense<M1> & v, MotionDense<M2> & mout) const
284  {
285  // Linear
286  CartesianAxis3::alphaCross(-m_w,v.linear(),mout.linear());
287 
288  // Angular
289  CartesianAxis3::alphaCross(-m_w,v.angular(),mout.angular());
290  }
291 
292  template<typename M1>
293  MotionPlain motionAction(const MotionDense<M1> & v) const
294  {
295  MotionPlain res;
296  motionAction(v,res);
297  return res;
298  }
299 
300  Scalar & angularRate() { return m_w; }
301  const Scalar & angularRate() const { return m_w; }
302 
303  bool isEqual_impl(const MotionRevoluteTpl & other) const
304  {
305  return m_w == other.m_w;
306  }
307 
308  protected:
309 
310  Scalar m_w;
311  }; // struct MotionRevoluteTpl
312 
313  template<typename S1, int O1, int axis, typename MotionDerived>
314  typename MotionDerived::MotionPlain
315  operator+(const MotionRevoluteTpl<S1,O1,axis> & m1,
316  const MotionDense<MotionDerived> & m2)
317  {
318  typename MotionDerived::MotionPlain res(m2);
319  res += m1;
320  return res;
321  }
322 
323  template<typename MotionDerived, typename S2, int O2, int axis>
324  EIGEN_STRONG_INLINE
325  typename MotionDerived::MotionPlain
326  operator^(const MotionDense<MotionDerived> & m1, const MotionRevoluteTpl<S2,O2,axis>& m2)
327  {
328  return m2.motionAction(m1);
329  }
330 
331  template<typename Scalar, int Options, int axis> struct ConstraintRevoluteTpl;
332 
333  template<typename Scalar, int Options, int axis>
334  struct SE3GroupAction< ConstraintRevoluteTpl<Scalar,Options,axis> >
335  { typedef Eigen::Matrix<Scalar,6,1,Options> ReturnType; };
336 
337  template<typename Scalar, int Options, int axis, typename MotionDerived>
338  struct MotionAlgebraAction< ConstraintRevoluteTpl<Scalar,Options,axis>, MotionDerived >
339  { typedef Eigen::Matrix<Scalar,6,1,Options> ReturnType; };
340 
341  template<typename Scalar, int Options, int axis, typename ForceDerived>
342  struct ConstraintForceOp< ConstraintRevoluteTpl<Scalar,Options,axis>, ForceDerived>
343  { typedef typename ForceDense<ForceDerived>::ConstAngularType::template ConstFixedSegmentReturnType<1>::Type ReturnType; };
344 
345  template<typename Scalar, int Options, int axis, typename ForceSet>
346  struct ConstraintForceSetOp< ConstraintRevoluteTpl<Scalar,Options,axis>, ForceSet>
347  { typedef typename Eigen::MatrixBase<ForceSet>::ConstRowXpr ReturnType; };
348 
349  template<typename _Scalar, int _Options, int axis>
350  struct traits< ConstraintRevoluteTpl<_Scalar,_Options,axis> >
351  {
352  typedef _Scalar Scalar;
353  enum { Options = _Options };
354  enum {
355  LINEAR = 0,
356  ANGULAR = 3
357  };
359  typedef Eigen::Matrix<Scalar,1,1,Options> JointForce;
360  typedef Eigen::Matrix<Scalar,6,1,Options> DenseBase;
361  typedef DenseBase MatrixReturnType;
362  typedef const DenseBase ConstMatrixReturnType;
363  }; // traits ConstraintRevoluteTpl
364 
365  template<typename _Scalar, int _Options, int axis>
366  struct ConstraintRevoluteTpl
367  : ConstraintBase< ConstraintRevoluteTpl<_Scalar,_Options,axis> >
368  {
369  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
370 
371  PINOCCHIO_CONSTRAINT_TYPEDEF_TPL(ConstraintRevoluteTpl)
372  enum { NV = 1 };
373 
374  typedef SpatialAxis<ANGULAR+axis> Axis;
375 
377 
378  template<typename Vector1Like>
379  JointMotion __mult__(const Eigen::MatrixBase<Vector1Like> & v) const
380  { return JointMotion(v[0]); }
381 
382  template<typename S1, int O1>
384  se3Action(const SE3Tpl<S1,O1> & m) const
385  {
386  typedef typename SE3GroupAction<ConstraintRevoluteTpl>::ReturnType ReturnType;
387  ReturnType res;
388  res.template segment<3>(LINEAR) = m.translation().cross(m.rotation().col(axis));
389  res.template segment<3>(ANGULAR) = m.rotation().col(axis);
390  return res;
391  }
392 
393  template<typename S1, int O1>
395  se3ActionInverse(const SE3Tpl<S1,O1> & m) const
396  {
397  typedef typename SE3GroupAction<ConstraintRevoluteTpl>::ReturnType ReturnType;
398  typedef typename Axis::CartesianAxis3 CartesianAxis3;
399  ReturnType res;
400  res.template segment<3>(LINEAR).noalias() = m.rotation().transpose()*CartesianAxis3::cross(m.translation());
401  res.template segment<3>(ANGULAR) = m.rotation().transpose().col(axis);
402  return res;
403  }
404 
405  int nv_impl() const { return NV; }
406 
408  {
409  const ConstraintRevoluteTpl & ref;
410  TransposeConst(const ConstraintRevoluteTpl & ref) : ref(ref) {}
411 
412  template<typename ForceDerived>
413  typename ConstraintForceOp<ConstraintRevoluteTpl,ForceDerived>::ReturnType
414  operator*(const ForceDense<ForceDerived> & f) const
415  { return f.angular().template segment<1>(axis); }
416 
418  template<typename Derived>
419  typename ConstraintForceSetOp<ConstraintRevoluteTpl,Derived>::ReturnType
420  operator*(const Eigen::MatrixBase<Derived> & F) const
421  {
422  assert(F.rows()==6);
423  return F.row(ANGULAR + axis);
424  }
425  }; // struct TransposeConst
426 
427  TransposeConst transpose() const { return TransposeConst(*this); }
428 
429  /* CRBA joint operators
430  * - ForceSet::Block = ForceSet
431  * - ForceSet operator* (Inertia Y,Constraint S)
432  * - MatrixBase operator* (Constraint::Transpose S, ForceSet::Block)
433  * - SE3::act(ForceSet::Block)
434  */
435  DenseBase matrix_impl() const
436  {
437  DenseBase S;
439  v << Axis();
440  return S;
441  }
442 
443  template<typename MotionDerived>
445  motionAction(const MotionDense<MotionDerived> & m) const
446  {
448  ReturnType res;
449  MotionRef<ReturnType> v(res);
450  v = m.cross(Axis());
451  return res;
452  }
453 
454  bool isEqual(const ConstraintRevoluteTpl &) const { return true; }
455 
456  }; // struct ConstraintRevoluteTpl
457 
458  template<typename _Scalar, int _Options, int _axis>
460  {
461  typedef _Scalar Scalar;
462 
463  enum
464  {
465  Options = _Options,
466  axis = _axis
467  };
468  };
469 
470  template<typename S1, int O1,typename S2, int O2, int axis>
471  struct MultiplicationOp<InertiaTpl<S1,O1>, ConstraintRevoluteTpl<S2,O2,axis> >
472  {
473  typedef Eigen::Matrix<S2,6,1,O2> ReturnType;
474  };
475 
476  /* [CRBA] ForceSet operator* (Inertia Y,Constraint S) */
477  namespace impl
478  {
479  template<typename S1, int O1, typename S2, int O2>
481  {
482  typedef InertiaTpl<S1,O1> Inertia;
484  typedef typename MultiplicationOp<Inertia,Constraint>::ReturnType ReturnType;
485  static inline ReturnType run(const Inertia & Y,
486  const Constraint & /*constraint*/)
487  {
488  ReturnType res;
489 
490  /* Y(:,3) = ( 0,-z, y, I00+yy+zz, I01-xy , I02-xz ) */
491  const S1
492  &m = Y.mass(),
493  &x = Y.lever()[0],
494  &y = Y.lever()[1],
495  &z = Y.lever()[2];
496  const typename Inertia::Symmetric3 & I = Y.inertia();
497 
498  res <<
499  (S2)0,
500  -m*z,
501  m*y,
502  I(0,0)+m*(y*y+z*z),
503  I(0,1)-m*x*y,
504  I(0,2)-m*x*z;
505 
506  return res;
507  }
508  };
509 
510  template<typename S1, int O1, typename S2, int O2>
512  {
513  typedef InertiaTpl<S1,O1> Inertia;
515  typedef typename MultiplicationOp<Inertia,Constraint>::ReturnType ReturnType;
516  static inline ReturnType run(const Inertia & Y,
517  const Constraint & /*constraint*/)
518  {
519  ReturnType res;
520 
521  /* Y(:,4) = ( z, 0,-x, I10-xy , I11+xx+zz, I12-yz ) */
522  const S1
523  &m = Y.mass(),
524  &x = Y.lever()[0],
525  &y = Y.lever()[1],
526  &z = Y.lever()[2];
527  const typename Inertia::Symmetric3 & I = Y.inertia();
528 
529  res <<
530  m*z,
531  (S2)0,
532  -m*x,
533  I(1,0)-m*x*y,
534  I(1,1)+m*(x*x+z*z),
535  I(1,2)-m*y*z;
536 
537  return res;
538  }
539  };
540 
541  template<typename S1, int O1, typename S2, int O2>
543  {
544  typedef InertiaTpl<S1,O1> Inertia;
546  typedef typename MultiplicationOp<Inertia,Constraint>::ReturnType ReturnType;
547  static inline ReturnType run(const Inertia & Y,
548  const Constraint & /*constraint*/)
549  {
550  ReturnType res;
551 
552  /* Y(:,5) = (-y, x, 0, I20-xz , I21-yz , I22+xx+yy) */
553  const S1
554  &m = Y.mass(),
555  &x = Y.lever()[0],
556  &y = Y.lever()[1],
557  &z = Y.lever()[2];
558  const typename Inertia::Symmetric3 & I = Y.inertia();
559 
560  res <<
561  -m*y,
562  m*x,
563  (S2)0,
564  I(2,0)-m*x*z,
565  I(2,1)-m*y*z,
566  I(2,2)+m*(x*x+y*y);
567 
568  return res;
569  }
570  };
571  } // namespace impl
572 
573  template<typename M6Like,typename S2, int O2, int axis>
574  struct MultiplicationOp<Eigen::MatrixBase<M6Like>, ConstraintRevoluteTpl<S2,O2,axis> >
575  {
576  typedef typename M6Like::ConstColXpr ReturnType;
577  };
578 
579  /* [ABA] operator* (Inertia Y,Constraint S) */
580  namespace impl
581  {
582  template<typename M6Like, typename Scalar, int Options, int axis>
583  struct LhsMultiplicationOp<Eigen::MatrixBase<M6Like>, ConstraintRevoluteTpl<Scalar,Options,axis> >
584  {
586  typedef typename MultiplicationOp<Eigen::MatrixBase<M6Like>,Constraint>::ReturnType ReturnType;
587  static inline ReturnType run(const Eigen::MatrixBase<M6Like> & Y,
588  const Constraint & /*constraint*/)
589  {
590  EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(M6Like,6,6);
591  return Y.col(Inertia::ANGULAR + axis);
592  }
593  };
594  } // namespace impl
595 
596  template<typename _Scalar, int _Options, int axis>
597  struct traits< JointRevoluteTpl<_Scalar,_Options,axis> >
598  {
599  enum {
600  NQ = 1,
601  NV = 1
602  };
603  typedef _Scalar Scalar;
604  enum { Options = _Options };
611 
612  // [ABA]
613  typedef Eigen::Matrix<Scalar,6,NV,Options> U_t;
614  typedef Eigen::Matrix<Scalar,NV,NV,Options> D_t;
615  typedef Eigen::Matrix<Scalar,6,NV,Options> UD_t;
616 
617  PINOCCHIO_JOINT_DATA_BASE_ACCESSOR_DEFAULT_RETURN_TYPE
618 
619  typedef Eigen::Matrix<Scalar,NQ,1,Options> ConfigVector_t;
620  typedef Eigen::Matrix<Scalar,NV,1,Options> TangentVector_t;
621  };
622 
623  template<typename Scalar, int Options, int axis>
624  struct traits< JointDataRevoluteTpl<Scalar,Options,axis> >
626 
627  template<typename Scalar, int Options, int axis>
628  struct traits< JointModelRevoluteTpl<Scalar,Options,axis> >
630 
631  template<typename _Scalar, int _Options, int axis>
632  struct JointDataRevoluteTpl : public JointDataBase< JointDataRevoluteTpl<_Scalar,_Options,axis> >
633  {
634  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
636  PINOCCHIO_JOINT_DATA_TYPEDEF_TEMPLATE(JointDerived);
637  PINOCCHIO_JOINT_DATA_BASE_DEFAULT_ACCESSOR
638 
639  Constraint_t S;
640  Transformation_t M;
641  Motion_t v;
642  Bias_t c;
643 
644  // [ABA] specific data
645  U_t U;
646  D_t Dinv;
647  UD_t UDinv;
648 
650  : M((Scalar)0,(Scalar)1)
651  , v((Scalar)0)
652  , U(U_t::Zero())
653  , Dinv(D_t::Zero())
654  , UDinv(UD_t::Zero())
655  {}
656 
657  static std::string classname()
658  {
659  return std::string("JointDataR") + axisLabel<axis>();
660  }
661  std::string shortname() const { return classname(); }
662 
663  }; // struct JointDataRevoluteTpl
664 
665  template<typename NewScalar, typename Scalar, int Options, int axis>
666  struct CastType< NewScalar, JointModelRevoluteTpl<Scalar,Options,axis> >
667  {
669  };
670 
671  template<typename _Scalar, int _Options, int axis>
672  struct JointModelRevoluteTpl
673  : public JointModelBase< JointModelRevoluteTpl<_Scalar,_Options,axis> >
674  {
675  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
676  typedef JointRevoluteTpl<_Scalar,_Options,axis> JointDerived;
677  PINOCCHIO_JOINT_TYPEDEF_TEMPLATE(JointDerived);
678 
680  using Base::id;
681  using Base::idx_q;
682  using Base::idx_v;
683  using Base::setIndexes;
684 
685  JointDataDerived createData() const { return JointDataDerived(); }
686 
688 
689  template<typename ConfigVector>
690  EIGEN_DONT_INLINE
691  void calc(JointDataDerived & data,
692  const typename Eigen::MatrixBase<ConfigVector> & qs) const
693  {
694  typedef typename ConfigVector::Scalar OtherScalar;
695 
696  const OtherScalar & q = qs[idx_q()];
697  OtherScalar ca,sa; SINCOS(q,&sa,&ca);
698  data.M.setValues(sa,ca);
699  }
700 
701  template<typename ConfigVector, typename TangentVector>
702  EIGEN_DONT_INLINE
703  void calc(JointDataDerived & data,
704  const typename Eigen::MatrixBase<ConfigVector> & qs,
705  const typename Eigen::MatrixBase<TangentVector> & vs) const
706  {
707  calc(data,qs.derived());
708 
709  data.v.angularRate() = static_cast<Scalar>(vs[idx_v()]);
710  }
711 
712  template<typename Matrix6Like>
713  void calc_aba(JointDataDerived & data,
714  const Eigen::MatrixBase<Matrix6Like> & I,
715  const bool update_I) const
716  {
717  data.U = I.col(Inertia::ANGULAR + axis);
718  data.Dinv[0] = Scalar(1)/I(Inertia::ANGULAR + axis,Inertia::ANGULAR + axis);
719  data.UDinv.noalias() = data.U * data.Dinv[0];
720 
721  if (update_I)
722  PINOCCHIO_EIGEN_CONST_CAST(Matrix6Like,I) -= data.UDinv * data.U.transpose();
723  }
724 
725  static std::string classname()
726  {
727  return std::string("JointModelR") + axisLabel<axis>();
728  }
729  std::string shortname() const { return classname(); }
730 
732  template<typename NewScalar>
734  {
736  ReturnType res;
737  res.setIndexes(id(),idx_q(),idx_v());
738  return res;
739  }
740 
741  }; // struct JointModelRevoluteTpl
742 
746 
750 
754 
755 } //namespace pinocchio
756 
757 #include <boost/type_traits.hpp>
758 
759 namespace boost
760 {
761  template<typename Scalar, int Options, int axis>
762  struct has_nothrow_constructor< ::pinocchio::JointModelRevoluteTpl<Scalar,Options,axis> >
763  : public integral_constant<bool,true> {};
764 
765  template<typename Scalar, int Options, int axis>
766  struct has_nothrow_copy< ::pinocchio::JointModelRevoluteTpl<Scalar,Options,axis> >
767  : public integral_constant<bool,true> {};
768 
769  template<typename Scalar, int Options, int axis>
770  struct has_nothrow_constructor< ::pinocchio::JointDataRevoluteTpl<Scalar,Options,axis> >
771  : public integral_constant<bool,true> {};
772 
773  template<typename Scalar, int Options, int axis>
774  struct has_nothrow_copy< ::pinocchio::JointDataRevoluteTpl<Scalar,Options,axis> >
775  : public integral_constant<bool,true> {};
776 }
777 
778 #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>
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)
void SINCOS(const S1 &a, S2 *sa, S3 *ca)
Computes sin/cos values of a given input scalar.
Definition: sincos.hpp:27
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)
 .