pinocchio  2.2.1-dirty
spatial/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  inertia() = Symmetric3(I3);
185  }
186 
187  InertiaTpl(Scalar mass, const Vector3 & com, const Symmetric3 & rotational_inertia)
188  : m_mass(mass), m_com(com), m_inertia(rotational_inertia)
189  {}
190 
191  InertiaTpl(const InertiaTpl & clone) // Clone constructor for std::vector
192  : m_mass(clone.mass()), m_com(clone.lever()), m_inertia(clone.inertia())
193  {}
194 
195  template<int O2>
196  InertiaTpl(const InertiaTpl<Scalar,O2> & clone)
197  : m_mass(clone.mass())
198  , m_com(clone.lever())
199  , m_inertia(clone.inertia().matrix())
200  {}
201 
202  // Initializers
203  static InertiaTpl Zero()
204  {
205  return InertiaTpl(Scalar(0),
206  Vector3::Zero(),
207  Symmetric3::Zero());
208  }
209 
210  void setZero() { mass() = Scalar(0); lever().setZero(); inertia().setZero(); }
211 
212  static InertiaTpl Identity()
213  {
214  return InertiaTpl(Scalar(1),
215  Vector3::Zero(),
216  Symmetric3::Identity());
217  }
218 
219  void setIdentity ()
220  {
221  mass() = Scalar(1); lever().setZero(); inertia().setIdentity();
222  }
223 
224  static InertiaTpl Random()
225  {
226  // We have to shoot "I" definite positive and not only symmetric.
227  return InertiaTpl(Eigen::internal::random<Scalar>()+1,
228  Vector3::Random(),
229  Symmetric3::RandomPositive());
230  }
231 
232  static InertiaTpl FromEllipsoid(const Scalar m, const Scalar x,
233  const Scalar y, const Scalar z)
234  {
235  Scalar a = m * (y*y + z*z) / Scalar(5);
236  Scalar b = m * (x*x + z*z) / Scalar(5);
237  Scalar c = m * (y*y + x*x) / Scalar(5);
238  return InertiaTpl(m, Vector3::Zero(), Symmetric3(a, Scalar(0), b,
239  Scalar(0), Scalar(0), c));
240  }
241 
242  static InertiaTpl FromCylinder(const Scalar m, const Scalar r, const Scalar l)
243  {
244  Scalar a = m * (r*r / Scalar(4) + l*l / Scalar(12));
245  Scalar c = m * (r*r / Scalar(2));
246  return InertiaTpl(m, Vector3::Zero(), Symmetric3(a, Scalar(0), a,
247  Scalar(0), Scalar(0), c));
248  }
249 
250  static InertiaTpl FromBox(const Scalar m, const Scalar x,
251  const Scalar y, const Scalar z)
252  {
253  Scalar a = m * (y*y + z*z) / Scalar(12);
254  Scalar b = m * (x*x + z*z) / Scalar(12);
255  Scalar c = m * (y*y + x*x) / Scalar(12);
256  return InertiaTpl(m, Vector3::Zero(), Symmetric3(a, Scalar(0), b,
257  Scalar(0), Scalar(0), c));
258  }
259 
260  void setRandom()
261  {
262  mass() = static_cast<Scalar>(std::rand())/static_cast<Scalar>(RAND_MAX);
263  lever().setRandom(); inertia().setRandom();
264  }
265 
266  Matrix6 matrix_impl() const
267  {
268  Matrix6 M;
269 
270  M.template block<3,3>(LINEAR, LINEAR ).setZero();
271  M.template block<3,3>(LINEAR, LINEAR ).diagonal().fill (mass());
272  M.template block<3,3>(ANGULAR,LINEAR ) = alphaSkew(mass(),lever());
273  M.template block<3,3>(LINEAR, ANGULAR) = -M.template block<3,3>(ANGULAR, LINEAR);
274  M.template block<3,3>(ANGULAR,ANGULAR) = (inertia() - AlphaSkewSquare(mass(),lever())).matrix();
275 
276  return M;
277  }
278 
285  Vector10 toDynamicParameters() const
286  {
287  Vector10 v;
288 
289  v[0] = mass();
290  v.template segment<3>(1) = mass() * lever();
291  v.template segment<6>(4) = (inertia() - AlphaSkewSquare(mass(),lever())).data();
292 
293  return v;
294  }
295 
304  template<typename Vector10Like>
305  static InertiaTpl FromDynamicParameters(const Eigen::MatrixBase<Vector10Like> & params)
306  {
307  PINOCCHIO_ASSERT_MATRIX_SPECIFIC_SIZE(Vector10Like, params, 10, 1);
308 
309  const Scalar & mass = params[0];
310  Vector3 lever = params.template segment<3>(1);
311  lever /= mass;
312 
313  return InertiaTpl(mass, lever, Symmetric3(params.template segment<6>(4)) + AlphaSkewSquare(mass,lever));
314  }
315 
316  // Arithmetic operators
317  InertiaTpl & __equl__(const InertiaTpl & clone)
318  {
319  mass() = clone.mass(); lever() = clone.lever(); inertia() = clone.inertia();
320  return *this;
321  }
322 
323  // Required by std::vector boost::python bindings.
324  bool isEqual( const InertiaTpl& Y2 ) const
325  {
326  return (mass()==Y2.mass()) && (lever()==Y2.lever()) && (inertia()==Y2.inertia());
327  }
328 
329  bool isApprox_impl(const InertiaTpl & other,
330  const Scalar & prec = Eigen::NumTraits<Scalar>::dummy_precision()) const
331  {
332  using math::fabs;
333  return fabs(mass() - other.mass()) <= prec
334  && lever().isApprox(other.lever(),prec)
335  && inertia().isApprox(other.inertia(),prec);
336  }
337 
338  bool isZero_impl(const Scalar & prec = Eigen::NumTraits<Scalar>::dummy_precision()) const
339  {
340  using math::fabs;
341  return fabs(mass()) <= prec
342  && lever().isZero(prec)
343  && inertia().isZero(prec);
344  }
345 
346  InertiaTpl __plus__(const InertiaTpl & Yb) const
347  {
348  /* Y_{a+b} = ( m_a+m_b,
349  * (m_a*c_a + m_b*c_b ) / (m_a + m_b),
350  * I_a + I_b - (m_a*m_b)/(m_a+m_b) * AB_x * AB_x )
351  */
352 
353  const Scalar & mab = mass()+Yb.mass();
354  const Scalar mab_inv = Scalar(1)/mab;
355  const Vector3 & AB = (lever()-Yb.lever()).eval();
356  return InertiaTpl(mab,
357  (mass()*lever()+Yb.mass()*Yb.lever())*mab_inv,
358  inertia()+Yb.inertia() - (mass()*Yb.mass()*mab_inv)* typename Symmetric3::SkewSquare(AB));
359  }
360 
361  InertiaTpl& __pequ__(const InertiaTpl & Yb)
362  {
363  const InertiaTpl& Ya = *this;
364  const Scalar & mab = Ya.mass()+Yb.mass();
365  const Scalar mab_inv = 1./mab;
366  const Vector3 & AB = (Ya.lever()-Yb.lever()).eval();
367  lever() *= (mass()*mab_inv); lever() += (Yb.mass()*mab_inv)*Yb.lever(); //c *= mab_inv;
368  inertia() += Yb.inertia(); inertia() -= (Ya.mass()*Yb.mass()*mab_inv)* typename Symmetric3::SkewSquare(AB);
369  mass() = mab;
370  return *this;
371  }
372 
373  template<typename MotionDerived>
375  __mult__(const MotionDense<MotionDerived> & v) const
376  {
378  ReturnType f;
379  __mult__(v,f);
380  return f;
381  }
382 
383  template<typename MotionDerived, typename ForceDerived>
384  void __mult__(const MotionDense<MotionDerived> & v, ForceDense<ForceDerived> & f) const
385  {
386  f.linear().noalias() = mass()*(v.linear() - lever().cross(v.angular()));
387  Symmetric3::rhsMult(inertia(),v.angular(),f.angular());
388  f.angular() += lever().cross(f.linear());
389 // f.angular().noalias() = c.cross(f.linear()) + I*v.angular();
390  }
391 
392  Scalar vtiv_impl(const Motion & v) const
393  {
394  const Vector3 cxw (lever().cross(v.angular()));
395  Scalar res = mass() * (v.linear().squaredNorm() - Scalar(2)*v.linear().dot(cxw));
396  const Vector3 mcxcxw (-mass()*lever().cross(cxw));
397  res += v.angular().dot(mcxcxw);
398  res += inertia().vtiv(v.angular());
399 
400  return res;
401  }
402 
403  Matrix6 variation(const Motion & v) const
404  {
405  Matrix6 res;
406  const Motion mv(v*mass());
407 
408  res.template block<3,3>(LINEAR,ANGULAR) = -skew(mv.linear()) - skewSquare(mv.angular(),lever()) + skewSquare(lever(),mv.angular());
409  res.template block<3,3>(ANGULAR,LINEAR) = res.template block<3,3>(LINEAR,ANGULAR).transpose();
410 
411 // res.template block<3,3>(LINEAR,LINEAR) = mv.linear()*c.transpose(); // use as temporary variable
412 // res.template block<3,3>(ANGULAR,ANGULAR) = res.template block<3,3>(LINEAR,LINEAR) - res.template block<3,3>(LINEAR,LINEAR).transpose();
413  res.template block<3,3>(ANGULAR,ANGULAR) = -skewSquare(mv.linear(),lever()) - skewSquare(lever(),mv.linear());
414 
415  res.template block<3,3>(LINEAR,LINEAR) = (inertia() - AlphaSkewSquare(mass(),lever())).matrix();
416 
417  res.template block<3,3>(ANGULAR,ANGULAR) -= res.template block<3,3>(LINEAR,LINEAR) * skew(v.angular());
418  res.template block<3,3>(ANGULAR,ANGULAR) += cross(v.angular(),res.template block<3,3>(LINEAR,LINEAR));
419 
420  res.template block<3,3>(LINEAR,LINEAR).setZero();
421  return res;
422  }
423 
424  template<typename M6>
425  static void vxi_impl(const Motion & v,
426  const InertiaTpl & I,
427  const Eigen::MatrixBase<M6> & Iout)
428  {
429  EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(M6,6,6);
430  M6 & Iout_ = PINOCCHIO_EIGEN_CONST_CAST(M6,Iout);
431 
432  // Block 1,1
433  alphaSkew(I.mass(),v.angular(),Iout_.template block<3,3>(LINEAR,LINEAR));
434 // Iout_.template block<3,3>(LINEAR,LINEAR) = alphaSkew(I.mass(),v.angular());
435  const Vector3 mc(I.mass()*I.lever());
436 
437  // Block 1,2
438  skewSquare(-v.angular(),mc,Iout_.template block<3,3>(LINEAR,ANGULAR));
439 
440 
442  alphaSkew(I.mass(),v.linear(),Iout_.template block<3,3>(ANGULAR,LINEAR));
443  Iout_.template block<3,3>(ANGULAR,LINEAR) -= Iout_.template block<3,3>(LINEAR,ANGULAR);
444 
446  skewSquare(-v.linear(),mc,Iout_.template block<3,3>(ANGULAR,ANGULAR));
447 
448  // TODO: I do not why, but depending on the CPU, these three lines can give
449  // wrong output.
450  // typename Symmetric3::AlphaSkewSquare mcxcx(I.mass(),I.lever());
451  // const Symmetric3 I_mcxcx(I.inertia() - mcxcx);
452  // Iout_.template block<3,3>(ANGULAR,ANGULAR) += I_mcxcx.vxs(v.angular());
453  Symmetric3 mcxcx(typename Symmetric3::AlphaSkewSquare(I.mass(),I.lever()));
454  Iout_.template block<3,3>(ANGULAR,ANGULAR) += I.inertia().vxs(v.angular());
455  Iout_.template block<3,3>(ANGULAR,ANGULAR) -= mcxcx.vxs(v.angular());
456 
457  }
458 
459  template<typename M6>
460  static void ivx_impl(const Motion & v,
461  const InertiaTpl & I,
462  const Eigen::MatrixBase<M6> & Iout)
463  {
464  EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(M6,6,6);
465  M6 & Iout_ = PINOCCHIO_EIGEN_CONST_CAST(M6,Iout);
466 
467  // Block 1,1
468  alphaSkew(I.mass(),v.angular(),Iout_.template block<3,3>(LINEAR,LINEAR));
469 
470  // Block 2,1
471  const Vector3 mc(I.mass()*I.lever());
472  skewSquare(mc,v.angular(),Iout_.template block<3,3>(ANGULAR,LINEAR));
473 
474  // Block 1,2
475  alphaSkew(I.mass(),v.linear(),Iout_.template block<3,3>(LINEAR,ANGULAR));
476 
477  // Block 2,2
478  cross(-I.lever(),Iout_.template block<3,3>(ANGULAR,LINEAR),Iout_.template block<3,3>(ANGULAR,ANGULAR));
479  Iout_.template block<3,3>(ANGULAR,ANGULAR) += I.inertia().svx(v.angular());
480  for(int k = 0; k < 3; ++k)
481  Iout_.template block<3,3>(ANGULAR,ANGULAR).col(k) += I.lever().cross(Iout_.template block<3,3>(LINEAR,ANGULAR).col(k));
482 
483  // Block 1,2
484  Iout_.template block<3,3>(LINEAR,ANGULAR) -= Iout_.template block<3,3>(ANGULAR,LINEAR);
485 
486  }
487 
488  // Getters
489  Scalar mass() const { return m_mass; }
490  const Vector3 & lever() const { return m_com; }
491  const Symmetric3 & inertia() const { return m_inertia; }
492 
493  Scalar & mass() { return m_mass; }
494  Vector3 & lever() { return m_com; }
495  Symmetric3 & inertia() { return m_inertia; }
496 
498  InertiaTpl se3Action_impl(const SE3 & M) const
499  {
500  /* The multiplication RIR' has a particular form that could be used, however it
501  * does not seems to be more efficient, see http://stackoverflow.m_comom/questions/
502  * 13215467/eigen-best-way-to-evaluate-asa-transpose-and-store-the-result-in-a-symmetric .*/
503  return InertiaTpl(mass(),
504  M.translation()+M.rotation()*lever(),
505  inertia().rotate(M.rotation()));
506  }
507 
509  InertiaTpl se3ActionInverse_impl(const SE3 & M) const
510  {
511  return InertiaTpl(mass(),
512  M.rotation().transpose()*(lever()-M.translation()),
513  inertia().rotate(M.rotation().transpose()) );
514  }
515 
516  Force vxiv( const Motion& v ) const
517  {
518  const Vector3 & mcxw = mass()*lever().cross(v.angular());
519  const Vector3 & mv_mcxw = mass()*v.linear()-mcxw;
520  return Force( v.angular().cross(mv_mcxw),
521  v.angular().cross(lever().cross(mv_mcxw)+inertia()*v.angular())-v.linear().cross(mcxw) );
522  }
523 
524  void disp_impl(std::ostream & os) const
525  {
526  os
527  << " m = " << mass() << "\n"
528  << " c = " << lever().transpose() << "\n"
529  << " I = \n" << inertia().matrix() << "";
530  }
531 
533  template<typename NewScalar>
535  {
536  return InertiaTpl<NewScalar,Options>(static_cast<NewScalar>(mass()),
537  lever().template cast<NewScalar>(),
538  inertia().template cast<NewScalar>());
539  }
540 
541  protected:
542  Scalar m_mass;
543  Vector3 m_com;
544  Symmetric3 m_inertia;
545 
546  }; // class InertiaTpl
547 
548 } // namespace pinocchio
549 
550 #endif // ifndef __pinocchio_inertia_hpp__
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
Derived_t se3Action(const SE3 &M) const
aI = aXb.act(bI)
InertiaTpl< NewScalar, Options > cast() const
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...
InertiaTpl se3ActionInverse_impl(const SE3 &M) const
bI = aXb.actInv(aI)
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...
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
ConstAngularType angular() const
Return the angular part of the force vector.
Definition: force-base.hpp:40
static void vxs(const Eigen::MatrixBase< Vector3 > &v, const Symmetric3Tpl &S3, const Eigen::MatrixBase< Matrix3 > &M)
Performs the operation . This operation is equivalent to applying the cross product of v on each colu...
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
Main pinocchio namespace.
Definition: treeview.dox:24
static void svx(const Eigen::MatrixBase< Vector3 > &v, const Symmetric3Tpl &S3, const Eigen::MatrixBase< Matrix3 > &M)
Performs the operation .
InertiaTpl se3Action_impl(const SE3 &M) const
aI = aXb.act(bI)
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
Common traits structure to fully define base classes for CRTP.
Definition: spatial/fwd.hpp:32
Derived_t se3ActionInverse(const SE3 &M) const
bI = aXb.actInv(aI)
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
ConstLinearType linear() const
Return the linear part of the force vector.
Definition: force-base.hpp:47
Vector10 toDynamicParameters() const
static InertiaTpl FromDynamicParameters(const Eigen::MatrixBase< Vector10Like > &params)