joint-spherical.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_spherical_hpp__
7 #define __pinocchio_joint_spherical_hpp__
8 
9 #include "pinocchio/macros.hpp"
10 #include "pinocchio/multibody/joint/joint-base.hpp"
11 #include "pinocchio/multibody/constraint.hpp"
12 #include "pinocchio/math/sincos.hpp"
13 #include "pinocchio/spatial/inertia.hpp"
14 #include "pinocchio/spatial/skew.hpp"
15 
16 namespace pinocchio
17 {
18 
19  template<typename Scalar, int Options = 0> struct MotionSphericalTpl;
21 
22  template<typename Scalar, int Options>
24  {
26  };
27 
28  template<typename Scalar, int Options, typename MotionDerived>
30  {
32  };
33 
34  template<typename _Scalar, int _Options>
35  struct traits< MotionSphericalTpl<_Scalar,_Options> >
36  {
37  typedef _Scalar Scalar;
38  enum { Options = _Options };
39  typedef Eigen::Matrix<Scalar,3,1,Options> Vector3;
40  typedef Eigen::Matrix<Scalar,6,1,Options> Vector6;
41  typedef Eigen::Matrix<Scalar,4,4,Options> Matrix4;
42  typedef Eigen::Matrix<Scalar,6,6,Options> Matrix6;
43  typedef typename PINOCCHIO_EIGEN_REF_CONST_TYPE(Vector6) ToVectorConstReturnType;
44  typedef typename PINOCCHIO_EIGEN_REF_TYPE(Vector6) ToVectorReturnType;
45  typedef Vector3 AngularType;
46  typedef Vector3 LinearType;
47  typedef const Vector3 ConstAngularType;
48  typedef const Vector3 ConstLinearType;
49  typedef Matrix6 ActionMatrixType;
50  typedef Matrix4 HomogeneousMatrixType;
52  typedef MotionPlain PlainReturnType;
53  enum {
54  LINEAR = 0,
55  ANGULAR = 3
56  };
57  }; // traits MotionSphericalTpl
58 
59  template<typename _Scalar, int _Options>
60  struct MotionSphericalTpl
61  : MotionBase< MotionSphericalTpl<_Scalar,_Options> >
62  {
63  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
64 
66 
68 
69  template<typename Vector3Like>
70  MotionSphericalTpl(const Eigen::MatrixBase<Vector3Like> & w)
71  : m_w(w)
72  {}
73 
74  Vector3 & operator() () { return m_w; }
75  const Vector3 & operator() () const { return m_w; }
76 
77  inline PlainReturnType plain() const
78  {
79  return PlainReturnType(PlainReturnType::Vector3::Zero(), m_w);
80  }
81 
82  template<typename MotionDerived>
83  void addTo(MotionDense<MotionDerived> & other) const
84  {
85  other.angular() += m_w;
86  }
87 
88  template<typename Derived>
89  void setTo(MotionDense<Derived> & other) const
90  {
91  other.linear().setZero();
92  other.angular() = m_w;
93  }
94 
96  {
97  return MotionSphericalTpl(m_w + other.m_w);
98  }
99 
100  bool isEqual_impl(const MotionSphericalTpl & other) const
101  {
102  return m_w == other.m_w;
103  }
104 
105  template<typename MotionDerived>
106  bool isEqual_impl(const MotionDense<MotionDerived> & other) const
107  {
108  return other.angular() == m_w && other.linear().isZero(0);
109  }
110 
111  template<typename S2, int O2, typename D2>
113  {
114  // Angular
115  v.angular().noalias() = m.rotation() * m_w;
116 
117  // Linear
118  v.linear().noalias() = m.translation().cross(v.angular());
119  }
120 
121  template<typename S2, int O2>
122  MotionPlain se3Action_impl(const SE3Tpl<S2,O2> & m) const
123  {
124  MotionPlain res;
125  se3Action_impl(m,res);
126  return res;
127  }
128 
129  template<typename S2, int O2, typename D2>
131  {
132  // Linear
133  // TODO: use v.angular() as temporary variable
134  Vector3 v3_tmp;
135  v3_tmp.noalias() = m_w.cross(m.translation());
136  v.linear().noalias() = m.rotation().transpose() * v3_tmp;
137 
138  // Angular
139  v.angular().noalias() = m.rotation().transpose() * m_w;
140  }
141 
142  template<typename S2, int O2>
143  MotionPlain se3ActionInverse_impl(const SE3Tpl<S2,O2> & m) const
144  {
145  MotionPlain res;
146  se3ActionInverse_impl(m,res);
147  return res;
148  }
149 
150  template<typename M1, typename M2>
151  void motionAction(const MotionDense<M1> & v, MotionDense<M2> & mout) const
152  {
153  // Linear
154  mout.linear().noalias() = v.linear().cross(m_w);
155 
156  // Angular
157  mout.angular().noalias() = v.angular().cross(m_w);
158  }
159 
160  template<typename M1>
161  MotionPlain motionAction(const MotionDense<M1> & v) const
162  {
163  MotionPlain res;
164  motionAction(v,res);
165  return res;
166  }
167 
168  const Vector3 & angular() const { return m_w; }
169  Vector3 & angular() { return m_w; }
170 
171  protected:
172 
173  Vector3 m_w;
174  }; // struct MotionSphericalTpl
175 
176  template<typename S1, int O1, typename MotionDerived>
177  inline typename MotionDerived::MotionPlain
179  {
180  return typename MotionDerived::MotionPlain(m2.linear(),m2.angular() + m1.angular());
181  }
182 
183  template<typename Scalar, int Options> struct ConstraintSphericalTpl;
184 
185  template<typename _Scalar, int _Options>
186  struct traits< ConstraintSphericalTpl<_Scalar,_Options> >
187  {
188  typedef _Scalar Scalar;
189  enum { Options = _Options };
190  enum {
191  LINEAR = 0,
192  ANGULAR = 3
193  };
195  typedef Eigen::Matrix<Scalar,3,1,Options> JointForce;
196  typedef Eigen::Matrix<Scalar,6,3,Options> DenseBase;
197  typedef DenseBase MatrixReturnType;
198  typedef const DenseBase ConstMatrixReturnType;
199  }; // struct traits struct ConstraintSphericalTpl
200 
201  template<typename _Scalar, int _Options>
203  : public ConstraintBase< ConstraintSphericalTpl<_Scalar,_Options> >
204  {
205  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
206 
208 
210 
211  enum { NV = 3 };
212 
213  int nv_impl() const { return NV; }
214 
215  template<typename Vector3Like>
216  JointMotion __mult__(const Eigen::MatrixBase<Vector3Like> & w) const
217  {
218  EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Vector3Like,3);
219  return JointMotion(w);
220  }
221 
223  {
224  template<typename Derived>
227  { return phi.angular(); }
228 
229  /* [CRBA] MatrixBase operator* (Constraint::Transpose S, ForceSet::Block) */
230  template<typename MatrixDerived>
231  const typename SizeDepType<3>::RowsReturn<MatrixDerived>::ConstType
232  operator*(const Eigen::MatrixBase<MatrixDerived> & F) const
233  {
234  assert(F.rows()==6);
235  return F.derived().template middleRows<3>(Inertia::ANGULAR);
236  }
237  };
238 
240 
241  DenseBase matrix_impl() const
242  {
243  DenseBase S;
244  S.template block <3,3>(LINEAR,0).setZero();
245  S.template block <3,3>(ANGULAR,0).setIdentity();
246  return S;
247  }
248 
249  template<typename S1, int O1>
250  Eigen::Matrix<S1,6,3,O1> se3Action(const SE3Tpl<S1,O1> & m) const
251  {
252  Eigen::Matrix<S1,6,3,O1> X_subspace;
253  cross(m.translation(),m.rotation(),X_subspace.template middleRows<3>(LINEAR));
254  X_subspace.template middleRows<3>(ANGULAR) = m.rotation();
255 
256  return X_subspace;
257  }
258 
259  template<typename S1, int O1>
260  Eigen::Matrix<S1,6,3,O1> se3ActionInverse(const SE3Tpl<S1,O1> & m) const
261  {
262  Eigen::Matrix<S1,6,3,O1> X_subspace;
263  AxisX::cross(m.translation(),X_subspace.template middleRows<3>(ANGULAR).col(0));
264  AxisY::cross(m.translation(),X_subspace.template middleRows<3>(ANGULAR).col(1));
265  AxisZ::cross(m.translation(),X_subspace.template middleRows<3>(ANGULAR).col(2));
266 
267  X_subspace.template middleRows<3>(LINEAR).noalias()
268  = m.rotation().transpose() * X_subspace.template middleRows<3>(ANGULAR);
269  X_subspace.template middleRows<3>(ANGULAR) = m.rotation().transpose();
270 
271  return X_subspace;
272  }
273 
274  template<typename MotionDerived>
275  DenseBase motionAction(const MotionDense<MotionDerived> & m) const
276  {
277  const typename MotionDerived::ConstLinearType v = m.linear();
278  const typename MotionDerived::ConstAngularType w = m.angular();
279 
280  DenseBase res;
281  skew(v,res.template middleRows<3>(LINEAR));
282  skew(w,res.template middleRows<3>(ANGULAR));
283 
284  return res;
285  }
286 
287  bool isEqual(const ConstraintSphericalTpl &) const { return true; }
288 
289  }; // struct ConstraintSphericalTpl
290 
291  template<typename MotionDerived, typename S2, int O2>
292  inline typename MotionDerived::MotionPlain
294  const MotionSphericalTpl<S2,O2> & m2)
295  {
296  return m2.motionAction(m1);
297  }
298 
299  /* [CRBA] ForceSet operator* (Inertia Y,Constraint S) */
300  template<typename S1, int O1, typename S2, int O2>
301  inline Eigen::Matrix<S2,6,3,O2>
304  {
305  typedef InertiaTpl<S1,O1> Inertia;
306  typedef typename Inertia::Symmetric3 Symmetric3;
307  Eigen::Matrix<S2,6,3,O2> M;
308  // M.block <3,3> (Inertia::LINEAR, 0) = - Y.mass () * skew(Y.lever ());
309  M.template block<3,3>(Inertia::LINEAR,0) = alphaSkew(-Y.mass(), Y.lever());
310  M.template block<3,3>(Inertia::ANGULAR,0) = (Y.inertia() - typename Symmetric3::AlphaSkewSquare(Y.mass(), Y.lever())).matrix();
311  return M;
312  }
313 
314  /* [ABA] Y*S operator*/
315  template<typename M6Like, typename S2, int O2>
316  inline typename SizeDepType<3>::ColsReturn<M6Like>::ConstType
317  operator*(const Eigen::MatrixBase<M6Like> & Y,
319  {
320  typedef ConstraintSphericalTpl<S2,O2> Constraint;
321  return Y.derived().template middleCols<3>(Constraint::ANGULAR);
322  }
323 
324  template<typename S1, int O1>
326  { typedef Eigen::Matrix<S1,6,3,O1> ReturnType; };
327 
328  template<typename S1, int O1, typename MotionDerived>
329  struct MotionAlgebraAction< ConstraintSphericalTpl<S1,O1>,MotionDerived >
330  { typedef Eigen::Matrix<S1,6,3,O1> ReturnType; };
331 
332  template<typename Scalar, int Options> struct JointSphericalTpl;
333 
334  template<typename _Scalar, int _Options>
335  struct traits< JointSphericalTpl<_Scalar,_Options> >
336  {
337  enum {
338  NQ = 4,
339  NV = 3
340  };
341  typedef _Scalar Scalar;
342  enum { Options = _Options };
349 
350  // [ABA]
351  typedef Eigen::Matrix<Scalar,6,NV,Options> U_t;
352  typedef Eigen::Matrix<Scalar,NV,NV,Options> D_t;
353  typedef Eigen::Matrix<Scalar,6,NV,Options> UD_t;
354 
356 
357  typedef Eigen::Matrix<Scalar,NQ,1,Options> ConfigVector_t;
358  typedef Eigen::Matrix<Scalar,NV,1,Options> TangentVector_t;
359  };
360 
361  template<typename Scalar, int Options>
364 
365  template<typename Scalar, int Options>
368 
369  template<typename _Scalar, int _Options>
370  struct JointDataSphericalTpl : public JointDataBase< JointDataSphericalTpl<_Scalar,_Options> >
371  {
372  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
373 
377 
378  Constraint_t S;
379  Transformation_t M;
380  Motion_t v;
381  Bias_t c;
382 
383  // [ABA] specific data
384  U_t U;
385  D_t Dinv;
386  UD_t UDinv;
387 
389  : M(Transformation_t::Identity())
390  , v(Motion_t::Vector3::Zero())
391  , U(U_t::Zero())
392  , Dinv(D_t::Zero())
393  , UDinv(UD_t::Zero())
394  {}
395 
396  static std::string classname() { return std::string("JointDataSpherical"); }
397  std::string shortname() const { return classname(); }
398 
399  }; // struct JointDataSphericalTpl
400 
402  template<typename _Scalar, int _Options>
404  : public JointModelBase< JointModelSphericalTpl<_Scalar,_Options> >
405  {
406  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
407 
409  PINOCCHIO_JOINT_TYPEDEF_TEMPLATE(JointDerived);
410 
412  using Base::id;
413  using Base::idx_q;
414  using Base::idx_v;
415  using Base::setIndexes;
416 
417  JointDataDerived createData() const { return JointDataDerived(); }
418 
419  const std::vector<bool> hasConfigurationLimit() const
420  {
421  return {false, false, false, false};
422  }
423 
424  const std::vector<bool> hasConfigurationLimitInTangent() const
425  {
426  return {false, false, false};
427  }
428 
429  template<typename ConfigVectorLike>
430  inline void forwardKinematics(Transformation_t & M, const Eigen::MatrixBase<ConfigVectorLike> & q_joint) const
431  {
432  typedef typename ConfigVectorLike::Scalar Scalar;
433  EIGEN_STATIC_ASSERT_SAME_VECTOR_SIZE(ConfigVector_t,ConfigVectorLike);
434  typedef typename Eigen::Quaternion<Scalar,PINOCCHIO_EIGEN_PLAIN_TYPE(ConfigVectorLike)::Options> Quaternion;
435  typedef Eigen::Map<const Quaternion> ConstQuaternionMap;
436 
437  ConstQuaternionMap quat(q_joint.derived().data());
438  //assert(math::fabs(quat.coeffs().squaredNorm()-1.) <= sqrt(Eigen::NumTraits<typename V::Scalar>::epsilon())); TODO: check validity of the rhs precision
439  assert(math::fabs(static_cast<Scalar>(quat.coeffs().squaredNorm()-1)) <= 1e-4);
440 
441  M.rotation(quat.matrix());
442  M.translation().setZero();
443  }
444 
445  template<typename QuaternionDerived>
446  void calc(JointDataDerived & data,
447  const typename Eigen::QuaternionBase<QuaternionDerived> & quat) const
448  {
449  data.M.rotation(quat.matrix());
450  }
451 
452  template<typename ConfigVector>
453  EIGEN_DONT_INLINE
454  void calc(JointDataDerived & data,
455  const typename Eigen::PlainObjectBase<ConfigVector> & qs) const
456  {
457  typedef typename Eigen::Quaternion<typename ConfigVector::Scalar,ConfigVector::Options> Quaternion;
458  typedef Eigen::Map<const Quaternion> ConstQuaternionMap;
459 
460  ConstQuaternionMap quat(qs.template segment<NQ>(idx_q()).data());
461  calc(data,quat);
462  }
463 
464  template<typename ConfigVector>
465  EIGEN_DONT_INLINE
466  void calc(JointDataDerived & data,
467  const typename Eigen::MatrixBase<ConfigVector> & qs) const
468  {
469  typedef typename Eigen::Quaternion<Scalar,Options> Quaternion;
470 
471  const Quaternion quat(qs.template segment<NQ>(idx_q()));
472  calc(data,quat);
473  }
474 
475  template<typename ConfigVector, typename TangentVector>
476  void calc(JointDataDerived & data,
477  const typename Eigen::MatrixBase<ConfigVector> & qs,
478  const typename Eigen::MatrixBase<TangentVector> & vs) const
479  {
480  calc(data,qs.derived());
481 
482  data.v.angular() = vs.template segment<NV>(idx_v());
483  }
484 
485  template<typename Matrix6Like>
486  void calc_aba(JointDataDerived & data,
487  const Eigen::MatrixBase<Matrix6Like> & I,
488  const bool update_I) const
489  {
490  data.U = I.template block<6,3>(0,Inertia::ANGULAR);
491 
492  // compute inverse
493 // data.Dinv.setIdentity();
494 // data.U.template middleRows<3>(Inertia::ANGULAR).llt().solveInPlace(data.Dinv);
495  internal::PerformStYSInversion<Scalar>::run(data.U.template middleRows<3>(Inertia::ANGULAR),data.Dinv);
496 
497  data.UDinv.template middleRows<3>(Inertia::ANGULAR).setIdentity(); // can be put in data constructor
498  data.UDinv.template middleRows<3>(Inertia::LINEAR).noalias() = data.U.template block<3,3>(Inertia::LINEAR, 0) * data.Dinv;
499 
500  if (update_I)
501  {
502  Matrix6Like & I_ = PINOCCHIO_EIGEN_CONST_CAST(Matrix6Like,I);
503  I_.template block<3,3>(Inertia::LINEAR,Inertia::LINEAR)
504  -= data.UDinv.template middleRows<3>(Inertia::LINEAR) * I_.template block<3,3> (Inertia::ANGULAR, Inertia::LINEAR);
505  I_.template block<6,3>(0,Inertia::ANGULAR).setZero();
506  I_.template block<3,3>(Inertia::ANGULAR,Inertia::LINEAR).setZero();
507  }
508  }
509 
510  static std::string classname() { return std::string("JointModelSpherical"); }
511  std::string shortname() const { return classname(); }
512 
514  template<typename NewScalar>
516  {
518  ReturnType res;
519  res.setIndexes(id(),idx_q(),idx_v());
520  return res;
521  }
522 
523  }; // struct JointModelSphericalTpl
524 
525 } // namespace pinocchio
526 
527 #include <boost/type_traits.hpp>
528 
529 namespace boost
530 {
531  template<typename Scalar, int Options>
532  struct has_nothrow_constructor< ::pinocchio::JointModelSphericalTpl<Scalar,Options> >
533  : public integral_constant<bool,true> {};
534 
535  template<typename Scalar, int Options>
536  struct has_nothrow_copy< ::pinocchio::JointModelSphericalTpl<Scalar,Options> >
537  : public integral_constant<bool,true> {};
538 
539  template<typename Scalar, int Options>
540  struct has_nothrow_constructor< ::pinocchio::JointDataSphericalTpl<Scalar,Options> >
541  : public integral_constant<bool,true> {};
542 
543  template<typename Scalar, int Options>
544  struct has_nothrow_copy< ::pinocchio::JointDataSphericalTpl<Scalar,Options> >
545  : public integral_constant<bool,true> {};
546 }
547 
548 #endif // ifndef __pinocchio_joint_spherical_hpp__
JointCollectionTpl const Eigen::MatrixBase< ConfigVectorType > const Eigen::MatrixBase< TangentVectorType > & v
InertiaTpl< double, 0 > Inertia
const Symmetric3 & inertia() const
Eigen::Matrix< S1, 6, 3, O1 > se3ActionInverse(const SE3Tpl< S1, O1 > &m) const
int NQ
Definition: dpendulum.py:8
Eigen::Matrix< S1, 6, 3, O1 > se3Action(const SE3Tpl< S1, O1 > &m) const
EIGEN_DONT_INLINE void calc(JointDataDerived &data, const typename Eigen::MatrixBase< ConfigVector > &qs) const
void forwardKinematics(Transformation_t &M, const Eigen::MatrixBase< ConfigVectorLike > &q_joint) 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...
Eigen::Matrix< Scalar, NV, 1, Options > TangentVector_t
bool isEqual(const ConstraintSphericalTpl &) const
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...
MotionSphericalTpl(const Eigen::MatrixBase< Vector3Like > &w)
JointModelBase< JointModelSphericalTpl > Base
MotionSphericalTpl __plus__(const MotionSphericalTpl &other) const
Return type of the ation of a Motion onto an object of type D.
#define MOTION_TYPEDEF_TPL(Derived)
PINOCCHIO_EIGEN_REF_CONST_TYPE(Matrix6Like) operator*(const Eigen
void calc_aba(JointDataDerived &data, const Eigen::MatrixBase< Matrix6Like > &I, const bool update_I) const
JointMotion __mult__(const Eigen::MatrixBase< Vector3Like > &w) const
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
void calc(JointDataDerived &data, const typename Eigen::QuaternionBase< QuaternionDerived > &quat) const
PlainReturnType plain() const
ConstLinearType linear() const
Definition: motion-base.hpp:22
JointModelSphericalTpl< Scalar, Options > JointModelDerived
ConstAngularType angular() const
Return the angular part of the force vector.
Definition: force-base.hpp:35
#define PINOCCHIO_EIGEN_CONST_CAST(TYPE, OBJ)
Macro for an automatic const_cast.
bool isEqual_impl(const MotionSphericalTpl &other) const
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
JointDataDerived createData() const
#define PINOCCHIO_JOINT_DATA_BASE_ACCESSOR_DEFAULT_RETURN_TYPE
EIGEN_DONT_INLINE void calc(JointDataDerived &data, const typename Eigen::PlainObjectBase< ConfigVector > &qs) const
void cross(const Eigen::MatrixBase< Vector3 > &v, const Eigen::MatrixBase< Matrix3xIn > &Min, const Eigen::MatrixBase< Matrix3xOut > &Mout)
Applies the cross product onto the columns of M.
Definition: skew.hpp:211
#define PINOCCHIO_JOINT_DATA_TYPEDEF_TEMPLATE(Joint)
void motionAction(const MotionDense< M1 > &v, MotionDense< M2 > &mout) const
MotionSphericalTpl< double > MotionSpherical
static EIGEN_STRONG_INLINE void run(const Eigen::MatrixBase< M1 > &StYS, const Eigen::MatrixBase< M2 > &Dinv)
static void cross(const Eigen::MatrixBase< V3_in > &vin, const Eigen::MatrixBase< V3_out > &vout)
PINOCCHIO_JOINT_CAST_TYPE_SPECIALIZATION(JointModelFreeFlyerTpl)
quat
PINOCCHIO_JOINT_DATA_BASE_ACCESSOR_DEFAULT_RETURN_TYPE typedef Eigen::Matrix< Scalar, NQ, 1, Options > ConfigVector_t
#define PINOCCHIO_EIGEN_REF_TYPE(D)
MotionPlain se3ActionInverse_impl(const SE3Tpl< S2, O2 > &m) const
bool isEqual_impl(const MotionDense< MotionDerived > &other) const
PINOCCHIO_JOINT_DATA_BASE_DEFAULT_ACCESSOR Constraint_t S
Main pinocchio namespace.
Definition: timings.cpp:28
const SizeDepType< 3 >::RowsReturn< MatrixDerived >::ConstType operator*(const Eigen::MatrixBase< MatrixDerived > &F) const
void addTo(MotionDense< MotionDerived > &other) const
void se3Action_impl(const SE3Tpl< S2, O2 > &m, MotionDense< D2 > &v) const
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 alphaSkew(const Scalar alpha, const Eigen::MatrixBase< Vector3 > &v, const Eigen::MatrixBase< Matrix3 > &M)
Computes the skew representation of a given 3d vector multiplied by a given scalar. i.e. the antisymmetric matrix representation of the cross product operator ( )
Definition: skew.hpp:124
void setZero(std::vector< MatType, Eigen::aligned_allocator< MatType > > &Ms)
const std::vector< bool > hasConfigurationLimitInTangent() const
void calc(JointDataDerived &data, const typename Eigen::MatrixBase< ConfigVector > &qs, const typename Eigen::MatrixBase< TangentVector > &vs) const
DenseBase motionAction(const MotionDense< MotionDerived > &m) const
Common traits structure to fully define base classes for CRTP.
Definition: src/fwd.hpp:44
Symmetric3Tpl< double, 0 > Symmetric3
JointModelSphericalTpl< NewScalar, Options > cast() const
JointDataSphericalTpl< Scalar, Options > JointDataDerived
void se3ActionInverse_impl(const SE3Tpl< S2, O2 > &m, MotionDense< D2 > &v) const
void skew(const Eigen::MatrixBase< Vector3 > &v, const Eigen::MatrixBase< Matrix3 > &M)
Computes the skew representation of a given 3d vector, i.e. the antisymmetric matrix representation o...
Definition: skew.hpp:21
MotionPlain se3Action_impl(const SE3Tpl< S2, O2 > &m) const
#define PINOCCHIO_CONSTRAINT_TYPEDEF_TPL(DERIVED)
ConstraintSphericalTpl< Scalar, Options > Constraint_t
const Vector3 & angular() const
M
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...
MotionPlain motionAction(const MotionDense< M1 > &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 Derived & derived()
EIGEN_MAKE_ALIGNED_OPERATOR_NEW typedef JointSphericalTpl< _Scalar, _Options > JointDerived
void setTo(MotionDense< Derived > &other) const
const std::vector< bool > hasConfigurationLimit() const
MultiplicationOp< InertiaTpl< Scalar, Options >, ConstraintDerived >::ReturnType operator*(const InertiaTpl< Scalar, Options > &Y, const ConstraintBase< ConstraintDerived > &constraint)
 .
EIGEN_MAKE_ALIGNED_OPERATOR_NEW typedef JointSphericalTpl< _Scalar, _Options > JointDerived
#define PINOCCHIO_JOINT_TYPEDEF_TEMPLATE(Joint)


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