pinocchio  2.7.1
A fast and flexible implementation of Rigid Body Dynamics algorithms and their analytical derivatives
inertia.hpp
1 //
2 // Copyright (c) 2015-2021 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>
21  class InertiaBase
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>
50  ForceTpl<typename traits<MotionDerived>::Scalar,traits<MotionDerived>::Options>
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;
144  typedef Symmetric3Tpl<T,U> Symmetric3;
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 
247  static InertiaTpl FromSphere(const Scalar mass, const Scalar radius)
248  {
249  return FromEllipsoid(mass,radius,radius,radius);
250  }
251 
260  static InertiaTpl FromEllipsoid(const Scalar mass,
261  const Scalar x,
262  const Scalar y,
263  const Scalar z)
264  {
265  const Scalar a = mass * (y*y + z*z) / Scalar(5);
266  const Scalar b = mass * (x*x + z*z) / Scalar(5);
267  const Scalar c = mass * (y*y + x*x) / Scalar(5);
268  return InertiaTpl(mass, Vector3::Zero(), Symmetric3(a, Scalar(0), b,
269  Scalar(0), Scalar(0), c));
270  }
271 
279  static InertiaTpl FromCylinder(const Scalar mass,
280  const Scalar radius,
281  const Scalar length)
282  {
283  const Scalar radius_square = radius * radius;
284  const Scalar a = mass * (radius_square / Scalar(4) + length*length / Scalar(12));
285  const Scalar c = mass * (radius_square / Scalar(2));
286  return InertiaTpl(mass, Vector3::Zero(), Symmetric3(a, Scalar(0), a,
287  Scalar(0), Scalar(0), c));
288  }
289 
298  static InertiaTpl FromBox(const Scalar mass,
299  const Scalar x,
300  const Scalar y,
301  const Scalar z)
302  {
303  const Scalar a = mass * (y*y + z*z) / Scalar(12);
304  const Scalar b = mass * (x*x + z*z) / Scalar(12);
305  const Scalar c = mass * (y*y + x*x) / Scalar(12);
306  return InertiaTpl(mass, Vector3::Zero(), Symmetric3(a, Scalar(0), b,
307  Scalar(0), Scalar(0), c));
308  }
309 
310  void setRandom()
311  {
312  mass() = static_cast<Scalar>(std::rand())/static_cast<Scalar>(RAND_MAX);
313  lever().setRandom(); inertia().setRandom();
314  }
315 
316  Matrix6 matrix_impl() const
317  {
318  Matrix6 M;
319 
320  M.template block<3,3>(LINEAR, LINEAR ).setZero();
321  M.template block<3,3>(LINEAR, LINEAR ).diagonal().fill (mass());
322  M.template block<3,3>(ANGULAR,LINEAR ) = alphaSkew(mass(),lever());
323  M.template block<3,3>(LINEAR, ANGULAR) = -M.template block<3,3>(ANGULAR, LINEAR);
324  M.template block<3,3>(ANGULAR,ANGULAR) = (inertia() - AlphaSkewSquare(mass(),lever())).matrix();
325 
326  return M;
327  }
328 
335  Vector10 toDynamicParameters() const
336  {
337  Vector10 v;
338 
339  v[0] = mass();
340  v.template segment<3>(1).noalias() = mass() * lever();
341  v.template segment<6>(4) = (inertia() - AlphaSkewSquare(mass(),lever())).data();
342 
343  return v;
344  }
345 
354  template<typename Vector10Like>
355  static InertiaTpl FromDynamicParameters(const Eigen::MatrixBase<Vector10Like> & params)
356  {
357  PINOCCHIO_ASSERT_MATRIX_SPECIFIC_SIZE(Vector10Like, params, 10, 1);
358 
359  const Scalar & mass = params[0];
360  Vector3 lever = params.template segment<3>(1);
361  lever /= mass;
362 
363  return InertiaTpl(mass, lever, Symmetric3(params.template segment<6>(4)) + AlphaSkewSquare(mass,lever));
364  }
365 
366  // Arithmetic operators
367  InertiaTpl & __equl__(const InertiaTpl & clone)
368  {
369  mass() = clone.mass(); lever() = clone.lever(); inertia() = clone.inertia();
370  return *this;
371  }
372 
373  // Required by std::vector boost::python bindings.
374  bool isEqual( const InertiaTpl& Y2 ) const
375  {
376  return (mass()==Y2.mass()) && (lever()==Y2.lever()) && (inertia()==Y2.inertia());
377  }
378 
379  bool isApprox_impl(const InertiaTpl & other,
380  const Scalar & prec = Eigen::NumTraits<Scalar>::dummy_precision()) const
381  {
382  using math::fabs;
383  return fabs(static_cast<Scalar>(mass() - other.mass())) <= prec
384  && lever().isApprox(other.lever(),prec)
385  && inertia().isApprox(other.inertia(),prec);
386  }
387 
388  bool isZero_impl(const Scalar & prec = Eigen::NumTraits<Scalar>::dummy_precision()) const
389  {
390  using math::fabs;
391  return fabs(mass()) <= prec
392  && lever().isZero(prec)
393  && inertia().isZero(prec);
394  }
395 
396  InertiaTpl __plus__(const InertiaTpl & Yb) const
397  {
398  /* Y_{a+b} = ( m_a+m_b,
399  * (m_a*c_a + m_b*c_b ) / (m_a + m_b),
400  * I_a + I_b - (m_a*m_b)/(m_a+m_b) * AB_x * AB_x )
401  */
402 
403  const Scalar eps = ::Eigen::NumTraits<Scalar>::epsilon();
404 
405  const Scalar & mab = mass()+Yb.mass();
406  const Scalar mab_inv = Scalar(1)/math::max((Scalar)(mass()+Yb.mass()),eps);
407  const Vector3 & AB = (lever()-Yb.lever()).eval();
408  return InertiaTpl(mab,
409  (mass()*lever()+Yb.mass()*Yb.lever())*mab_inv,
410  inertia()+Yb.inertia() - (mass()*Yb.mass()*mab_inv)* typename Symmetric3::SkewSquare(AB));
411  }
412 
413  InertiaTpl& __pequ__(const InertiaTpl & Yb)
414  {
415  const Scalar eps = ::Eigen::NumTraits<Scalar>::epsilon();
416 
417  const InertiaTpl& Ya = *this;
418  const Scalar & mab = mass()+Yb.mass();
419  const Scalar mab_inv = Scalar(1)/math::max((Scalar)(mass()+Yb.mass()),eps);
420  const Vector3 & AB = (Ya.lever()-Yb.lever()).eval();
421  lever() *= (mass()*mab_inv); lever() += (Yb.mass()*mab_inv)*Yb.lever(); //c *= mab_inv;
422  inertia() += Yb.inertia(); inertia() -= (Ya.mass()*Yb.mass()*mab_inv)* typename Symmetric3::SkewSquare(AB);
423  mass() = mab;
424  return *this;
425  }
426 
427  template<typename MotionDerived>
428  ForceTpl<typename traits<MotionDerived>::Scalar,traits<MotionDerived>::Options>
429  __mult__(const MotionDense<MotionDerived> & v) const
430  {
431  typedef ForceTpl<typename traits<MotionDerived>::Scalar,traits<MotionDerived>::Options> ReturnType;
432  ReturnType f;
433  __mult__(v,f);
434  return f;
435  }
436 
437  template<typename MotionDerived, typename ForceDerived>
438  void __mult__(const MotionDense<MotionDerived> & v, ForceDense<ForceDerived> & f) const
439  {
440  f.linear().noalias() = mass()*(v.linear() - lever().cross(v.angular()));
441  Symmetric3::rhsMult(inertia(),v.angular(),f.angular());
442  f.angular() += lever().cross(f.linear());
443 // f.angular().noalias() = c.cross(f.linear()) + I*v.angular();
444  }
445 
446  Scalar vtiv_impl(const Motion & v) const
447  {
448  const Vector3 cxw (lever().cross(v.angular()));
449  Scalar res = mass() * (v.linear().squaredNorm() - Scalar(2)*v.linear().dot(cxw));
450  const Vector3 mcxcxw (-mass()*lever().cross(cxw));
451  res += v.angular().dot(mcxcxw);
452  res += inertia().vtiv(v.angular());
453 
454  return res;
455  }
456 
457  Matrix6 variation(const Motion & v) const
458  {
459  Matrix6 res;
460  const Motion mv(v*mass());
461 
462  res.template block<3,3>(LINEAR,ANGULAR) = -skew(mv.linear()) - skewSquare(mv.angular(),lever()) + skewSquare(lever(),mv.angular());
463  res.template block<3,3>(ANGULAR,LINEAR) = res.template block<3,3>(LINEAR,ANGULAR).transpose();
464 
465 // res.template block<3,3>(LINEAR,LINEAR) = mv.linear()*c.transpose(); // use as temporary variable
466 // res.template block<3,3>(ANGULAR,ANGULAR) = res.template block<3,3>(LINEAR,LINEAR) - res.template block<3,3>(LINEAR,LINEAR).transpose();
467  res.template block<3,3>(ANGULAR,ANGULAR) = -skewSquare(mv.linear(),lever()) - skewSquare(lever(),mv.linear());
468 
469  res.template block<3,3>(LINEAR,LINEAR) = (inertia() - AlphaSkewSquare(mass(),lever())).matrix();
470 
471  res.template block<3,3>(ANGULAR,ANGULAR) -= res.template block<3,3>(LINEAR,LINEAR) * skew(v.angular());
472  res.template block<3,3>(ANGULAR,ANGULAR) += cross(v.angular(),res.template block<3,3>(LINEAR,LINEAR));
473 
474  res.template block<3,3>(LINEAR,LINEAR).setZero();
475  return res;
476  }
477 
478  template<typename M6>
479  static void vxi_impl(const Motion & v,
480  const InertiaTpl & I,
481  const Eigen::MatrixBase<M6> & Iout)
482  {
483  EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(M6,6,6);
484  M6 & Iout_ = PINOCCHIO_EIGEN_CONST_CAST(M6,Iout);
485 
486  // Block 1,1
487  alphaSkew(I.mass(),v.angular(),Iout_.template block<3,3>(LINEAR,LINEAR));
488 // Iout_.template block<3,3>(LINEAR,LINEAR) = alphaSkew(I.mass(),v.angular());
489  const Vector3 mc(I.mass()*I.lever());
490 
491  // Block 1,2
492  skewSquare(-v.angular(),mc,Iout_.template block<3,3>(LINEAR,ANGULAR));
493 
494 
496  alphaSkew(I.mass(),v.linear(),Iout_.template block<3,3>(ANGULAR,LINEAR));
497  Iout_.template block<3,3>(ANGULAR,LINEAR) -= Iout_.template block<3,3>(LINEAR,ANGULAR);
498 
500  skewSquare(-v.linear(),mc,Iout_.template block<3,3>(ANGULAR,ANGULAR));
501 
502  // TODO: I do not why, but depending on the CPU, these three lines can give
503  // wrong output.
504  // typename Symmetric3::AlphaSkewSquare mcxcx(I.mass(),I.lever());
505  // const Symmetric3 I_mcxcx(I.inertia() - mcxcx);
506  // Iout_.template block<3,3>(ANGULAR,ANGULAR) += I_mcxcx.vxs(v.angular());
507  Symmetric3 mcxcx(typename Symmetric3::AlphaSkewSquare(I.mass(),I.lever()));
508  Iout_.template block<3,3>(ANGULAR,ANGULAR) += I.inertia().vxs(v.angular());
509  Iout_.template block<3,3>(ANGULAR,ANGULAR) -= mcxcx.vxs(v.angular());
510 
511  }
512 
513  template<typename M6>
514  static void ivx_impl(const Motion & v,
515  const InertiaTpl & I,
516  const Eigen::MatrixBase<M6> & Iout)
517  {
518  EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(M6,6,6);
519  M6 & Iout_ = PINOCCHIO_EIGEN_CONST_CAST(M6,Iout);
520 
521  // Block 1,1
522  alphaSkew(I.mass(),v.angular(),Iout_.template block<3,3>(LINEAR,LINEAR));
523 
524  // Block 2,1
525  const Vector3 mc(I.mass()*I.lever());
526  skewSquare(mc,v.angular(),Iout_.template block<3,3>(ANGULAR,LINEAR));
527 
528  // Block 1,2
529  alphaSkew(I.mass(),v.linear(),Iout_.template block<3,3>(LINEAR,ANGULAR));
530 
531  // Block 2,2
532  cross(-I.lever(),Iout_.template block<3,3>(ANGULAR,LINEAR),Iout_.template block<3,3>(ANGULAR,ANGULAR));
533  Iout_.template block<3,3>(ANGULAR,ANGULAR) += I.inertia().svx(v.angular());
534  for(int k = 0; k < 3; ++k)
535  Iout_.template block<3,3>(ANGULAR,ANGULAR).col(k) += I.lever().cross(Iout_.template block<3,3>(LINEAR,ANGULAR).col(k));
536 
537  // Block 1,2
538  Iout_.template block<3,3>(LINEAR,ANGULAR) -= Iout_.template block<3,3>(ANGULAR,LINEAR);
539 
540  }
541 
542  // Getters
543  Scalar mass() const { return m_mass; }
544  const Vector3 & lever() const { return m_com; }
545  const Symmetric3 & inertia() const { return m_inertia; }
546 
547  Scalar & mass() { return m_mass; }
548  Vector3 & lever() { return m_com; }
549  Symmetric3 & inertia() { return m_inertia; }
550 
552  InertiaTpl se3Action_impl(const SE3 & M) const
553  {
554  /* The multiplication RIR' has a particular form that could be used, however it
555  * does not seems to be more efficient, see http://stackoverflow.m_comom/questions/
556  * 13215467/eigen-best-way-to-evaluate-asa-transpose-and-store-the-result-in-a-symmetric .*/
557  return InertiaTpl(mass(),
558  M.translation()+M.rotation()*lever(),
559  inertia().rotate(M.rotation()));
560  }
561 
563  InertiaTpl se3ActionInverse_impl(const SE3 & M) const
564  {
565  return InertiaTpl(mass(),
566  M.rotation().transpose()*(lever()-M.translation()),
567  inertia().rotate(M.rotation().transpose()) );
568  }
569 
570  Force vxiv( const Motion& v ) const
571  {
572  const Vector3 & mcxw = mass()*lever().cross(v.angular());
573  const Vector3 & mv_mcxw = mass()*v.linear()-mcxw;
574  return Force( v.angular().cross(mv_mcxw),
575  v.angular().cross(lever().cross(mv_mcxw)+inertia()*v.angular())-v.linear().cross(mcxw) );
576  }
577 
578  void disp_impl(std::ostream & os) const
579  {
580  os
581  << " m = " << mass() << "\n"
582  << " c = " << lever().transpose() << "\n"
583  << " I = \n" << inertia().matrix() << "";
584  }
585 
587  template<typename NewScalar>
588  InertiaTpl<NewScalar,Options> cast() const
589  {
590  return InertiaTpl<NewScalar,Options>(static_cast<NewScalar>(mass()),
591  lever().template cast<NewScalar>(),
592  inertia().template cast<NewScalar>());
593  }
594 
595  // TODO: adjust code
596 // /// \brief Check whether *this is a valid inertia, resulting from a positive mass distribution
597 // bool isValid() const
598 // {
599 // return
600 // (m_mass > Scalar(0) && m_inertia.isValid())
601 // || (m_mass == Scalar(0) && (m_inertia.data().array() == Scalar(0)).all());
602 // }
603 
604  protected:
605  Scalar m_mass;
606  Vector3 m_com;
607  Symmetric3 m_inertia;
608 
609  }; // class InertiaTpl
610 
611 } // namespace pinocchio
612 
613 #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::FromEllipsoid
static InertiaTpl FromEllipsoid(const Scalar mass, const Scalar x, const Scalar y, const Scalar z)
Computes the Inertia of an ellipsoid defined by its mass and main semi-axis dimensions (x,...
Definition: inertia.hpp:260
pinocchio::InertiaTpl::FromDynamicParameters
static InertiaTpl FromDynamicParameters(const Eigen::MatrixBase< Vector10Like > &params)
Definition: inertia.hpp:355
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::InertiaTpl::FromCylinder
static InertiaTpl FromCylinder(const Scalar mass, const Scalar radius, const Scalar length)
Computes the Inertia of a cylinder defined by its mass, radius and length along the Z axis.
Definition: inertia.hpp:279
pinocchio::SE3Tpl< double, 0 >
pinocchio::InertiaTpl::FromSphere
static InertiaTpl FromSphere(const Scalar mass, const Scalar radius)
Computes the Inertia of a sphere defined by its mass and its radius.
Definition: inertia.hpp:247
pinocchio::MotionDense
Definition: fwd.hpp:41
pinocchio::InertiaTpl::se3ActionInverse_impl
InertiaTpl se3ActionInverse_impl(const SE3 &M) const
bI = aXb.actInv(aI)
Definition: inertia.hpp:563
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:335
pinocchio::InertiaTpl::cast
InertiaTpl< NewScalar, Options > cast() const
Definition: inertia.hpp:588
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::FromBox
static InertiaTpl FromBox(const Scalar mass, const Scalar x, const Scalar y, const Scalar z)
Computes the Inertia of a box defined by its mass and main dimensions (x,y,z).
Definition: inertia.hpp:298
pinocchio::InertiaTpl::se3Action_impl
InertiaTpl se3Action_impl(const SE3 &M) const
aI = aXb.act(bI)
Definition: inertia.hpp:552
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:11