6 #ifndef __pinocchio_inertia_hpp__ 7 #define __pinocchio_inertia_hpp__ 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" 20 template<
class Derived>
25 typedef Derived Derived_t;
26 SPATIAL_TYPEDEF_TEMPLATE(Derived_t);
29 Derived_t & derived() {
return *
static_cast<Derived_t*
>(
this); }
30 const Derived_t & derived()
const {
return *
static_cast<const Derived_t*
>(
this); }
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(); }
39 Matrix6 matrix()
const {
return derived().matrix_impl(); }
40 operator Matrix6 ()
const {
return matrix(); }
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); }
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); }
49 template<
typename MotionDerived>
52 {
return derived().__mult__(v); }
54 Scalar vtiv(
const Motion & v)
const {
return derived().vtiv_impl(v); }
55 Matrix6 variation(
const Motion & v)
const {
return derived().variation_impl(v); }
65 static void vxi(
const Motion & v,
const Derived & I,
const Eigen::MatrixBase<M6> & Iout)
67 EIGEN_STATIC_ASSERT_SAME_MATRIX_SIZE(M6, Matrix6);
68 Derived::vxi_impl(v,I,Iout);
74 vxi(v,derived(),Iout);
86 static void ivx(
const Motion & v,
const Derived & I,
const Eigen::MatrixBase<M6> & Iout)
88 EIGEN_STATIC_ASSERT_SAME_MATRIX_SIZE(M6, Matrix6);
89 Derived::ivx_impl(v,I,Iout);
95 ivx(v,derived(),Iout);
99 void setZero() { derived().setZero(); }
100 void setIdentity() { derived().setIdentity(); }
101 void setRandom() { derived().setRandom(); }
103 bool isApprox(
const Derived & other,
const Scalar & prec = Eigen::NumTraits<Scalar>::dummy_precision())
const 104 {
return derived().isApprox_impl(other, prec); }
106 bool isZero(
const Scalar & prec = Eigen::NumTraits<Scalar>::dummy_precision())
const 107 {
return derived().isZero_impl(prec); }
110 Derived_t
se3Action(
const SE3 & M)
const {
return derived().se3Action_impl(M); }
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)
125 template<
typename T,
int U>
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;
151 template<
typename _Scalar,
int _Options>
155 friend class InertiaBase< InertiaTpl< _Scalar, _Options > >;
156 SPATIAL_TYPEDEF_TEMPLATE(InertiaTpl);
157 enum { Options = _Options };
158 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
162 typedef typename Eigen::Matrix<_Scalar, 10, 1, _Options> Vector10;
169 InertiaTpl(
const Scalar mass,
const Vector3 & com,
const Matrix3 & rotational_inertia)
170 : m_mass(mass), m_com(com), m_inertia(rotational_inertia)
173 InertiaTpl(
const Matrix6 & I6)
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);
181 Matrix3 I3 (mc_cross * mc_cross);
183 I3 += I6.template block<3,3>(ANGULAR,ANGULAR);
187 InertiaTpl(Scalar mass,
const Vector3 & com,
const Symmetric3 & rotational_inertia)
188 : m_mass(mass), m_com(com), m_inertia(rotational_inertia)
191 InertiaTpl(
const InertiaTpl & clone)
192 : m_mass(clone.mass()), m_com(clone.lever()), m_inertia(clone.inertia())
197 : m_mass(clone.mass())
198 , m_com(clone.lever())
199 , m_inertia(clone.inertia().matrix())
203 static InertiaTpl Zero()
205 return InertiaTpl(Scalar(0),
210 void setZero() { mass() = Scalar(0); lever().setZero(); inertia().setZero(); }
212 static InertiaTpl Identity()
214 return InertiaTpl(Scalar(1),
216 Symmetric3::Identity());
221 mass() = Scalar(1); lever().setZero(); inertia().setIdentity();
224 static InertiaTpl Random()
227 return InertiaTpl(Eigen::internal::random<Scalar>()+1,
229 Symmetric3::RandomPositive());
232 static InertiaTpl FromEllipsoid(
const Scalar m,
const Scalar x,
233 const Scalar y,
const Scalar z)
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));
242 static InertiaTpl FromCylinder(
const Scalar m,
const Scalar r,
const Scalar l)
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));
250 static InertiaTpl FromBox(
const Scalar m,
const Scalar x,
251 const Scalar y,
const Scalar z)
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));
262 mass() =
static_cast<Scalar
>(std::rand())/static_cast<Scalar>(RAND_MAX);
263 lever().setRandom(); inertia().setRandom();
266 Matrix6 matrix_impl()
const 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();
290 v.template segment<3>(1) = mass() * lever();
291 v.template segment<6>(4) = (inertia() - AlphaSkewSquare(mass(),lever())).data();
304 template<
typename Vector10Like>
307 PINOCCHIO_ASSERT_MATRIX_SPECIFIC_SIZE(Vector10Like, params, 10, 1);
309 const Scalar & mass = params[0];
310 Vector3 lever = params.template segment<3>(1);
313 return InertiaTpl(mass, lever,
Symmetric3(params.template segment<6>(4)) + AlphaSkewSquare(mass,lever));
317 InertiaTpl & __equl__(
const InertiaTpl & clone)
319 mass() = clone.mass(); lever() = clone.lever(); inertia() = clone.inertia();
324 bool isEqual(
const InertiaTpl& Y2 )
const 326 return (mass()==Y2.mass()) && (lever()==Y2.lever()) && (inertia()==Y2.inertia());
329 bool isApprox_impl(
const InertiaTpl & other,
330 const Scalar & prec = Eigen::NumTraits<Scalar>::dummy_precision())
const 333 return fabs(mass() - other.mass()) <= prec
334 && lever().isApprox(other.lever(),prec)
335 && inertia().isApprox(other.inertia(),prec);
338 bool isZero_impl(
const Scalar & prec = Eigen::NumTraits<Scalar>::dummy_precision())
const 341 return fabs(mass()) <= prec
342 && lever().isZero(prec)
343 && inertia().isZero(prec);
346 InertiaTpl __plus__(
const InertiaTpl & Yb)
const 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));
361 InertiaTpl& __pequ__(
const InertiaTpl & Yb)
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();
368 inertia() += Yb.inertia(); inertia() -= (Ya.mass()*Yb.mass()*mab_inv)*
typename Symmetric3::SkewSquare(AB);
373 template<
typename MotionDerived>
383 template<
typename MotionDerived,
typename ForceDerived>
386 f.
linear().noalias() = mass()*(v.linear() - lever().cross(v.angular()));
387 Symmetric3::rhsMult(inertia(),v.angular(),f.
angular());
392 Scalar vtiv_impl(
const Motion & v)
const 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());
403 Matrix6 variation(
const Motion & v)
const 406 const Motion mv(v*mass());
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();
413 res.template block<3,3>(ANGULAR,ANGULAR) = -
skewSquare(mv.linear(),lever()) -
skewSquare(lever(),mv.linear());
415 res.template block<3,3>(LINEAR,LINEAR) = (inertia() - AlphaSkewSquare(mass(),lever())).matrix();
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));
420 res.template block<3,3>(LINEAR,LINEAR).setZero();
424 template<
typename M6>
425 static void vxi_impl(
const Motion & v,
426 const InertiaTpl & I,
427 const Eigen::MatrixBase<M6> & Iout)
429 EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(M6,6,6);
430 M6 & Iout_ = PINOCCHIO_EIGEN_CONST_CAST(M6,Iout);
433 alphaSkew(I.mass(),v.angular(),Iout_.template block<3,3>(LINEAR,LINEAR));
435 const Vector3 mc(I.mass()*I.lever());
438 skewSquare(-v.angular(),mc,Iout_.template block<3,3>(LINEAR,ANGULAR));
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);
446 skewSquare(-v.linear(),mc,Iout_.template block<3,3>(ANGULAR,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());
459 template<
typename M6>
460 static void ivx_impl(
const Motion & v,
461 const InertiaTpl & I,
462 const Eigen::MatrixBase<M6> & Iout)
464 EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(M6,6,6);
465 M6 & Iout_ = PINOCCHIO_EIGEN_CONST_CAST(M6,Iout);
468 alphaSkew(I.mass(),v.angular(),Iout_.template block<3,3>(LINEAR,LINEAR));
471 const Vector3 mc(I.mass()*I.lever());
472 skewSquare(mc,v.angular(),Iout_.template block<3,3>(ANGULAR,LINEAR));
475 alphaSkew(I.mass(),v.linear(),Iout_.template block<3,3>(LINEAR,ANGULAR));
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));
484 Iout_.template block<3,3>(LINEAR,ANGULAR) -= Iout_.template block<3,3>(ANGULAR,LINEAR);
489 Scalar mass()
const {
return m_mass; }
490 const Vector3 & lever()
const {
return m_com; }
491 const Symmetric3 & inertia()
const {
return m_inertia; }
493 Scalar & mass() {
return m_mass; }
494 Vector3 & lever() {
return m_com; }
503 return InertiaTpl(mass(),
504 M.translation()+M.rotation()*lever(),
505 inertia().rotate(M.rotation()));
511 return InertiaTpl(mass(),
512 M.rotation().transpose()*(lever()-M.translation()),
513 inertia().rotate(M.rotation().transpose()) );
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) );
524 void disp_impl(std::ostream & os)
const 527 <<
" m = " << mass() <<
"\n" 528 <<
" c = " << lever().transpose() <<
"\n" 529 <<
" I = \n" << inertia().matrix() <<
"";
533 template<
typename NewScalar>
537 lever().template cast<NewScalar>(),
538 inertia().template cast<NewScalar>());
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...
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...
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...
InertiaTpl se3ActionInverse_impl(const SE3 &M) const
bI = aXb.actInv(aI)
ConstAngularType angular() const
Return the angular part of the force vector.
Derived_t se3ActionInverse(const SE3 &M) const
bI = aXb.actInv(aI)
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.
InertiaTpl< NewScalar, Options > cast() const
Derived_t se3Action(const SE3 &M) const
aI = aXb.act(bI)
Vector10 toDynamicParameters() const
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...
Main pinocchio namespace.
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 ( )
Common traits structure to fully define base classes for CRTP.
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...
ConstLinearType linear() const
Return the linear part of the force vector.
static InertiaTpl FromDynamicParameters(const Eigen::MatrixBase< Vector10Like > ¶ms)