joint-revolute.hpp
Go to the documentation of this file.
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>
23  {
25  };
26 
27  template<typename Scalar, int Options, int axis, typename 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,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;
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;
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;
81  }; // traits TransformRevoluteTpl
82 
83  template<typename Scalar, int Options, int axis>
86 
87  template<typename _Scalar, int _Options, int axis>
88  struct TransformRevoluteTpl : SE3Base< TransformRevoluteTpl<_Scalar,_Options,axis> >
89  {
90  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
92 
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>
109  se3action(const SE3Tpl<S2,O2> & m) const
110  {
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 
211 
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>
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>
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 
312  }; // struct MotionRevoluteTpl
313 
314  template<typename S1, int O1, int axis, typename MotionDerived>
315  typename MotionDerived::MotionPlain
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
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>
336  { typedef Eigen::Matrix<Scalar,6,1,Options> ReturnType; };
337 
338  template<typename Scalar, int Options, int axis, typename MotionDerived>
340  { typedef Eigen::Matrix<Scalar,6,1,Options> ReturnType; };
341 
342  template<typename Scalar, int Options, int axis, typename ForceDerived>
344  { typedef typename ForceDense<ForceDerived>::ConstAngularType::template ConstFixedSegmentReturnType<1>::Type ReturnType; };
345 
346  template<typename Scalar, int Options, int axis, typename 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  };
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 
373  enum { NV = 1 };
374 
376 
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>
385  se3Action(const SE3Tpl<S1,O1> & m) const
386  {
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>
397  {
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 
409  {
411  TransposeConst(const ConstraintRevoluteTpl & ref) : ref(ref) {}
412 
413  template<typename ForceDerived>
416  { return f.angular().template segment<1>(axis); }
417 
419  template<typename Derived>
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;
440  v << Axis();
441  return S;
442  }
443 
444  template<typename MotionDerived>
447  {
449  ReturnType 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>
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>
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>
482  {
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>
513  {
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>
544  {
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  {
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 };
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 
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>
627 
628  template<typename Scalar, int Options, int axis>
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
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 
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>
668  {
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
678  PINOCCHIO_JOINT_TYPEDEF_TEMPLATE(JointDerived);
679 
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 
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>
745  {
747  ReturnType res;
748  res.setIndexes(id(),idx_q(),idx_v());
749  return res;
750  }
751 
752  }; // struct JointModelRevoluteTpl
753 
757 
761 
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__
traits< TransformRevoluteTpl< Scalar, Options, axis > >::PlainType ReturnType
JointCollectionTpl const Eigen::MatrixBase< ConfigVectorType > const Eigen::MatrixBase< TangentVectorType > & v
#define PINOCCHIO_SE3_TYPEDEF_TPL(Derived)
const Symmetric3 & inertia() const
JointDataRevoluteTpl< Scalar, Options, axis > JointDataDerived
Forward declaration of the multiplication operation return type. Should be overloaded, otherwise it will procude a compilation error.
Definition: binary-op.hpp:15
void se3Action_impl(const SE3Tpl< S2, O2 > &m, MotionDense< D2 > &v) const
bool isEqual(const TransformRevoluteTpl &other) const
JointRevoluteTpl< double, 0, 1 > JointRY
SE3GroupAction< ConstraintRevoluteTpl >::ReturnType se3ActionInverse(const SE3Tpl< S1, O1 > &m) const
MotionPlain se3ActionInverse_impl(const SE3Tpl< S2, O2 > &m) const
const std::vector< bool > hasConfigurationLimitInTangent() const
int NQ
Definition: dpendulum.py:8
MotionPlain motionAction(const MotionDense< M1 > &v) const
axis
TransformRevoluteTpl< Scalar, Options, axis > Transformation_t
int idx_q(const JointModelTpl< Scalar, Options, JointCollectionTpl > &jmodel)
Visit a JointModelTpl through JointIdxQVisitor to get the index in the full model configuration space...
JointDataRevoluteTpl< double, 0, 0 > JointDataRX
MotionAlgebraAction< ConstraintRevoluteTpl, MotionDerived >::ReturnType motionAction(const MotionDense< MotionDerived > &m) const
y
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.
#define MOTION_TYPEDEF_TPL(Derived)
PlainReturnType plain() const
PINOCCHIO_EIGEN_REF_CONST_TYPE(Matrix6Like) operator*(const Eigen
JointModelRevoluteTpl< Scalar, Options, axis > JointModelDerived
const std::vector< bool > hasConfigurationLimit() const
JointRevoluteTpl< double, 0, 2 > JointRZ
SE3GroupAction< ConstraintRevoluteTpl >::ReturnType se3Action(const SE3Tpl< S1, O1 > &m) const
const Vector3 & lever() const
void addTo(MotionDense< MotionDerived > &v) const
ConstraintForceSetOp< ConstraintRevoluteTpl, Derived >::ReturnType operator*(const Eigen::MatrixBase< Derived > &F) const
[CRBA] MatrixBase operator* (Constraint::Transpose S, ForceSet::Block)
JointIndex id(const JointModelTpl< Scalar, Options, JointCollectionTpl > &jmodel)
Visit a JointModelTpl through JointIdVisitor to get the index of the joint in the kinematic chain...
#define PINOCCHIO_JOINT_DATA_BASE_DEFAULT_ACCESSOR
Axis::CartesianAxis3 CartesianAxis3
traits< PlainType >::HomogeneousMatrixType HomogeneousMatrixType
void calc_aba(JointDataDerived &data, const Eigen::MatrixBase< Matrix6Like > &I, const bool update_I) const
void se3ActionInverse_impl(const SE3Tpl< S2, O2 > &m, MotionDense< D2 > &v) const
EIGEN_MAKE_ALIGNED_OPERATOR_NEW typedef JointRevoluteTpl< _Scalar, _Options, axis > JointDerived
EIGEN_DONT_INLINE void calc(JointDataDerived &data, const typename Eigen::MatrixBase< ConfigVector > &qs, const typename Eigen::MatrixBase< TangentVector > &vs) const
TransformRevoluteTpl(const Scalar &sin, const Scalar &cos)
JointDataRevoluteTpl< double, 0, 2 > JointDataRZ
ConstLinearType linear() const
Definition: motion-base.hpp:22
SE3GroupAction< TransformRevoluteTpl >::ReturnType se3action(const SE3Tpl< S2, O2 > &m) const
ConstAngularType angular() const
Return the angular part of the force vector.
Definition: force-base.hpp:35
JointCollectionTpl const Eigen::MatrixBase< ConfigVectorType > & q
#define PINOCCHIO_EIGEN_CONST_CAST(TYPE, OBJ)
Macro for an automatic const_cast.
void _setRotation(typename PlainType::AngularRef &rot) const
ConstraintForceOp< ConstraintRevoluteTpl, ForceDerived >::ReturnType operator*(const ForceDense< ForceDerived > &f) const
MotionDerived::MotionPlain operator+(const MotionPlanarTpl< Scalar, Options > &m1, const MotionDense< MotionDerived > &m2)
ConstAngularType angular() const
Definition: motion-base.hpp:21
MotionAlgebraAction< OtherSpatialType, Derived >::ReturnType cross(const OtherSpatialType &d) const
Definition: motion-base.hpp:87
SE3::Scalar Scalar
Definition: conversions.cpp:13
bool isEqual(const ConstraintRevoluteTpl &) const
EIGEN_MAKE_ALIGNED_OPERATOR_NEW typedef JointRevoluteTpl< _Scalar, _Options, axis > JointDerived
#define PINOCCHIO_JOINT_DATA_BASE_ACCESSOR_DEFAULT_RETURN_TYPE
JointDataDerived createData() const
JointModelRevoluteTpl< double, 0, 1 > JointModelRY
PINOCCHIO_JOINT_DATA_BASE_DEFAULT_ACCESSOR Constraint_t S
JointDataRevoluteTpl< double, 0, 1 > JointDataRY
JointModelRevoluteTpl< NewScalar, Options, axis > cast() const
#define PINOCCHIO_JOINT_DATA_TYPEDEF_TEMPLATE(Joint)
x
ForceDense< ForceDerived >::ConstAngularType::template ConstFixedSegmentReturnType< 1 >::Type ReturnType
JointModelRevoluteTpl< double, 0, 2 > JointModelRZ
const Scalar & angularRate() const
EIGEN_STRONG_INLINE void motionAction(const MotionDense< M1 > &v, MotionDense< M2 > &mout) const
MotionPlain se3Action_impl(const SE3Tpl< S2, O2 > &m) const
void setTo(MotionDense< MotionDerived > &m) const
EIGEN_DONT_INLINE void calc(JointDataDerived &data, const typename Eigen::MatrixBase< ConfigVector > &qs) const
void SINCOS(const S1 &a, S2 *sa, S3 *ca)
Computes sin/cos values of a given input scalar.
Definition: sincos.hpp:26
SpatialAxis< ANGULAR+axis > Axis
float m
void setValues(const OtherScalar &sin, const OtherScalar &cos)
SpatialAxis< axis+ANGULAR > Axis
JointMotion __mult__(const Eigen::MatrixBase< Vector1Like > &v) const
PINOCCHIO_JOINT_DATA_BASE_ACCESSOR_DEFAULT_RETURN_TYPE typedef Eigen::Matrix< Scalar, NQ, 1, Options > ConfigVector_t
#define PINOCCHIO_EIGEN_REF_TYPE(D)
Main pinocchio namespace.
Definition: timings.cpp:28
Base class for rigid transformation.
Definition: se3-base.hpp:30
res
void setIndexes(JointModelTpl< Scalar, Options, JointCollectionTpl > &jmodel, JointIndex id, int q, int v)
Visit a JointModelTpl through JointSetIndexesVisitor to set the indexes of the joint in the kinematic...
TransposeConst transpose() const
MotionRevoluteTpl(const Eigen::MatrixBase< Vector1Like > &v)
Common traits structure to fully define base classes for CRTP.
Definition: src/fwd.hpp:44
Symmetric3Tpl< double, 0 > Symmetric3
#define PINOCCHIO_CONSTRAINT_TYPEDEF_TPL(DERIVED)
MotionRevoluteTpl __mult__(const OtherScalar &alpha) const
static void motionAction(const MotionDense< MotionDerived > &v, const Eigen::MatrixBase< Mat > &iF, Eigen::MatrixBase< MatRet > const &jF)
Action of a motion on a set of forces, represented by a 6xN matrix whose each column represent a spat...
NV
Definition: dcrba.py:444
w
Definition: ur5x4.py:45
bool isEqual_impl(const MotionRevoluteTpl &other) const
Type of the cast of a class C templated by Scalar and Options, to a new NewScalar type...
Definition: src/fwd.hpp:55
MotionDerived::MotionPlain operator^(const MotionDense< MotionDerived > &m1, const MotionPlanarTpl< S2, O2 > &m2)
JointRevoluteTpl< double, 0, 0 > JointRX
Return type of the Constraint::Transpose * Force operation.
JointModelRevoluteTpl< double, 0, 0 > JointModelRX
MatrixDerived plain(const Eigen::PlainObjectBase< MatrixDerived > &m)
ReturnTypeNotDefined ReturnType
JointModelBase< JointModelRevoluteTpl > Base
ConstraintRevoluteTpl< Scalar, Options, axis > Constraint_t
MotionRevoluteTpl(const Scalar &w)
TransposeConst(const ConstraintRevoluteTpl &ref)
#define PINOCCHIO_JOINT_TYPEDEF_TEMPLATE(Joint)


pinocchio
Author(s):
autogenerated on Fri Jun 23 2023 02:38:31