pinocchio  2.5.6
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 & mab = mass()+Yb.mass();
368  const Scalar mab_inv = Scalar(1)/mab;
369  const Vector3 & AB = (lever()-Yb.lever()).eval();
370  return InertiaTpl(mab,
371  (mass()*lever()+Yb.mass()*Yb.lever())*mab_inv,
372  inertia()+Yb.inertia() - (mass()*Yb.mass()*mab_inv)* typename Symmetric3::SkewSquare(AB));
373  }
374 
375  InertiaTpl& __pequ__(const InertiaTpl & Yb)
376  {
377  const InertiaTpl& Ya = *this;
378  const Scalar & mab = Ya.mass()+Yb.mass();
379  const Scalar mab_inv = Scalar(1)/mab;
380  const Vector3 & AB = (Ya.lever()-Yb.lever()).eval();
381  lever() *= (mass()*mab_inv); lever() += (Yb.mass()*mab_inv)*Yb.lever(); //c *= mab_inv;
382  inertia() += Yb.inertia(); inertia() -= (Ya.mass()*Yb.mass()*mab_inv)* typename Symmetric3::SkewSquare(AB);
383  mass() = mab;
384  return *this;
385  }
386 
387  template<typename MotionDerived>
388  ForceTpl<typename traits<MotionDerived>::Scalar,traits<MotionDerived>::Options>
389  __mult__(const MotionDense<MotionDerived> & v) const
390  {
391  typedef ForceTpl<typename traits<MotionDerived>::Scalar,traits<MotionDerived>::Options> ReturnType;
392  ReturnType f;
393  __mult__(v,f);
394  return f;
395  }
396 
397  template<typename MotionDerived, typename ForceDerived>
398  void __mult__(const MotionDense<MotionDerived> & v, ForceDense<ForceDerived> & f) const
399  {
400  f.linear().noalias() = mass()*(v.linear() - lever().cross(v.angular()));
401  Symmetric3::rhsMult(inertia(),v.angular(),f.angular());
402  f.angular() += lever().cross(f.linear());
403 // f.angular().noalias() = c.cross(f.linear()) + I*v.angular();
404  }
405 
406  Scalar vtiv_impl(const Motion & v) const
407  {
408  const Vector3 cxw (lever().cross(v.angular()));
409  Scalar res = mass() * (v.linear().squaredNorm() - Scalar(2)*v.linear().dot(cxw));
410  const Vector3 mcxcxw (-mass()*lever().cross(cxw));
411  res += v.angular().dot(mcxcxw);
412  res += inertia().vtiv(v.angular());
413 
414  return res;
415  }
416 
417  Matrix6 variation(const Motion & v) const
418  {
419  Matrix6 res;
420  const Motion mv(v*mass());
421 
422  res.template block<3,3>(LINEAR,ANGULAR) = -skew(mv.linear()) - skewSquare(mv.angular(),lever()) + skewSquare(lever(),mv.angular());
423  res.template block<3,3>(ANGULAR,LINEAR) = res.template block<3,3>(LINEAR,ANGULAR).transpose();
424 
425 // res.template block<3,3>(LINEAR,LINEAR) = mv.linear()*c.transpose(); // use as temporary variable
426 // res.template block<3,3>(ANGULAR,ANGULAR) = res.template block<3,3>(LINEAR,LINEAR) - res.template block<3,3>(LINEAR,LINEAR).transpose();
427  res.template block<3,3>(ANGULAR,ANGULAR) = -skewSquare(mv.linear(),lever()) - skewSquare(lever(),mv.linear());
428 
429  res.template block<3,3>(LINEAR,LINEAR) = (inertia() - AlphaSkewSquare(mass(),lever())).matrix();
430 
431  res.template block<3,3>(ANGULAR,ANGULAR) -= res.template block<3,3>(LINEAR,LINEAR) * skew(v.angular());
432  res.template block<3,3>(ANGULAR,ANGULAR) += cross(v.angular(),res.template block<3,3>(LINEAR,LINEAR));
433 
434  res.template block<3,3>(LINEAR,LINEAR).setZero();
435  return res;
436  }
437 
438  template<typename M6>
439  static void vxi_impl(const Motion & v,
440  const InertiaTpl & I,
441  const Eigen::MatrixBase<M6> & Iout)
442  {
443  EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(M6,6,6);
444  M6 & Iout_ = PINOCCHIO_EIGEN_CONST_CAST(M6,Iout);
445 
446  // Block 1,1
447  alphaSkew(I.mass(),v.angular(),Iout_.template block<3,3>(LINEAR,LINEAR));
448 // Iout_.template block<3,3>(LINEAR,LINEAR) = alphaSkew(I.mass(),v.angular());
449  const Vector3 mc(I.mass()*I.lever());
450 
451  // Block 1,2
452  skewSquare(-v.angular(),mc,Iout_.template block<3,3>(LINEAR,ANGULAR));
453 
454 
456  alphaSkew(I.mass(),v.linear(),Iout_.template block<3,3>(ANGULAR,LINEAR));
457  Iout_.template block<3,3>(ANGULAR,LINEAR) -= Iout_.template block<3,3>(LINEAR,ANGULAR);
458 
460  skewSquare(-v.linear(),mc,Iout_.template block<3,3>(ANGULAR,ANGULAR));
461 
462  // TODO: I do not why, but depending on the CPU, these three lines can give
463  // wrong output.
464  // typename Symmetric3::AlphaSkewSquare mcxcx(I.mass(),I.lever());
465  // const Symmetric3 I_mcxcx(I.inertia() - mcxcx);
466  // Iout_.template block<3,3>(ANGULAR,ANGULAR) += I_mcxcx.vxs(v.angular());
467  Symmetric3 mcxcx(typename Symmetric3::AlphaSkewSquare(I.mass(),I.lever()));
468  Iout_.template block<3,3>(ANGULAR,ANGULAR) += I.inertia().vxs(v.angular());
469  Iout_.template block<3,3>(ANGULAR,ANGULAR) -= mcxcx.vxs(v.angular());
470 
471  }
472 
473  template<typename M6>
474  static void ivx_impl(const Motion & v,
475  const InertiaTpl & I,
476  const Eigen::MatrixBase<M6> & Iout)
477  {
478  EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(M6,6,6);
479  M6 & Iout_ = PINOCCHIO_EIGEN_CONST_CAST(M6,Iout);
480 
481  // Block 1,1
482  alphaSkew(I.mass(),v.angular(),Iout_.template block<3,3>(LINEAR,LINEAR));
483 
484  // Block 2,1
485  const Vector3 mc(I.mass()*I.lever());
486  skewSquare(mc,v.angular(),Iout_.template block<3,3>(ANGULAR,LINEAR));
487 
488  // Block 1,2
489  alphaSkew(I.mass(),v.linear(),Iout_.template block<3,3>(LINEAR,ANGULAR));
490 
491  // Block 2,2
492  cross(-I.lever(),Iout_.template block<3,3>(ANGULAR,LINEAR),Iout_.template block<3,3>(ANGULAR,ANGULAR));
493  Iout_.template block<3,3>(ANGULAR,ANGULAR) += I.inertia().svx(v.angular());
494  for(int k = 0; k < 3; ++k)
495  Iout_.template block<3,3>(ANGULAR,ANGULAR).col(k) += I.lever().cross(Iout_.template block<3,3>(LINEAR,ANGULAR).col(k));
496 
497  // Block 1,2
498  Iout_.template block<3,3>(LINEAR,ANGULAR) -= Iout_.template block<3,3>(ANGULAR,LINEAR);
499 
500  }
501 
502  // Getters
503  Scalar mass() const { return m_mass; }
504  const Vector3 & lever() const { return m_com; }
505  const Symmetric3 & inertia() const { return m_inertia; }
506 
507  Scalar & mass() { return m_mass; }
508  Vector3 & lever() { return m_com; }
509  Symmetric3 & inertia() { return m_inertia; }
510 
512  InertiaTpl se3Action_impl(const SE3 & M) const
513  {
514  /* The multiplication RIR' has a particular form that could be used, however it
515  * does not seems to be more efficient, see http://stackoverflow.m_comom/questions/
516  * 13215467/eigen-best-way-to-evaluate-asa-transpose-and-store-the-result-in-a-symmetric .*/
517  return InertiaTpl(mass(),
518  M.translation()+M.rotation()*lever(),
519  inertia().rotate(M.rotation()));
520  }
521 
524  {
525  return InertiaTpl(mass(),
526  M.rotation().transpose()*(lever()-M.translation()),
527  inertia().rotate(M.rotation().transpose()) );
528  }
529 
530  Force vxiv( const Motion& v ) const
531  {
532  const Vector3 & mcxw = mass()*lever().cross(v.angular());
533  const Vector3 & mv_mcxw = mass()*v.linear()-mcxw;
534  return Force( v.angular().cross(mv_mcxw),
535  v.angular().cross(lever().cross(mv_mcxw)+inertia()*v.angular())-v.linear().cross(mcxw) );
536  }
537 
538  void disp_impl(std::ostream & os) const
539  {
540  os
541  << " m = " << mass() << "\n"
542  << " c = " << lever().transpose() << "\n"
543  << " I = \n" << inertia().matrix() << "";
544  }
545 
547  template<typename NewScalar>
549  {
550  return InertiaTpl<NewScalar,Options>(static_cast<NewScalar>(mass()),
551  lever().template cast<NewScalar>(),
552  inertia().template cast<NewScalar>());
553  }
554 
555  protected:
556  Scalar m_mass;
557  Vector3 m_com;
558  Symmetric3 m_inertia;
559 
560  }; // class InertiaTpl
561 
562 } // namespace pinocchio
563 
564 #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:523
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:548
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:512
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