joint-revolute-unaligned.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_unaligned_hpp__
7 #define __pinocchio_joint_revolute_unaligned_hpp__
8 
9 #include "pinocchio/fwd.hpp"
10 #include "pinocchio/multibody/joint/joint-base.hpp"
11 #include "pinocchio/multibody/constraint.hpp"
12 #include "pinocchio/spatial/inertia.hpp"
13 #include "pinocchio/math/matrix.hpp"
14 
15 namespace pinocchio
16 {
17 
18  template<typename Scalar, int Options=0> struct MotionRevoluteUnalignedTpl;
20 
21  template<typename Scalar, int Options>
23  {
25  };
26 
27  template<typename Scalar, int Options, typename MotionDerived>
29  {
31  };
32 
33  template<typename _Scalar, int _Options>
34  struct traits< MotionRevoluteUnalignedTpl<_Scalar,_Options> >
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 MotionRevoluteUnalignedTpl
57 
58  template<typename _Scalar, int _Options>
60  : MotionBase< MotionRevoluteUnalignedTpl<_Scalar,_Options> >
61  {
62  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
64 
66 
67  template<typename Vector3Like, typename OtherScalar>
68  MotionRevoluteUnalignedTpl(const Eigen::MatrixBase<Vector3Like> & axis,
69  const OtherScalar & w)
70  : m_axis(axis)
71  , m_w(w)
72  {}
73 
74  inline PlainReturnType plain() const
75  {
76  return PlainReturnType(PlainReturnType::Vector3::Zero(),
77  m_axis*m_w);
78  }
79 
80  template<typename OtherScalar>
81  MotionRevoluteUnalignedTpl __mult__(const OtherScalar & alpha) const
82  {
83  return MotionRevoluteUnalignedTpl(m_axis,alpha*m_w);
84  }
85 
86  template<typename MotionDerived>
87  inline void addTo(MotionDense<MotionDerived> & v) const
88  {
89  v.angular() += m_axis*m_w;
90  }
91 
92  template<typename Derived>
93  void setTo(MotionDense<Derived> & other) const
94  {
95  other.linear().setZero();
96  other.angular().noalias() = m_axis*m_w;
97  }
98 
99  template<typename S2, int O2, typename D2>
101  {
102  // Angular
103  v.angular().noalias() = m_w * m.rotation() * m_axis;
104 
105  // Linear
106  v.linear().noalias() = m.translation().cross(v.angular());
107  }
108 
109  template<typename S2, int O2>
110  MotionPlain se3Action_impl(const SE3Tpl<S2,O2> & m) const
111  {
112  MotionPlain res;
113  se3Action_impl(m,res);
114  return res;
115  }
116 
117  template<typename S2, int O2, typename D2>
119  {
120  // Linear
121  // TODO: use v.angular() as temporary variable
122  Vector3 v3_tmp;
123  v3_tmp.noalias() = m_axis.cross(m.translation());
124  v3_tmp *= m_w;
125  v.linear().noalias() = m.rotation().transpose() * v3_tmp;
126 
127  // Angular
128  v.angular().noalias() = m.rotation().transpose() * m_axis;
129  v.angular() *= m_w;
130  }
131 
132  template<typename S2, int O2>
133  MotionPlain se3ActionInverse_impl(const SE3Tpl<S2,O2> & m) const
134  {
135  MotionPlain res;
136  se3ActionInverse_impl(m,res);
137  return res;
138  }
139 
140  template<typename M1, typename M2>
141  void motionAction(const MotionDense<M1> & v, MotionDense<M2> & mout) const
142  {
143  // Linear
144  mout.linear().noalias() = v.linear().cross(m_axis);
145  mout.linear() *= m_w;
146 
147  // Angular
148  mout.angular().noalias() = v.angular().cross(m_axis);
149  mout.angular() *= m_w;
150  }
151 
152  template<typename M1>
153  MotionPlain motionAction(const MotionDense<M1> & v) const
154  {
155  MotionPlain res;
156  motionAction(v,res);
157  return res;
158  }
159 
160  bool isEqual_impl(const MotionRevoluteUnalignedTpl & other) const
161  {
162  return m_axis == other.m_axis && m_w == other.m_w;
163  }
164 
165  const Scalar & angularRate() const { return m_w; }
166  Scalar & angularRate() { return m_w; }
167 
168  const Vector3 & axis() const { return m_axis; }
169  Vector3 & axis() { return m_axis; }
170 
171  protected:
172  Vector3 m_axis;
174 
175  }; // struct MotionRevoluteUnalignedTpl
176 
177  template<typename S1, int O1, typename MotionDerived>
178  inline typename MotionDerived::MotionPlain
180  const MotionDense<MotionDerived> & m2)
181  {
182  typename MotionDerived::MotionPlain res(m2);
183  res += m1;
184  return res;
185  }
186 
187  template<typename MotionDerived, typename S2, int O2>
188  inline typename MotionDerived::MotionPlain
191  {
192  return m2.motionAction(m1);
193  }
194 
195  template<typename Scalar, int Options> struct ConstraintRevoluteUnalignedTpl;
196 
197  template<typename _Scalar, int _Options>
198  struct traits< ConstraintRevoluteUnalignedTpl<_Scalar,_Options> >
199  {
200  typedef _Scalar Scalar;
201  enum { Options = _Options };
202  enum {
203  LINEAR = 0,
204  ANGULAR = 3
205  };
207  typedef Eigen::Matrix<Scalar,1,1,Options> JointForce;
208  typedef Eigen::Matrix<Scalar,6,1,Options> DenseBase;
209  typedef DenseBase MatrixReturnType;
210  typedef const DenseBase ConstMatrixReturnType;
211 
212  typedef Eigen::Matrix<Scalar,3,1,Options> Vector3;
213  }; // traits ConstraintRevoluteUnalignedTpl
214 
215  template<typename Scalar, int Options>
217  { typedef Eigen::Matrix<Scalar,6,1,Options> ReturnType; };
218 
219  template<typename Scalar, int Options, typename MotionDerived>
221  { typedef Eigen::Matrix<Scalar,6,1,Options> ReturnType; };
222 
223  template<typename Scalar, int Options, typename ForceDerived>
225  {
227  typedef Eigen::Matrix<typename PINOCCHIO_EIGEN_DOT_PRODUCT_RETURN_TYPE(Vector3,typename ForceDense<ForceDerived>::ConstAngularType),1,1,Options> ReturnType;
228  };
229 
230  template<typename Scalar, int Options, typename ForceSet>
232  {
235  typename Eigen::MatrixBase<const ForceSet>::template NRowsBlockXpr<3>::Type
237  };
238 
239  template<typename _Scalar, int _Options>
241  : ConstraintBase< ConstraintRevoluteUnalignedTpl<_Scalar,_Options> >
242  {
243  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
245 
246  enum { NV = 1 };
247 
249 
251 
252  template<typename Vector3Like>
253  ConstraintRevoluteUnalignedTpl(const Eigen::MatrixBase<Vector3Like> & axis)
254  : m_axis(axis)
255  {}
256 
257  template<typename Vector1Like>
258  JointMotion __mult__(const Eigen::MatrixBase<Vector1Like> & v) const
259  {
260  EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Vector1Like,1);
261  return JointMotion(m_axis,v[0]);
262  }
263 
264  template<typename S1, int O1>
266  se3Action(const SE3Tpl<S1,O1> & m) const
267  {
269 
270  /* X*S = [ R pxR ; 0 R ] [ 0 ; a ] = [ px(Ra) ; Ra ] */
271  ReturnType res;
272  res.template segment<3>(ANGULAR).noalias() = m.rotation() * m_axis;
273  res.template segment<3>(LINEAR).noalias() = m.translation().cross(res.template segment<3>(ANGULAR));
274  return res;
275  }
276 
277  template<typename S1, int O1>
280  {
282 
283  ReturnType res;
284  res.template segment<3>(ANGULAR).noalias() = m.rotation().transpose() * m_axis;
285  res.template segment<3>(LINEAR).noalias() = -m.rotation().transpose() * m.translation().cross(m_axis);
286  return res;
287  }
288 
289  int nv_impl() const { return NV; }
290 
292  {
295 
296  template<typename ForceDerived>
299  {
301  ReturnType res;
302  res[0] = ref.axis().dot(f.angular());
303  return res;
304  }
305 
306  /* [CRBA] MatrixBase operator* (Constraint::Transpose S, ForceSet::Block) */
307  template<typename ForceSet>
309  operator*(const Eigen::MatrixBase<ForceSet> & F)
310  {
311  EIGEN_STATIC_ASSERT(ForceSet::RowsAtCompileTime==6,THIS_METHOD_IS_ONLY_FOR_MATRICES_OF_A_SPECIFIC_SIZE)
312  /* Return ax.T * F[3:end,:] */
313  return ref.axis().transpose() * F.template middleRows<3>(ANGULAR);
314  }
315 
316  };
317 
318  TransposeConst transpose() const { return TransposeConst(*this); }
319 
320  /* CRBA joint operators
321  * - ForceSet::Block = ForceSet
322  * - ForceSet operator* (Inertia Y,Constraint S)
323  * - MatrixBase operator* (Constraint::Transpose S, ForceSet::Block)
324  * - SE3::act(ForceSet::Block)
325  */
326  DenseBase matrix_impl() const
327  {
328  DenseBase S;
329  S.template segment<3>(LINEAR).setZero();
330  S.template segment<3>(ANGULAR) = m_axis;
331  return S;
332  }
333 
334  template<typename MotionDerived>
337  {
338  const typename MotionDerived::ConstLinearType v = m.linear();
339  const typename MotionDerived::ConstAngularType w = m.angular();
340 
341  DenseBase res;
342  res.template segment<3>(LINEAR).noalias() = v.cross(m_axis);
343  res.template segment<3>(ANGULAR).noalias() = w.cross(m_axis);
344 
345  return res;
346  }
347 
348  const Vector3 & axis() const { return m_axis; }
349  Vector3 & axis() { return m_axis; }
350 
351  bool isEqual(const ConstraintRevoluteUnalignedTpl & other) const
352  {
353  return m_axis == other.m_axis;
354  }
355 
356  protected:
357 
358  Vector3 m_axis;
359 
360  }; // struct ConstraintRevoluteUnalignedTpl
361 
362  template<typename S1, int O1,typename S2, int O2>
364  {
365  typedef Eigen::Matrix<S2,6,1,O2> ReturnType;
366  };
367 
368  /* [CRBA] ForceSet operator* (Inertia Y,Constraint S) */
369  namespace impl
370  {
371  template<typename S1, int O1, typename S2, int O2>
373  {
377  static inline ReturnType run(const Inertia & Y,
378  const Constraint & cru)
379  {
380  ReturnType res;
381 
382  /* YS = [ m -mcx ; mcx I-mcxcx ] [ 0 ; w ] = [ mcxw ; Iw -mcxcxw ] */
383  const typename Inertia::Scalar & m = Y.mass();
384  const typename Inertia::Vector3 & c = Y.lever();
385  const typename Inertia::Symmetric3 & I = Y.inertia();
386 
387  res.template segment<3>(Inertia::LINEAR) = -m*c.cross(cru.axis());
388  res.template segment<3>(Inertia::ANGULAR).noalias() = I*cru.axis();
389  res.template segment<3>(Inertia::ANGULAR) += c.cross(res.template segment<3>(Inertia::LINEAR));
390 
391  return res;
392  }
393  };
394  } // namespace impl
395 
396  template<typename M6Like, typename Scalar, int Options>
397  struct MultiplicationOp<Eigen::MatrixBase<M6Like>, ConstraintRevoluteUnalignedTpl<Scalar,Options> >
398  {
399  typedef typename SizeDepType<3>::ColsReturn<M6Like>::ConstType M6LikeCols;
401 
403  typedef typename Constraint::Vector3 Vector3;
405  };
406 
407  /* [ABA] operator* (Inertia Y,Constraint S) */
408  namespace impl
409  {
410  template<typename M6Like, typename Scalar, int Options>
411  struct LhsMultiplicationOp<Eigen::MatrixBase<M6Like>, ConstraintRevoluteUnalignedTpl<Scalar,Options> >
412  {
415 
416  static inline ReturnType run(const Eigen::MatrixBase<M6Like> & Y,
417  const Constraint & cru)
418  {
419  EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(M6Like,6,6);
420  return Y.derived().template middleCols<3>(Constraint::ANGULAR) * cru.axis();
421  }
422  };
423  } // namespace impl
424 
425  template<typename Scalar, int Options> struct JointRevoluteUnalignedTpl;
426 
427  template<typename _Scalar, int _Options>
428  struct traits< JointRevoluteUnalignedTpl<_Scalar,_Options> >
429  {
430  enum {
431  NQ = 1,
432  NV = 1
433  };
434  typedef _Scalar Scalar;
435  enum { Options = _Options };
442 
443  // [ABA]
444  typedef Eigen::Matrix<Scalar,6,NV,Options> U_t;
445  typedef Eigen::Matrix<Scalar,NV,NV,Options> D_t;
446  typedef Eigen::Matrix<Scalar,6,NV,Options> UD_t;
447 
449 
450  typedef Eigen::Matrix<Scalar,NQ,1,Options> ConfigVector_t;
451  typedef Eigen::Matrix<Scalar,NV,1,Options> TangentVector_t;
452 
453  };
454 
455  template<typename Scalar, int Options>
458 
459  template<typename Scalar, int Options>
462 
463  template<typename _Scalar, int _Options>
465  : public JointDataBase< JointDataRevoluteUnalignedTpl<_Scalar,_Options> >
466  {
467  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
471 
472  Transformation_t M;
473  Constraint_t S;
474  Motion_t v;
475  Bias_t c;
476 
477  // [ABA] specific data
478  U_t U;
479  D_t Dinv;
480  UD_t UDinv;
481 
483  : M(Transformation_t::Identity())
484  , S(Constraint_t::Vector3::Zero())
485  , v(Constraint_t::Vector3::Zero(),(Scalar)0)
486  , U(U_t::Zero())
487  , Dinv(D_t::Zero())
488  , UDinv(UD_t::Zero())
489  {}
490 
491  template<typename Vector3Like>
492  JointDataRevoluteUnalignedTpl(const Eigen::MatrixBase<Vector3Like> & axis)
493  : M(Transformation_t::Identity())
494  , S(axis)
495  , v(axis,(Scalar)NAN)
496  , U(U_t::Zero())
497  , Dinv(D_t::Zero())
498  , UDinv(UD_t::Zero())
499  {}
500 
501  static std::string classname() { return std::string("JointDataRevoluteUnaligned"); }
502  std::string shortname() const { return classname(); }
503 
504  }; // struct JointDataRevoluteUnalignedTpl
505 
507  template<typename _Scalar, int _Options>
509  : public JointModelBase< JointModelRevoluteUnalignedTpl<_Scalar,_Options> >
510  {
511  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
513  PINOCCHIO_JOINT_TYPEDEF_TEMPLATE(JointDerived);
514  typedef Eigen::Matrix<Scalar,3,1,_Options> Vector3;
515 
517  using Base::id;
518  using Base::idx_q;
519  using Base::idx_v;
520  using Base::setIndexes;
521 
523 
525  const Scalar & y,
526  const Scalar & z)
527  : axis(x,y,z)
528  {
529  axis.normalize();
530  assert(isUnitary(axis) && "Rotation axis is not unitary");
531  }
532 
533  template<typename Vector3Like>
534  JointModelRevoluteUnalignedTpl(const Eigen::MatrixBase<Vector3Like> & axis)
535  : axis(axis)
536  {
537  EIGEN_STATIC_ASSERT_VECTOR_ONLY(Vector3Like);
538  assert(isUnitary(axis) && "Rotation axis is not unitary");
539  }
540 
541  JointDataDerived createData() const { return JointDataDerived(axis); }
542 
543  using Base::isEqual;
544  bool isEqual(const JointModelRevoluteUnalignedTpl & other) const
545  {
546  return Base::isEqual(other) && axis == other.axis;
547  }
548 
549  const std::vector<bool> hasConfigurationLimit() const
550  {
551  return {true};
552  }
553 
554  const std::vector<bool> hasConfigurationLimitInTangent() const
555  {
556  return {true};
557  }
558 
559  template<typename ConfigVector>
560  void calc(JointDataDerived & data,
561  const typename Eigen::MatrixBase<ConfigVector> & qs) const
562  {
563  typedef typename ConfigVector::Scalar OtherScalar;
564  typedef Eigen::AngleAxis<Scalar> AngleAxis;
565 
566  const OtherScalar & q = qs[idx_q()];
567 
568  data.M.rotation(AngleAxis(q,axis).toRotationMatrix());
569  }
570 
571  template<typename ConfigVector, typename TangentVector>
572  void calc(JointDataDerived & data,
573  const typename Eigen::MatrixBase<ConfigVector> & qs,
574  const typename Eigen::MatrixBase<TangentVector> & vs) const
575  {
576  calc(data,qs.derived());
577 
578  data.v.angularRate() = static_cast<Scalar>(vs[idx_v()]);
579  }
580 
581  template<typename Matrix6Like>
582  void calc_aba(JointDataDerived & data, const Eigen::MatrixBase<Matrix6Like> & I, const bool update_I) const
583  {
584  data.U.noalias() = I.template middleCols<3>(Motion::ANGULAR) * axis;
585  data.Dinv[0] = (Scalar)(1)/axis.dot(data.U.template segment<3>(Motion::ANGULAR));
586  data.UDinv.noalias() = data.U * data.Dinv;
587 
588  if (update_I)
589  PINOCCHIO_EIGEN_CONST_CAST(Matrix6Like,I) -= data.UDinv * data.U.transpose();
590  }
591 
592  static std::string classname() { return std::string("JointModelRevoluteUnaligned"); }
593  std::string shortname() const { return classname(); }
594 
596  template<typename NewScalar>
598  {
600  ReturnType res(axis.template cast<NewScalar>());
601  res.setIndexes(id(),idx_q(),idx_v());
602  return res;
603  }
604 
605  // data
606 
610  Vector3 axis;
611  }; // struct JointModelRevoluteUnalignedTpl
612 
613 } //namespace pinocchio
614 
615 #include <boost/type_traits.hpp>
616 
617 namespace boost
618 {
619  template<typename Scalar, int Options>
620  struct has_nothrow_constructor< ::pinocchio::JointModelRevoluteUnalignedTpl<Scalar,Options> >
621  : public integral_constant<bool,true> {};
622 
623  template<typename Scalar, int Options>
624  struct has_nothrow_copy< ::pinocchio::JointModelRevoluteUnalignedTpl<Scalar,Options> >
625  : public integral_constant<bool,true> {};
626 
627  template<typename Scalar, int Options>
628  struct has_nothrow_constructor< ::pinocchio::JointDataRevoluteUnalignedTpl<Scalar,Options> >
629  : public integral_constant<bool,true> {};
630 
631  template<typename Scalar, int Options>
632  struct has_nothrow_copy< ::pinocchio::JointDataRevoluteUnalignedTpl<Scalar,Options> >
633  : public integral_constant<bool,true> {};
634 }
635 
636 
637 #endif // ifndef __pinocchio_joint_revolute_unaligned_hpp__
JointCollectionTpl const Eigen::MatrixBase< ConfigVectorType > const Eigen::MatrixBase< TangentVectorType > & v
const Symmetric3 & inertia() const
void calc(JointDataDerived &data, const typename Eigen::MatrixBase< ConfigVector > &qs, const typename Eigen::MatrixBase< TangentVector > &vs) const
Forward declaration of the multiplication operation return type. Should be overloaded, otherwise it will procude a compilation error.
Definition: binary-op.hpp:15
int NQ
Definition: dpendulum.py:8
axis
ConstraintForceOp< ConstraintRevoluteUnalignedTpl, ForceDerived >::ReturnType operator*(const ForceDense< ForceDerived > &f) const
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...
MotionPlain se3ActionInverse_impl(const SE3Tpl< S2, O2 > &m) const
Return type of the Constraint::Transpose * ForceSet operation.
void motionAction(const MotionDense< M1 > &v, MotionDense< M2 > &mout) const
MatrixMatrixProduct< Eigen::Transpose< const Vector3 >, typename Eigen::MatrixBase< const ForceSet >::template NRowsBlockXpr< 3 >::Type >::type ReturnType
void calc_aba(JointDataDerived &data, const Eigen::MatrixBase< Matrix6Like > &I, const bool update_I) const
Return type of the ation of a Motion onto an object of type D.
SE3GroupAction< ConstraintRevoluteUnalignedTpl >::ReturnType se3ActionInverse(const SE3Tpl< S1, O1 > &m) const
#define MOTION_TYPEDEF_TPL(Derived)
PINOCCHIO_EIGEN_REF_CONST_TYPE(Matrix6Like) operator*(const Eigen
void addTo(MotionDense< MotionDerived > &v) const
MotionPlain motionAction(const MotionDense< M1 > &v) const
PINOCCHIO_JOINT_DATA_BASE_ACCESSOR_DEFAULT_RETURN_TYPE typedef Eigen::Matrix< Scalar, NQ, 1, Options > ConfigVector_t
const Vector3 & lever() const
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
MotionPlain se3Action_impl(const SE3Tpl< S2, O2 > &m) const
SE3GroupAction< ConstraintRevoluteUnalignedTpl >::ReturnType se3Action(const SE3Tpl< S1, O1 > &m) const
bool isEqual(const JointModelRevoluteUnalignedTpl &other) const
Eigen::Matrix< Scalar, 3, 1, _Options > Vector3
Vec3f c
bool isEqual(const ConstraintRevoluteUnalignedTpl &other) const
MotionRevoluteUnalignedTpl(const Eigen::MatrixBase< Vector3Like > &axis, const OtherScalar &w)
ConstLinearType linear() const
Definition: motion-base.hpp:22
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.
bool isUnitary(const Eigen::MatrixBase< MatrixLike > &mat, const typename MatrixLike::RealScalar &prec=Eigen::NumTraits< typename MatrixLike::Scalar >::dummy_precision())
Check whether the input matrix is Unitary within the given precision.
MotionDerived::MotionPlain operator+(const MotionPlanarTpl< Scalar, Options > &m1, const MotionDense< MotionDerived > &m2)
ConstAngularType angular() const
Definition: motion-base.hpp:21
SE3::Scalar Scalar
Definition: conversions.cpp:13
virtual bool isEqual(const CollisionGeometry &other) const=0
const std::vector< bool > hasConfigurationLimit() const
#define PINOCCHIO_JOINT_DATA_BASE_ACCESSOR_DEFAULT_RETURN_TYPE
JointModelBase< JointModelRevoluteUnalignedTpl > Base
void setTo(MotionDense< Derived > &other) const
#define PINOCCHIO_JOINT_DATA_TYPEDEF_TEMPLATE(Joint)
MotionRevoluteUnalignedTpl __mult__(const OtherScalar &alpha) const
MotionAlgebraAction< ConstraintRevoluteUnalignedTpl, MotionDerived >::ReturnType motionAction(const MotionDense< MotionDerived > &m) const
MotionRevoluteUnalignedTpl< double > MotionRevoluteUnaligned
PINOCCHIO_JOINT_CAST_TYPE_SPECIALIZATION(JointModelFreeFlyerTpl)
float m
#define PINOCCHIO_EIGEN_REF_TYPE(D)
JointModelRevoluteUnalignedTpl(const Eigen::MatrixBase< Vector3Like > &axis)
PINOCCHIO_JOINT_DATA_BASE_DEFAULT_ACCESSOR Transformation_t M
bool isEqual_impl(const MotionRevoluteUnalignedTpl &other) const
JointModelRevoluteUnalignedTpl(const Scalar &x, const Scalar &y, const Scalar &z)
Main pinocchio namespace.
Definition: timings.cpp:28
Eigen::AngleAxis< FCL_REAL > AngleAxis
Eigen::Matrix< typename PINOCCHIO_EIGEN_DOT_PRODUCT_RETURN_TYPE(Vector3, typename ForceDense< ForceDerived >::ConstAngularType), 1, 1, Options > ReturnType
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...
void se3Action_impl(const SE3Tpl< S2, O2 > &m, MotionDense< D2 > &v) const
void calc(JointDataDerived &data, const typename Eigen::MatrixBase< ConfigVector > &qs) const
EIGEN_MAKE_ALIGNED_OPERATOR_NEW typedef JointRevoluteUnalignedTpl< _Scalar, _Options > JointDerived
void setZero(std::vector< MatType, Eigen::aligned_allocator< MatType > > &Ms)
Common traits structure to fully define base classes for CRTP.
Definition: src/fwd.hpp:44
Symmetric3Tpl< double, 0 > Symmetric3
void se3ActionInverse_impl(const SE3Tpl< S2, O2 > &m, MotionDense< D2 > &v) const
JointModelRevoluteUnalignedTpl< NewScalar, Options > cast() const
void toRotationMatrix(const Eigen::MatrixBase< Vector3 > &axis, const Scalar &cos_value, const Scalar &sin_value, const Eigen::MatrixBase< Matrix3 > &res)
Computes a rotation matrix from a vector and values of sin and cos orientations values.
Definition: rotation.hpp:24
#define PINOCCHIO_CONSTRAINT_TYPEDEF_TPL(DERIVED)
ConstraintForceSetOp< ConstraintRevoluteUnalignedTpl, ForceSet >::ReturnType operator*(const Eigen::MatrixBase< ForceSet > &F)
Eigen::ProductReturnType< M1, M2 >::Type type
Definition: math/matrix.hpp:69
traits< ConstraintRevoluteUnalignedTpl >::Vector3 Vector3
JointModelRevoluteUnalignedTpl< Scalar, Options > JointModelDerived
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...
JointMotion __mult__(const Eigen::MatrixBase< Vector1Like > &v) const
NV
Definition: dcrba.py:444
w
Definition: ur5x4.py:45
MotionDerived::MotionPlain operator^(const MotionDense< MotionDerived > &m1, const MotionPlanarTpl< S2, O2 > &m2)
EIGEN_MAKE_ALIGNED_OPERATOR_NEW typedef JointRevoluteUnalignedTpl< _Scalar, _Options > JointDerived
Return type of the Constraint::Transpose * Force operation.
JointDataRevoluteUnalignedTpl(const Eigen::MatrixBase< Vector3Like > &axis)
ReturnTypeNotDefined ReturnType
ConstraintRevoluteUnalignedTpl(const Eigen::MatrixBase< Vector3Like > &axis)
const std::vector< bool > hasConfigurationLimitInTangent() const
#define PINOCCHIO_JOINT_TYPEDEF_TEMPLATE(Joint)


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