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>
157 enum { Options = _Options };
158 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
160 typedef typename Symmetric3::AlphaSkewSquare AlphaSkewSquare;
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);
184 const Symmetric3 S3(I3);
188 InertiaTpl(Scalar mass,
const Vector3 & com,
const Symmetric3 & rotational_inertia)
189 : m_mass(mass), m_com(com), m_inertia(rotational_inertia)
192 InertiaTpl(
const InertiaTpl & clone)
193 : m_mass(clone.mass()), m_com(clone.lever()), m_inertia(clone.inertia())
196 InertiaTpl& operator=(
const InertiaTpl & clone)
198 m_mass = clone.mass();
199 m_com = clone.lever();
200 m_inertia = clone.inertia();
205 InertiaTpl(
const InertiaTpl<Scalar,O2> & clone)
206 : m_mass(clone.mass())
207 , m_com(clone.lever())
208 , m_inertia(clone.inertia().matrix())
212 static InertiaTpl Zero()
214 return InertiaTpl(Scalar(0),
219 void setZero() { mass() = Scalar(0); lever().setZero(); inertia().setZero(); }
221 static InertiaTpl Identity()
223 return InertiaTpl(Scalar(1),
225 Symmetric3::Identity());
230 mass() = Scalar(1); lever().setZero(); inertia().setIdentity();
233 static InertiaTpl Random()
236 return InertiaTpl(Eigen::internal::random<Scalar>()+1,
238 Symmetric3::RandomPositive());
241 static InertiaTpl FromSphere(
const Scalar m,
const Scalar radius)
243 return FromEllipsoid(m,radius,radius,radius);
246 static InertiaTpl FromEllipsoid(
const Scalar m,
const Scalar x,
247 const Scalar y,
const Scalar z)
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));
256 static InertiaTpl FromCylinder(
const Scalar m,
const Scalar r,
const Scalar l)
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));
264 static InertiaTpl FromBox(
const Scalar m,
const Scalar x,
265 const Scalar y,
const Scalar z)
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));
276 mass() =
static_cast<Scalar
>(std::rand())/
static_cast<Scalar
>(RAND_MAX);
277 lever().setRandom(); inertia().setRandom();
280 Matrix6 matrix_impl()
const
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();
304 v.template segment<3>(1) = mass() * lever();
305 v.template segment<6>(4) = (inertia() - AlphaSkewSquare(mass(),lever())).data();
318 template<
typename Vector10Like>
321 PINOCCHIO_ASSERT_MATRIX_SPECIFIC_SIZE(Vector10Like, params, 10, 1);
323 const Scalar & mass = params[0];
324 Vector3 lever = params.template segment<3>(1);
327 return InertiaTpl(mass, lever,
Symmetric3(params.template segment<6>(4)) + AlphaSkewSquare(mass,lever));
333 mass() = clone.mass(); lever() = clone.lever(); inertia() = clone.inertia();
338 bool isEqual(
const InertiaTpl& Y2 )
const
340 return (mass()==Y2.mass()) && (lever()==Y2.lever()) && (inertia()==Y2.inertia());
343 bool isApprox_impl(
const InertiaTpl & other,
344 const Scalar & prec = Eigen::NumTraits<Scalar>::dummy_precision())
const
347 return fabs(
static_cast<Scalar
>(mass() - other.mass())) <= prec
348 && lever().isApprox(other.lever(),prec)
349 && inertia().isApprox(other.inertia(),prec);
352 bool isZero_impl(
const Scalar & prec = Eigen::NumTraits<Scalar>::dummy_precision())
const
355 return fabs(mass()) <= prec
356 && lever().isZero(prec)
357 && inertia().isZero(prec);
360 InertiaTpl __plus__(
const InertiaTpl & Yb)
const
367 const Scalar eps = ::Eigen::NumTraits<Scalar>::epsilon();
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));
377 InertiaTpl& __pequ__(
const InertiaTpl & Yb)
379 const Scalar eps = ::Eigen::NumTraits<Scalar>::epsilon();
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();
386 inertia() += Yb.inertia(); inertia() -= (Ya.mass()*Yb.mass()*mab_inv)*
typename Symmetric3::SkewSquare(AB);
391 template<
typename MotionDerived>
392 ForceTpl<typename traits<MotionDerived>::Scalar,traits<MotionDerived>::Options>
393 __mult__(
const MotionDense<MotionDerived> & v)
const
395 typedef ForceTpl<typename traits<MotionDerived>::Scalar,traits<MotionDerived>::Options> ReturnType;
401 template<
typename MotionDerived,
typename ForceDerived>
402 void __mult__(
const MotionDense<MotionDerived> & v, ForceDense<ForceDerived> & f)
const
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());
410 Scalar vtiv_impl(
const Motion & v)
const
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());
421 Matrix6 variation(
const Motion & v)
const
424 const Motion mv(v*mass());
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();
431 res.template block<3,3>(ANGULAR,ANGULAR) = -
skewSquare(mv.linear(),lever()) -
skewSquare(lever(),mv.linear());
433 res.template block<3,3>(LINEAR,LINEAR) = (inertia() - AlphaSkewSquare(mass(),lever())).matrix();
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));
438 res.template block<3,3>(LINEAR,LINEAR).setZero();
442 template<
typename M6>
443 static void vxi_impl(
const Motion & v,
444 const InertiaTpl & I,
445 const Eigen::MatrixBase<M6> & Iout)
447 EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(M6,6,6);
448 M6 & Iout_ = PINOCCHIO_EIGEN_CONST_CAST(M6,Iout);
451 alphaSkew(I.mass(),v.angular(),Iout_.template block<3,3>(LINEAR,LINEAR));
453 const Vector3 mc(I.mass()*I.lever());
456 skewSquare(-v.angular(),mc,Iout_.template block<3,3>(LINEAR,ANGULAR));
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);
464 skewSquare(-v.linear(),mc,Iout_.template block<3,3>(ANGULAR,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());
477 template<
typename M6>
478 static void ivx_impl(
const Motion & v,
479 const InertiaTpl & I,
480 const Eigen::MatrixBase<M6> & Iout)
482 EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(M6,6,6);
483 M6 & Iout_ = PINOCCHIO_EIGEN_CONST_CAST(M6,Iout);
486 alphaSkew(I.mass(),v.angular(),Iout_.template block<3,3>(LINEAR,LINEAR));
489 const Vector3 mc(I.mass()*I.lever());
490 skewSquare(mc,v.angular(),Iout_.template block<3,3>(ANGULAR,LINEAR));
493 alphaSkew(I.mass(),v.linear(),Iout_.template block<3,3>(LINEAR,ANGULAR));
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));
502 Iout_.template block<3,3>(LINEAR,ANGULAR) -= Iout_.template block<3,3>(ANGULAR,LINEAR);
507 Scalar mass()
const {
return m_mass; }
508 const Vector3 & lever()
const {
return m_com; }
509 const Symmetric3 & inertia()
const {
return m_inertia; }
511 Scalar & mass() {
return m_mass; }
512 Vector3 & lever() {
return m_com; }
513 Symmetric3 & inertia() {
return m_inertia; }
522 M.translation()+M.rotation()*lever(),
523 inertia().rotate(M.rotation()));
530 M.rotation().transpose()*(lever()-M.translation()),
531 inertia().rotate(M.rotation().transpose()) );
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) );
542 void disp_impl(std::ostream & os)
const
545 <<
" m = " << mass() <<
"\n"
546 <<
" c = " << lever().transpose() <<
"\n"
547 <<
" I = \n" << inertia().matrix() <<
"";
551 template<
typename NewScalar>
555 lever().
template cast<NewScalar>(),
556 inertia().
template cast<NewScalar>());
577 #endif // ifndef __pinocchio_inertia_hpp__