pinocchio  2.6.0
A fast and flexible implementation of Rigid Body Dynamics algorithms and their analytical derivatives
inertia.hpp
1 //
2 // Copyright (c) 2015-2019 CNRS INRIA
3 // Copyright (c) 2016 Wandercraft, 86 rue de Paris 91400 Orsay, France.
4 //
5 
6 #ifndef __pinocchio_inertia_hpp__
7 #define __pinocchio_inertia_hpp__
8 
9 #include <iostream>
10 
11 #include "pinocchio/math/fwd.hpp"
12 #include "pinocchio/spatial/symmetric3.hpp"
13 #include "pinocchio/spatial/force.hpp"
14 #include "pinocchio/spatial/motion.hpp"
15 #include "pinocchio/spatial/skew.hpp"
16 
17 namespace pinocchio
18 {
19 
20  template< class Derived>
22  {
23  protected:
24 
25  typedef Derived Derived_t;
26  SPATIAL_TYPEDEF_TEMPLATE(Derived_t);
27 
28  public:
29  Derived_t & derived() { return *static_cast<Derived_t*>(this); }
30  const Derived_t & derived() const { return *static_cast<const Derived_t*>(this); }
31 
32  Scalar mass() const { return static_cast<const Derived_t*>(this)->mass(); }
33  Scalar & mass() { return static_cast<const Derived_t*>(this)->mass(); }
34  const Vector3 & lever() const { return static_cast<const Derived_t*>(this)->lever(); }
35  Vector3 & lever() { return static_cast<const Derived_t*>(this)->lever(); }
36  const Symmetric3 & inertia() const { return static_cast<const Derived_t*>(this)->inertia(); }
37  Symmetric3 & inertia() { return static_cast<const Derived_t*>(this)->inertia(); }
38 
39  Matrix6 matrix() const { return derived().matrix_impl(); }
40  operator Matrix6 () const { return matrix(); }
41 
42  Derived_t& operator= (const Derived_t& clone){return derived().__equl__(clone);}
43  bool operator==(const Derived_t & other) const {return derived().isEqual(other);}
44  bool operator!=(const Derived_t & other) const { return !(*this == other); }
45 
46  Derived_t& operator+= (const Derived_t & Yb) { return derived().__pequ__(Yb); }
47  Derived_t operator+(const Derived_t & Yb) const { return derived().__plus__(Yb); }
48 
49  template<typename MotionDerived>
51  operator*(const MotionDense<MotionDerived> & v) const
52  { return derived().__mult__(v); }
53 
54  Scalar vtiv(const Motion & v) const { return derived().vtiv_impl(v); }
55  Matrix6 variation(const Motion & v) const { return derived().variation_impl(v); }
56 
64  template<typename M6>
65  static void vxi(const Motion & v, const Derived & I, const Eigen::MatrixBase<M6> & Iout)
66  {
67  EIGEN_STATIC_ASSERT_SAME_MATRIX_SIZE(M6, Matrix6);
68  Derived::vxi_impl(v,I,Iout);
69  }
70 
71  Matrix6 vxi(const Motion & v) const
72  {
73  Matrix6 Iout;
74  vxi(v,derived(),Iout);
75  return Iout;
76  }
77 
85  template<typename M6>
86  static void ivx(const Motion & v, const Derived & I, const Eigen::MatrixBase<M6> & Iout)
87  {
88  EIGEN_STATIC_ASSERT_SAME_MATRIX_SIZE(M6, Matrix6);
89  Derived::ivx_impl(v,I,Iout);
90  }
91 
92  Matrix6 ivx(const Motion & v) const
93  {
94  Matrix6 Iout;
95  ivx(v,derived(),Iout);
96  return Iout;
97  }
98 
99  void setZero() { derived().setZero(); }
100  void setIdentity() { derived().setIdentity(); }
101  void setRandom() { derived().setRandom(); }
102 
103  bool isApprox(const Derived & other, const Scalar & prec = Eigen::NumTraits<Scalar>::dummy_precision()) const
104  { return derived().isApprox_impl(other, prec); }
105 
106  bool isZero(const Scalar & prec = Eigen::NumTraits<Scalar>::dummy_precision()) const
107  { return derived().isZero_impl(prec); }
108 
110  Derived_t se3Action(const SE3 & M) const { return derived().se3Action_impl(M); }
111 
113  Derived_t se3ActionInverse(const SE3 & M) const { return derived().se3ActionInverse_impl(M); }
114 
115  void disp(std::ostream & os) const { static_cast<const Derived_t*>(this)->disp_impl(os); }
116  friend std::ostream & operator << (std::ostream & os,const InertiaBase<Derived_t> & X)
117  {
118  X.disp(os);
119  return os;
120  }
121 
122  }; // class InertiaBase
123 
124 
125  template<typename T, int U>
126  struct traits< InertiaTpl<T, U> >
127  {
128  typedef T Scalar;
129  typedef Eigen::Matrix<T,3,1,U> Vector3;
130  typedef Eigen::Matrix<T,4,1,U> Vector4;
131  typedef Eigen::Matrix<T,6,1,U> Vector6;
132  typedef Eigen::Matrix<T,3,3,U> Matrix3;
133  typedef Eigen::Matrix<T,4,4,U> Matrix4;
134  typedef Eigen::Matrix<T,6,6,U> Matrix6;
135  typedef Matrix6 ActionMatrix_t;
136  typedef Vector3 Angular_t;
137  typedef Vector3 Linear_t;
138  typedef const Vector3 ConstAngular_t;
139  typedef const Vector3 ConstLinear_t;
140  typedef Eigen::Quaternion<T,U> Quaternion_t;
141  typedef SE3Tpl<T,U> SE3;
142  typedef ForceTpl<T,U> Force;
143  typedef MotionTpl<T,U> Motion;
145  enum {
146  LINEAR = 0,
147  ANGULAR = 3
148  };
149  }; // traits InertiaTpl
150 
151  template<typename _Scalar, int _Options>
152  class InertiaTpl : public InertiaBase< InertiaTpl< _Scalar, _Options > >
153  {
154  public:
155  friend class InertiaBase< InertiaTpl< _Scalar, _Options > >;
156  SPATIAL_TYPEDEF_TEMPLATE(InertiaTpl);
157  enum { Options = _Options };
158  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
159 
160  typedef typename Symmetric3::AlphaSkewSquare AlphaSkewSquare;
161 
162  typedef typename Eigen::Matrix<_Scalar, 10, 1, _Options> Vector10;
163 
164  public:
165  // Constructors
166  InertiaTpl()
167  {}
168 
169  InertiaTpl(const Scalar mass, const Vector3 & com, const Matrix3 & rotational_inertia)
170  : m_mass(mass), m_com(com), m_inertia(rotational_inertia)
171  {}
172 
173  InertiaTpl(const Matrix6 & I6)
174  {
175  assert((I6 - I6.transpose()).isMuchSmallerThan(I6));
176  mass() = I6(LINEAR, LINEAR);
177  const Matrix3 & mc_cross = I6.template block <3,3>(ANGULAR,LINEAR);
178  lever() = unSkew(mc_cross);
179  lever() /= mass();
180 
181  Matrix3 I3 (mc_cross * mc_cross);
182  I3 /= mass();
183  I3 += I6.template block<3,3>(ANGULAR,ANGULAR);
184  const Symmetric3 S3(I3);
185  inertia() = S3;
186  }
187 
188  InertiaTpl(Scalar mass, const Vector3 & com, const Symmetric3 & rotational_inertia)
189  : m_mass(mass), m_com(com), m_inertia(rotational_inertia)
190  {}
191 
192  InertiaTpl(const InertiaTpl & clone) // Copy constructor
193  : m_mass(clone.mass()), m_com(clone.lever()), m_inertia(clone.inertia())
194  {}
195 
196  InertiaTpl& operator=(const InertiaTpl & clone) // Copy assignment operator
197  {
198  m_mass = clone.mass();
199  m_com = clone.lever();
200  m_inertia = clone.inertia();
201  return *this;
202  }
203 
204  template<int O2>
205  InertiaTpl(const InertiaTpl<Scalar,O2> & clone)
206  : m_mass(clone.mass())
207  , m_com(clone.lever())
208  , m_inertia(clone.inertia().matrix())
209  {}
210 
211  // Initializers
212  static InertiaTpl Zero()
213  {
214  return InertiaTpl(Scalar(0),
215  Vector3::Zero(),
216  Symmetric3::Zero());
217  }
218 
219  void setZero() { mass() = Scalar(0); lever().setZero(); inertia().setZero(); }
220 
221  static InertiaTpl Identity()
222  {
223  return InertiaTpl(Scalar(1),
224  Vector3::Zero(),
225  Symmetric3::Identity());
226  }
227 
228  void setIdentity ()
229  {
230  mass() = Scalar(1); lever().setZero(); inertia().setIdentity();
231  }
232 
233  static InertiaTpl Random()
234  {
235  // We have to shoot "I" definite positive and not only symmetric.
236  return InertiaTpl(Eigen::internal::random<Scalar>()+1,
237  Vector3::Random(),
238  Symmetric3::RandomPositive());
239  }
240 
241  static InertiaTpl FromSphere(const Scalar m, const Scalar radius)
242  {
243  return FromEllipsoid(m,radius,radius,radius);
244  }
245 
246  static InertiaTpl FromEllipsoid(const Scalar m, const Scalar x,
247  const Scalar y, const Scalar z)
248  {
249  Scalar a = m * (y*y + z*z) / Scalar(5);
250  Scalar b = m * (x*x + z*z) / Scalar(5);
251  Scalar c = m * (y*y + x*x) / Scalar(5);
252  return InertiaTpl(m, Vector3::Zero(), Symmetric3(a, Scalar(0), b,
253  Scalar(0), Scalar(0), c));
254  }
255 
256  static InertiaTpl FromCylinder(const Scalar m, const Scalar r, const Scalar l)
257  {
258  Scalar a = m * (r*r / Scalar(4) + l*l / Scalar(12));
259  Scalar c = m * (r*r / Scalar(2));
260  return InertiaTpl(m, Vector3::Zero(), Symmetric3(a, Scalar(0), a,
261  Scalar(0), Scalar(0), c));
262  }
263 
264  static InertiaTpl FromBox(const Scalar m, const Scalar x,
265  const Scalar y, const Scalar z)
266  {
267  Scalar a = m * (y*y + z*z) / Scalar(12);
268  Scalar b = m * (x*x + z*z) / Scalar(12);
269  Scalar c = m * (y*y + x*x) / Scalar(12);
270  return InertiaTpl(m, Vector3::Zero(), Symmetric3(a, Scalar(0), b,
271  Scalar(0), Scalar(0), c));
272  }
273 
274  void setRandom()
275  {
276  mass() = static_cast<Scalar>(std::rand())/static_cast<Scalar>(RAND_MAX);
277  lever().setRandom(); inertia().setRandom();
278  }
279 
280  Matrix6 matrix_impl() const
281  {
282  Matrix6 M;
283 
284  M.template block<3,3>(LINEAR, LINEAR ).setZero();
285  M.template block<3,3>(LINEAR, LINEAR ).diagonal().fill (mass());
286  M.template block<3,3>(ANGULAR,LINEAR ) = alphaSkew(mass(),lever());
287  M.template block<3,3>(LINEAR, ANGULAR) = -M.template block<3,3>(ANGULAR, LINEAR);
288  M.template block<3,3>(ANGULAR,ANGULAR) = (inertia() - AlphaSkewSquare(mass(),lever())).matrix();
289 
290  return M;
291  }
292 
299  Vector10 toDynamicParameters() const
300  {
301  Vector10 v;
302 
303  v[0] = mass();
304  v.template segment<3>(1) = mass() * lever();
305  v.template segment<6>(4) = (inertia() - AlphaSkewSquare(mass(),lever())).data();
306 
307  return v;
308  }
309 
318  template<typename Vector10Like>
319  static InertiaTpl FromDynamicParameters(const Eigen::MatrixBase<Vector10Like> & params)
320  {
321  PINOCCHIO_ASSERT_MATRIX_SPECIFIC_SIZE(Vector10Like, params, 10, 1);
322 
323  const Scalar & mass = params[0];
324  Vector3 lever = params.template segment<3>(1);
325  lever /= mass;
326 
327  return InertiaTpl(mass, lever, Symmetric3(params.template segment<6>(4)) + AlphaSkewSquare(mass,lever));
328  }
329 
330  // Arithmetic operators
331  InertiaTpl & __equl__(const InertiaTpl & clone)
332  {
333  mass() = clone.mass(); lever() = clone.lever(); inertia() = clone.inertia();
334  return *this;
335  }
336 
337  // Required by std::vector boost::python bindings.
338  bool isEqual( const InertiaTpl& Y2 ) const
339  {
340  return (mass()==Y2.mass()) && (lever()==Y2.lever()) && (inertia()==Y2.inertia());
341  }
342 
343  bool isApprox_impl(const InertiaTpl & other,
344  const Scalar & prec = Eigen::NumTraits<Scalar>::dummy_precision()) const
345  {
346  using math::fabs;
347  return fabs(static_cast<Scalar>(mass() - other.mass())) <= prec
348  && lever().isApprox(other.lever(),prec)
349  && inertia().isApprox(other.inertia(),prec);
350  }
351 
352  bool isZero_impl(const Scalar & prec = Eigen::NumTraits<Scalar>::dummy_precision()) const
353  {
354  using math::fabs;
355  return fabs(mass()) <= prec
356  && lever().isZero(prec)
357  && inertia().isZero(prec);
358  }
359 
360  InertiaTpl __plus__(const InertiaTpl & Yb) const
361  {
362  /* Y_{a+b} = ( m_a+m_b,
363  * (m_a*c_a + m_b*c_b ) / (m_a + m_b),
364  * I_a + I_b - (m_a*m_b)/(m_a+m_b) * AB_x * AB_x )
365  */
366 
367  const Scalar eps = ::Eigen::NumTraits<Scalar>::epsilon();
368 
369  const Scalar & mab = mass()+Yb.mass();
370  const Scalar mab_inv = Scalar(1)/math::max((Scalar)(mass()+Yb.mass()),eps);
371  const Vector3 & AB = (lever()-Yb.lever()).eval();
372  return InertiaTpl(mab,
373  (mass()*lever()+Yb.mass()*Yb.lever())*mab_inv,
374  inertia()+Yb.inertia() - (mass()*Yb.mass()*mab_inv)* typename Symmetric3::SkewSquare(AB));
375  }
376 
377  InertiaTpl& __pequ__(const InertiaTpl & Yb)
378  {
379  const Scalar eps = ::Eigen::NumTraits<Scalar>::epsilon();
380 
381  const InertiaTpl& Ya = *this;
382  const Scalar & mab = mass()+Yb.mass();
383  const Scalar mab_inv = Scalar(1)/math::max((Scalar)(mass()+Yb.mass()),eps);
384  const Vector3 & AB = (Ya.lever()-Yb.lever()).eval();
385  lever() *= (mass()*mab_inv); lever() += (Yb.mass()*mab_inv)*Yb.lever(); //c *= mab_inv;
386  inertia() += Yb.inertia(); inertia() -= (Ya.mass()*Yb.mass()*mab_inv)* typename Symmetric3::SkewSquare(AB);
387  mass() = mab;
388  return *this;
389  }
390 
391  template<typename MotionDerived>
392  ForceTpl<typename traits<MotionDerived>::Scalar,traits<MotionDerived>::Options>
393  __mult__(const MotionDense<MotionDerived> & v) const
394  {
395  typedef ForceTpl<typename traits<MotionDerived>::Scalar,traits<MotionDerived>::Options> ReturnType;
396  ReturnType f;
397  __mult__(v,f);
398  return f;
399  }
400 
401  template<typename MotionDerived, typename ForceDerived>
402  void __mult__(const MotionDense<MotionDerived> & v, ForceDense<ForceDerived> & f) const
403  {
404  f.linear().noalias() = mass()*(v.linear() - lever().cross(v.angular()));
405  Symmetric3::rhsMult(inertia(),v.angular(),f.angular());
406  f.angular() += lever().cross(f.linear());
407 // f.angular().noalias() = c.cross(f.linear()) + I*v.angular();
408  }
409 
410  Scalar vtiv_impl(const Motion & v) const
411  {
412  const Vector3 cxw (lever().cross(v.angular()));
413  Scalar res = mass() * (v.linear().squaredNorm() - Scalar(2)*v.linear().dot(cxw));
414  const Vector3 mcxcxw (-mass()*lever().cross(cxw));
415  res += v.angular().dot(mcxcxw);
416  res += inertia().vtiv(v.angular());
417 
418  return res;
419  }
420 
421  Matrix6 variation(const Motion & v) const
422  {
423  Matrix6 res;
424  const Motion mv(v*mass());
425 
426  res.template block<3,3>(LINEAR,ANGULAR) = -skew(mv.linear()) - skewSquare(mv.angular(),lever()) + skewSquare(lever(),mv.angular());
427  res.template block<3,3>(ANGULAR,LINEAR) = res.template block<3,3>(LINEAR,ANGULAR).transpose();
428 
429 // res.template block<3,3>(LINEAR,LINEAR) = mv.linear()*c.transpose(); // use as temporary variable
430 // res.template block<3,3>(ANGULAR,ANGULAR) = res.template block<3,3>(LINEAR,LINEAR) - res.template block<3,3>(LINEAR,LINEAR).transpose();
431  res.template block<3,3>(ANGULAR,ANGULAR) = -skewSquare(mv.linear(),lever()) - skewSquare(lever(),mv.linear());
432 
433  res.template block<3,3>(LINEAR,LINEAR) = (inertia() - AlphaSkewSquare(mass(),lever())).matrix();
434 
435  res.template block<3,3>(ANGULAR,ANGULAR) -= res.template block<3,3>(LINEAR,LINEAR) * skew(v.angular());
436  res.template block<3,3>(ANGULAR,ANGULAR) += cross(v.angular(),res.template block<3,3>(LINEAR,LINEAR));
437 
438  res.template block<3,3>(LINEAR,LINEAR).setZero();
439  return res;
440  }
441 
442  template<typename M6>
443  static void vxi_impl(const Motion & v,
444  const InertiaTpl & I,
445  const Eigen::MatrixBase<M6> & Iout)
446  {
447  EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(M6,6,6);
448  M6 & Iout_ = PINOCCHIO_EIGEN_CONST_CAST(M6,Iout);
449 
450  // Block 1,1
451  alphaSkew(I.mass(),v.angular(),Iout_.template block<3,3>(LINEAR,LINEAR));
452 // Iout_.template block<3,3>(LINEAR,LINEAR) = alphaSkew(I.mass(),v.angular());
453  const Vector3 mc(I.mass()*I.lever());
454 
455  // Block 1,2
456  skewSquare(-v.angular(),mc,Iout_.template block<3,3>(LINEAR,ANGULAR));
457 
458 
460  alphaSkew(I.mass(),v.linear(),Iout_.template block<3,3>(ANGULAR,LINEAR));
461  Iout_.template block<3,3>(ANGULAR,LINEAR) -= Iout_.template block<3,3>(LINEAR,ANGULAR);
462 
464  skewSquare(-v.linear(),mc,Iout_.template block<3,3>(ANGULAR,ANGULAR));
465 
466  // TODO: I do not why, but depending on the CPU, these three lines can give
467  // wrong output.
468  // typename Symmetric3::AlphaSkewSquare mcxcx(I.mass(),I.lever());
469  // const Symmetric3 I_mcxcx(I.inertia() - mcxcx);
470  // Iout_.template block<3,3>(ANGULAR,ANGULAR) += I_mcxcx.vxs(v.angular());
471  Symmetric3 mcxcx(typename Symmetric3::AlphaSkewSquare(I.mass(),I.lever()));
472  Iout_.template block<3,3>(ANGULAR,ANGULAR) += I.inertia().vxs(v.angular());
473  Iout_.template block<3,3>(ANGULAR,ANGULAR) -= mcxcx.vxs(v.angular());
474 
475  }
476 
477  template<typename M6>
478  static void ivx_impl(const Motion & v,
479  const InertiaTpl & I,
480  const Eigen::MatrixBase<M6> & Iout)
481  {
482  EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(M6,6,6);
483  M6 & Iout_ = PINOCCHIO_EIGEN_CONST_CAST(M6,Iout);
484 
485  // Block 1,1
486  alphaSkew(I.mass(),v.angular(),Iout_.template block<3,3>(LINEAR,LINEAR));
487 
488  // Block 2,1
489  const Vector3 mc(I.mass()*I.lever());
490  skewSquare(mc,v.angular(),Iout_.template block<3,3>(ANGULAR,LINEAR));
491 
492  // Block 1,2
493  alphaSkew(I.mass(),v.linear(),Iout_.template block<3,3>(LINEAR,ANGULAR));
494 
495  // Block 2,2
496  cross(-I.lever(),Iout_.template block<3,3>(ANGULAR,LINEAR),Iout_.template block<3,3>(ANGULAR,ANGULAR));
497  Iout_.template block<3,3>(ANGULAR,ANGULAR) += I.inertia().svx(v.angular());
498  for(int k = 0; k < 3; ++k)
499  Iout_.template block<3,3>(ANGULAR,ANGULAR).col(k) += I.lever().cross(Iout_.template block<3,3>(LINEAR,ANGULAR).col(k));
500 
501  // Block 1,2
502  Iout_.template block<3,3>(LINEAR,ANGULAR) -= Iout_.template block<3,3>(ANGULAR,LINEAR);
503 
504  }
505 
506  // Getters
507  Scalar mass() const { return m_mass; }
508  const Vector3 & lever() const { return m_com; }
509  const Symmetric3 & inertia() const { return m_inertia; }
510 
511  Scalar & mass() { return m_mass; }
512  Vector3 & lever() { return m_com; }
513  Symmetric3 & inertia() { return m_inertia; }
514 
516  InertiaTpl se3Action_impl(const SE3 & M) const
517  {
518  /* The multiplication RIR' has a particular form that could be used, however it
519  * does not seems to be more efficient, see http://stackoverflow.m_comom/questions/
520  * 13215467/eigen-best-way-to-evaluate-asa-transpose-and-store-the-result-in-a-symmetric .*/
521  return InertiaTpl(mass(),
522  M.translation()+M.rotation()*lever(),
523  inertia().rotate(M.rotation()));
524  }
525 
528  {
529  return InertiaTpl(mass(),
530  M.rotation().transpose()*(lever()-M.translation()),
531  inertia().rotate(M.rotation().transpose()) );
532  }
533 
534  Force vxiv( const Motion& v ) const
535  {
536  const Vector3 & mcxw = mass()*lever().cross(v.angular());
537  const Vector3 & mv_mcxw = mass()*v.linear()-mcxw;
538  return Force( v.angular().cross(mv_mcxw),
539  v.angular().cross(lever().cross(mv_mcxw)+inertia()*v.angular())-v.linear().cross(mcxw) );
540  }
541 
542  void disp_impl(std::ostream & os) const
543  {
544  os
545  << " m = " << mass() << "\n"
546  << " c = " << lever().transpose() << "\n"
547  << " I = \n" << inertia().matrix() << "";
548  }
549 
551  template<typename NewScalar>
553  {
554  return InertiaTpl<NewScalar,Options>(static_cast<NewScalar>(mass()),
555  lever().template cast<NewScalar>(),
556  inertia().template cast<NewScalar>());
557  }
558 
559  // TODO: adjust code
560 // /// \brief Check whether *this is a valid inertia, resulting from a positive mass distribution
561 // bool isValid() const
562 // {
563 // return
564 // (m_mass > Scalar(0) && m_inertia.isValid())
565 // || (m_mass == Scalar(0) && (m_inertia.data().array() == Scalar(0)).all());
566 // }
567 
568  protected:
569  Scalar m_mass;
570  Vector3 m_com;
571  Symmetric3 m_inertia;
572 
573  }; // class InertiaTpl
574 
575 } // namespace pinocchio
576 
577 #endif // ifndef __pinocchio_inertia_hpp__
pinocchio::InertiaBase::se3ActionInverse
Derived_t se3ActionInverse(const SE3 &M) const
bI = aXb.actInv(aI)
Definition: inertia.hpp:113
pinocchio::InertiaTpl::FromDynamicParameters
static InertiaTpl FromDynamicParameters(const Eigen::MatrixBase< Vector10Like > &params)
Definition: inertia.hpp:319
continuous.x
x
— Training
Definition: continuous.py:157
pinocchio::InertiaBase::ivx
static void ivx(const Motion &v, const Derived &I, const Eigen::MatrixBase< M6 > &Iout)
Time variation operator. It computes the time derivative of an inertia I corresponding to the formula...
Definition: inertia.hpp:86
pinocchio::SE3Tpl< double, 0 >
pinocchio::MotionDense
Definition: fwd.hpp:41
pinocchio::InertiaTpl::se3ActionInverse_impl
InertiaTpl se3ActionInverse_impl(const SE3 &M) const
bI = aXb.actInv(aI)
Definition: inertia.hpp:527
pinocchio::skew
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
pinocchio::ForceTpl
Definition: force-tpl.hpp:35
pinocchio::InertiaBase
Definition: inertia.hpp:21
pinocchio::InertiaTpl::toDynamicParameters
Vector10 toDynamicParameters() const
Definition: inertia.hpp:299
pinocchio::InertiaTpl::cast
InertiaTpl< NewScalar, Options > cast() const
Definition: inertia.hpp:552
pinocchio::skewSquare
void skewSquare(const Eigen::MatrixBase< V1 > &u, const Eigen::MatrixBase< V2 > &v, const Eigen::MatrixBase< Matrix3 > &C)
Computes the square cross product linear operator C(u,v) such that for any vector w,...
Definition: skew.hpp:166
pinocchio::Symmetric3Tpl< double, 0 >
pinocchio::InertiaBase::vxi
static void vxi(const Motion &v, const Derived &I, const Eigen::MatrixBase< M6 > &Iout)
Time variation operator. It computes the time derivative of an inertia I corresponding to the formula...
Definition: inertia.hpp:65
pinocchio::InertiaTpl::se3Action_impl
InertiaTpl se3Action_impl(const SE3 &M) const
aI = aXb.act(bI)
Definition: inertia.hpp:516
pinocchio::cross
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
pinocchio::InertiaTpl
Definition: fwd.hpp:52
pinocchio::traits
Common traits structure to fully define base classes for CRTP.
Definition: fwd.hpp:44
pinocchio::alphaSkew
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....
Definition: skew.hpp:124
pinocchio::MotionTpl
Definition: fwd.hpp:43
pinocchio::unSkew
void unSkew(const Eigen::MatrixBase< Matrix3 > &M, const Eigen::MatrixBase< Vector3 > &v)
Inverse of skew operator. From a given skew-symmetric matrix M of dimension 3x3, it extracts the supp...
Definition: skew.hpp:82
pinocchio::InertiaBase::se3Action
Derived_t se3Action(const SE3 &M) const
aI = aXb.act(bI)
Definition: inertia.hpp:110
pinocchio
Main pinocchio namespace.
Definition: treeview.dox:24