Program Listing for File inertia.hpp
↰ Return to documentation for file (include/pinocchio/spatial/inertia.hpp
)
//
// Copyright (c) 2015-2021 CNRS INRIA
// Copyright (c) 2016 Wandercraft, 86 rue de Paris 91400 Orsay, France.
//
#ifndef __pinocchio_spatial_inertia_hpp__
#define __pinocchio_spatial_inertia_hpp__
#include "pinocchio/math/fwd.hpp"
#include "pinocchio/spatial/symmetric3.hpp"
#include "pinocchio/spatial/force.hpp"
#include "pinocchio/spatial/motion.hpp"
#include "pinocchio/spatial/skew.hpp"
namespace pinocchio
{
template<class Derived>
struct InertiaBase : NumericalBase<Derived>
{
SPATIAL_TYPEDEF_TEMPLATE(Derived);
Derived & derived()
{
return *static_cast<Derived *>(this);
}
const Derived & derived() const
{
return *static_cast<const Derived *>(this);
}
Derived & const_cast_derived() const
{
return *const_cast<Derived *>(&derived());
}
Scalar mass() const
{
return static_cast<const Derived *>(this)->mass();
}
Scalar & mass()
{
return static_cast<const Derived *>(this)->mass();
}
const Vector3 & lever() const
{
return static_cast<const Derived *>(this)->lever();
}
Vector3 & lever()
{
return static_cast<const Derived *>(this)->lever();
}
const Symmetric3 & inertia() const
{
return static_cast<const Derived *>(this)->inertia();
}
Symmetric3 & inertia()
{
return static_cast<const Derived *>(this)->inertia();
}
template<typename Matrix6Like>
void matrix(const Eigen::MatrixBase<Matrix6Like> & mat) const
{
derived().matrix_impl(mat);
}
Matrix6 matrix() const
{
return derived().matrix_impl();
}
operator Matrix6() const
{
return matrix();
}
template<typename Matrix6Like>
void inverse(const Eigen::MatrixBase<Matrix6Like> & mat) const
{
derived().inverse_impl(mat);
}
Matrix6 inverse() const
{
return derived().inverse_impl();
}
Derived & operator=(const Derived & clone)
{
return derived().__equl__(clone);
}
bool operator==(const Derived & other) const
{
return derived().isEqual(other);
}
bool operator!=(const Derived & other) const
{
return !(*this == other);
}
Derived & operator+=(const Derived & Yb)
{
return derived().__pequ__(Yb);
}
Derived operator+(const Derived & Yb) const
{
return derived().__plus__(Yb);
}
Derived & operator-=(const Derived & Yb)
{
return derived().__mequ__(Yb);
}
Derived operator-(const Derived & Yb) const
{
return derived().__minus__(Yb);
}
template<typename MotionDerived>
ForceTpl<typename traits<MotionDerived>::Scalar, traits<MotionDerived>::Options>
operator*(const MotionDense<MotionDerived> & v) const
{
return derived().__mult__(v);
}
template<typename MotionDerived>
Scalar vtiv(const MotionDense<MotionDerived> & v) const
{
return derived().vtiv_impl(v);
}
template<typename MotionDerived>
Matrix6 variation(const MotionDense<MotionDerived> & v) const
{
return derived().variation_impl(v);
}
template<typename MotionDerived, typename M6>
static void
vxi(const MotionDense<MotionDerived> & v, const Derived & I, const Eigen::MatrixBase<M6> & Iout)
{
EIGEN_STATIC_ASSERT_SAME_MATRIX_SIZE(M6, Matrix6);
Derived::vxi_impl(v, I, Iout);
}
template<typename MotionDerived>
Matrix6 vxi(const MotionDense<MotionDerived> & v) const
{
Matrix6 Iout;
vxi(v, derived(), Iout);
return Iout;
}
template<typename MotionDerived, typename M6>
static void
ivx(const MotionDense<MotionDerived> & v, const Derived & I, const Eigen::MatrixBase<M6> & Iout)
{
EIGEN_STATIC_ASSERT_SAME_MATRIX_SIZE(M6, Matrix6);
Derived::ivx_impl(v, I, Iout);
}
template<typename MotionDerived>
Matrix6 ivx(const MotionDense<MotionDerived> & v) const
{
Matrix6 Iout;
ivx(v, derived(), Iout);
return Iout;
}
void setZero()
{
derived().setZero();
}
void setIdentity()
{
derived().setIdentity();
}
void setRandom()
{
derived().setRandom();
}
bool isApprox(
const Derived & other,
const Scalar & prec = Eigen::NumTraits<Scalar>::dummy_precision()) const
{
return derived().isApprox_impl(other, prec);
}
bool isZero(const Scalar & prec = Eigen::NumTraits<Scalar>::dummy_precision()) const
{
return derived().isZero_impl(prec);
}
template<typename S2, int O2>
Derived se3Action(const SE3Tpl<S2, O2> & M) const
{
return derived().se3Action_impl(M);
}
template<typename S2, int O2>
Derived se3ActionInverse(const SE3Tpl<S2, O2> & M) const
{
return derived().se3ActionInverse_impl(M);
}
void disp(std::ostream & os) const
{
static_cast<const Derived *>(this)->disp_impl(os);
}
friend std::ostream & operator<<(std::ostream & os, const InertiaBase<Derived> & X)
{
X.disp(os);
return os;
}
};
// class InertiaBase
template<typename T, int U>
struct traits<InertiaTpl<T, U>>
{
typedef T Scalar;
typedef Eigen::Matrix<T, 3, 1, U> Vector3;
typedef Eigen::Matrix<T, 4, 1, U> Vector4;
typedef Eigen::Matrix<T, 6, 1, U> Vector6;
typedef Eigen::Matrix<T, 3, 3, U> Matrix3;
typedef Eigen::Matrix<T, 4, 4, U> Matrix4;
typedef Eigen::Matrix<T, 6, 6, U> Matrix6;
typedef Eigen::Matrix<T, 10, 10, U> Matrix10;
typedef Matrix6 ActionMatrix_t;
typedef Vector3 Angular_t;
typedef Vector3 Linear_t;
typedef const Vector3 ConstAngular_t;
typedef const Vector3 ConstLinear_t;
typedef Eigen::Quaternion<T, U> Quaternion_t;
typedef SE3Tpl<T, U> SE3;
typedef ForceTpl<T, U> Force;
typedef MotionTpl<T, U> Motion;
typedef Symmetric3Tpl<T, U> Symmetric3;
typedef PseudoInertiaTpl<T, U> PseudoInertia;
typedef LogCholeskyParametersTpl<T, U> LogCholeskyParameters;
enum
{
LINEAR = 0,
ANGULAR = 3
};
}; // traits InertiaTpl
template<typename _Scalar, int _Options>
struct InertiaTpl : public InertiaBase<InertiaTpl<_Scalar, _Options>>
{
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
SPATIAL_TYPEDEF_TEMPLATE(InertiaTpl);
enum
{
Options = _Options
};
typedef typename Symmetric3::AlphaSkewSquare AlphaSkewSquare;
typedef Eigen::Matrix<Scalar, 10, 1, Options> Vector10;
typedef Eigen::Matrix<Scalar, 10, 10, Options> Matrix10;
typedef PseudoInertiaTpl<Scalar, Options> PseudoInertia;
typedef LogCholeskyParametersTpl<Scalar, Options> LogCholeskyParameters;
// Constructors
InertiaTpl()
{
}
InertiaTpl(const Scalar & mass, const Vector3 & com, const Matrix3 & rotational_inertia)
: m_mass(mass)
, m_com(com)
, m_inertia(rotational_inertia)
{
}
explicit InertiaTpl(const Matrix6 & I6)
{
assert(check_expression_if_real<Scalar>(pinocchio::isZero(I6 - I6.transpose())));
mass() = I6(LINEAR, LINEAR);
const typename Matrix6::template ConstFixedBlockXpr<3, 3>::Type mc_cross =
I6.template block<3, 3>(ANGULAR, LINEAR);
lever() = unSkew(mc_cross);
lever() /= mass();
Matrix3 I3(mc_cross * mc_cross);
I3 /= mass();
I3 += I6.template block<3, 3>(ANGULAR, ANGULAR);
const Symmetric3 S3(I3);
inertia() = S3;
}
InertiaTpl(const Scalar & mass, const Vector3 & com, const Symmetric3 & rotational_inertia)
: m_mass(mass)
, m_com(com)
, m_inertia(rotational_inertia)
{
}
InertiaTpl(const InertiaTpl & clone) // Copy constructor
: m_mass(clone.mass())
, m_com(clone.lever())
, m_inertia(clone.inertia())
{
}
InertiaTpl & operator=(const InertiaTpl & clone) // Copy assignment operator
{
m_mass = clone.mass();
m_com = clone.lever();
m_inertia = clone.inertia();
return *this;
}
template<typename S2, int O2>
explicit InertiaTpl(const InertiaTpl<S2, O2> & clone)
{
*this = clone.template cast<Scalar>();
}
// Initializers
static InertiaTpl Zero()
{
return InertiaTpl(Scalar(0), Vector3::Zero(), Symmetric3::Zero());
}
void setZero()
{
mass() = Scalar(0);
lever().setZero();
inertia().setZero();
}
static InertiaTpl Identity()
{
return InertiaTpl(Scalar(1), Vector3::Zero(), Symmetric3::Identity());
}
void setIdentity()
{
mass() = Scalar(1);
lever().setZero();
inertia().setIdentity();
}
static InertiaTpl Random()
{
// We have to shoot "I" definite positive and not only symmetric.
return InertiaTpl(
Eigen::internal::random<Scalar>() + 1, Vector3::Random(), Symmetric3::RandomPositive());
}
static InertiaTpl FromSphere(const Scalar mass, const Scalar radius)
{
return FromEllipsoid(mass, radius, radius, radius);
}
static InertiaTpl
FromEllipsoid(const Scalar mass, const Scalar x, const Scalar y, const Scalar z)
{
const Scalar a = mass * (y * y + z * z) / Scalar(5);
const Scalar b = mass * (x * x + z * z) / Scalar(5);
const Scalar c = mass * (y * y + x * x) / Scalar(5);
return InertiaTpl(
mass, Vector3::Zero(), Symmetric3(a, Scalar(0), b, Scalar(0), Scalar(0), c));
}
static InertiaTpl FromCylinder(const Scalar mass, const Scalar radius, const Scalar length)
{
const Scalar radius_square = radius * radius;
const Scalar a = mass * (radius_square / Scalar(4) + length * length / Scalar(12));
const Scalar c = mass * (radius_square / Scalar(2));
return InertiaTpl(
mass, Vector3::Zero(), Symmetric3(a, Scalar(0), a, Scalar(0), Scalar(0), c));
}
static InertiaTpl FromBox(const Scalar mass, const Scalar x, const Scalar y, const Scalar z)
{
const Scalar a = mass * (y * y + z * z) / Scalar(12);
const Scalar b = mass * (x * x + z * z) / Scalar(12);
const Scalar c = mass * (y * y + x * x) / Scalar(12);
return InertiaTpl(
mass, Vector3::Zero(), Symmetric3(a, Scalar(0), b, Scalar(0), Scalar(0), c));
}
static InertiaTpl FromCapsule(const Scalar mass, const Scalar radius, const Scalar height)
{
const Scalar pi = boost::math::constants::pi<Scalar>();
// first need to compute mass repartition between cylinder and halfsphere
const Scalar v_cyl = pi * math::pow(radius, 2) * height;
const Scalar v_hs = Scalar(2) / Scalar(3) * math::pow(radius, 3) * pi;
const Scalar total_v = v_cyl + Scalar(2) * v_hs;
const Scalar m_cyl = mass * (v_cyl / total_v);
const Scalar m_hs = mass * (v_hs / total_v);
// Then Distance between halfSphere center and cylindere center.
const Scalar dist_hc_cyl = height / Scalar(2) + Scalar(0.375) * radius;
// Computes inertia terms
const Scalar ix_c =
m_cyl * (math::pow(height, 2) / Scalar(12) + math::pow(radius, 2) / Scalar(4));
const Scalar iz_c = m_cyl * math::pow(radius, 2) / Scalar(2);
// For halfsphere inertia see, "Dynamics: Theory and Applications" McGraw-Hill, New York,
// 1985, by T.R. Kane and D.A. Levinson, Appendix 23.
const Scalar ix_hs = m_hs * math::pow(radius, 2) * Scalar(0.259375);
const Scalar iz_hs = m_hs * math::pow(radius, 2) * Scalar(0.4);
// Put everything together using the parallel axis theorem
const Scalar ix = ix_c + Scalar(2) * (ix_hs + m_hs * math::pow(dist_hc_cyl, 2));
const Scalar iz = iz_c + Scalar(2) * iz_hs;
return InertiaTpl(
mass, Vector3::Zero(), Symmetric3(ix, Scalar(0), ix, Scalar(0), Scalar(0), iz));
}
void setRandom()
{
mass() = static_cast<Scalar>(std::rand()) / static_cast<Scalar>(RAND_MAX);
lever().setRandom();
inertia().setRandom();
}
template<typename Matrix6Like>
void matrix_impl(const Eigen::MatrixBase<Matrix6Like> & M_) const
{
Matrix6Like & M = M_.const_cast_derived();
M.template block<3, 3>(LINEAR, LINEAR).setZero();
M.template block<3, 3>(LINEAR, LINEAR).diagonal().fill(mass());
M.template block<3, 3>(ANGULAR, LINEAR) = alphaSkew(mass(), lever());
M.template block<3, 3>(LINEAR, ANGULAR) = -M.template block<3, 3>(ANGULAR, LINEAR);
M.template block<3, 3>(ANGULAR, ANGULAR) =
(inertia() - AlphaSkewSquare(mass(), lever())).matrix();
}
Matrix6 matrix_impl() const
{
Matrix6 M;
matrix_impl(M);
return M;
}
template<typename Matrix6Like>
void inverse_impl(const Eigen::MatrixBase<Matrix6Like> & M_) const
{
Matrix6Like & M = M_.const_cast_derived();
inertia().inverse(M.template block<3, 3>(ANGULAR, ANGULAR));
M.template block<3, 3>(LINEAR, ANGULAR).noalias() =
-M.template block<3, 3>(ANGULAR, ANGULAR).colwise().cross(lever());
M.template block<3, 3>(ANGULAR, LINEAR) = M.template block<3, 3>(LINEAR, ANGULAR).transpose();
const Scalar &cx = lever()[0], cy = lever()[1], cz = lever()[2];
M.template block<3, 3>(LINEAR, LINEAR).col(0).noalias() =
cy * M.template block<3, 3>(LINEAR, ANGULAR).col(2)
- cz * M.template block<3, 3>(LINEAR, ANGULAR).col(1);
M.template block<3, 3>(LINEAR, LINEAR).col(1).noalias() =
cz * M.template block<3, 3>(LINEAR, ANGULAR).col(0)
- cx * M.template block<3, 3>(LINEAR, ANGULAR).col(2);
M.template block<3, 3>(LINEAR, LINEAR).col(2).noalias() =
cx * M.template block<3, 3>(LINEAR, ANGULAR).col(1)
- cy * M.template block<3, 3>(LINEAR, ANGULAR).col(0);
const Scalar m_inv = Scalar(1) / mass();
M.template block<3, 3>(LINEAR, LINEAR).diagonal().array() += m_inv;
}
Matrix6 inverse_impl() const
{
Matrix6 res;
inverse_impl(res);
return res;
}
Vector10 toDynamicParameters() const
{
Vector10 v;
v[0] = mass();
v.template segment<3>(1).noalias() = mass() * lever();
v.template segment<6>(4) = (inertia() - AlphaSkewSquare(mass(), lever())).data();
return v;
}
template<typename Vector10Like>
static InertiaTpl FromDynamicParameters(const Eigen::MatrixBase<Vector10Like> & params)
{
PINOCCHIO_ASSERT_MATRIX_SPECIFIC_SIZE(Vector10Like, params, 10, 1);
const Scalar & mass = params[0];
Vector3 lever = params.template segment<3>(1);
lever /= mass;
return InertiaTpl(
mass, lever, Symmetric3(params.template segment<6>(4)) + AlphaSkewSquare(mass, lever));
}
static InertiaTpl FromPseudoInertia(const PseudoInertia & pseudo_inertia)
{
return pseudo_inertia.toInertia();
}
PseudoInertia toPseudoInertia() const
{
PseudoInertia pseudo_inertia = PseudoInertia::FromInertia(*this);
return pseudo_inertia;
}
static InertiaTpl FromLogCholeskyParameters(const LogCholeskyParameters & log_cholesky)
{
Vector10 dynamic_params = log_cholesky.toDynamicParameters();
return FromDynamicParameters(dynamic_params);
}
// Arithmetic operators
InertiaTpl & __equl__(const InertiaTpl & clone)
{
mass() = clone.mass();
lever() = clone.lever();
inertia() = clone.inertia();
return *this;
}
// Required by std::vector boost::python bindings.
bool isEqual(const InertiaTpl & Y2) const
{
return (mass() == Y2.mass()) && (lever() == Y2.lever()) && (inertia() == Y2.inertia());
}
bool isApprox_impl(
const InertiaTpl & other,
const Scalar & prec = Eigen::NumTraits<Scalar>::dummy_precision()) const
{
using math::fabs;
return fabs(static_cast<Scalar>(mass() - other.mass())) <= prec
&& lever().isApprox(other.lever(), prec) && inertia().isApprox(other.inertia(), prec);
}
bool isZero_impl(const Scalar & prec = Eigen::NumTraits<Scalar>::dummy_precision()) const
{
using math::fabs;
return fabs(mass()) <= prec && lever().isZero(prec) && inertia().isZero(prec);
}
InertiaTpl __plus__(const InertiaTpl & Yb) const
{
/* Y_{a+b} = ( m_a+m_b,
* (m_a*c_a + m_b*c_b ) / (m_a + m_b),
* I_a + I_b - (m_a*m_b)/(m_a+m_b) * AB_x * AB_x )
*/
const Scalar eps = ::Eigen::NumTraits<Scalar>::epsilon();
const Scalar & mab = mass() + Yb.mass();
const Scalar mab_inv = Scalar(1) / math::max(mab, eps);
const Vector3 & AB = (lever() - Yb.lever()).eval();
return InertiaTpl(
mab, (mass() * lever() + Yb.mass() * Yb.lever()) * mab_inv,
inertia() + Yb.inertia()
- (mass() * Yb.mass() * mab_inv) * typename Symmetric3::SkewSquare(AB));
}
InertiaTpl & __pequ__(const InertiaTpl & Yb)
{
static const Scalar eps = ::Eigen::NumTraits<Scalar>::epsilon();
const InertiaTpl & Ya = *this;
const Scalar & mab = mass() + Yb.mass();
const Scalar mab_inv = Scalar(1) / math::max(mab, eps);
const Vector3 & AB = (Ya.lever() - Yb.lever()).eval();
lever() *= (mass() * mab_inv);
lever() += (Yb.mass() * mab_inv) * Yb.lever(); // c *= mab_inv;
inertia() += Yb.inertia();
inertia() -= (Ya.mass() * Yb.mass() * mab_inv) * typename Symmetric3::SkewSquare(AB);
mass() = mab;
return *this;
}
InertiaTpl __minus__(const InertiaTpl & Yb) const
{
/* Y_{a} = ( m_{a+b}+m_b,
* (m_{a+b}*c_{a+b} - m_b*c_b ) / (m_a),
* I_{a+b} - I_b + (m_a*m_b)/(m_a+m_b) * AB_x * AB_x )
*/
const Scalar eps = ::Eigen::NumTraits<Scalar>::epsilon();
const Scalar ma = mass() - Yb.mass();
assert(check_expression_if_real<Scalar>(ma >= Scalar(0)));
const Scalar ma_inv = Scalar(1) / math::max(ma, eps);
const Vector3 c_a((mass() * lever() - Yb.mass() * Yb.lever()) * ma_inv);
const Vector3 AB = c_a - Yb.lever();
return InertiaTpl(
ma, c_a,
inertia() - Yb.inertia() + (ma * Yb.mass() / mass()) * typename Symmetric3::SkewSquare(AB));
}
InertiaTpl & __mequ__(const InertiaTpl & Yb)
{
static const Scalar eps = ::Eigen::NumTraits<Scalar>::epsilon();
const Scalar ma = mass() - Yb.mass();
assert(check_expression_if_real<Scalar>(ma >= Scalar(0)));
const Scalar ma_inv = Scalar(1) / math::max(ma, eps);
lever() *= (mass() * ma_inv);
lever().noalias() -= (Yb.mass() * ma_inv) * Yb.lever();
const Vector3 AB = lever() - Yb.lever();
inertia() -= Yb.inertia();
inertia() += (ma * Yb.mass() / mass()) * typename Symmetric3::SkewSquare(AB);
mass() = ma;
return *this;
}
template<typename MotionDerived>
ForceTpl<typename traits<MotionDerived>::Scalar, traits<MotionDerived>::Options>
__mult__(const MotionDense<MotionDerived> & v) const
{
typedef ForceTpl<typename traits<MotionDerived>::Scalar, traits<MotionDerived>::Options>
ReturnType;
ReturnType f;
__mult__(v, f);
return f;
}
template<typename MotionDerived, typename ForceDerived>
void __mult__(const MotionDense<MotionDerived> & v, ForceDense<ForceDerived> & f) const
{
f.linear().noalias() = mass() * (v.linear() - lever().cross(v.angular()));
Symmetric3::rhsMult(inertia(), v.angular(), f.angular());
f.angular() += lever().cross(f.linear());
// f.angular().noalias() = c.cross(f.linear()) + I*v.angular();
}
template<typename MotionDerived>
Scalar vtiv_impl(const MotionDense<MotionDerived> & v) const
{
const Vector3 cxw(lever().cross(v.angular()));
Scalar res = mass() * (v.linear().squaredNorm() - Scalar(2) * v.linear().dot(cxw));
const Vector3 mcxcxw(-mass() * lever().cross(cxw));
res += v.angular().dot(mcxcxw);
res += inertia().vtiv(v.angular());
return res;
}
template<typename MotionDerived>
Matrix6 variation(const MotionDense<MotionDerived> & v) const
{
Matrix6 res;
const Motion mv(v * mass());
res.template block<3, 3>(LINEAR, ANGULAR) =
-skew(mv.linear()) - skewSquare(mv.angular(), lever()) + skewSquare(lever(), mv.angular());
res.template block<3, 3>(ANGULAR, LINEAR) =
res.template block<3, 3>(LINEAR, ANGULAR).transpose();
// res.template block<3,3>(LINEAR,LINEAR) = mv.linear()*c.transpose(); // use as
// temporary variable res.template block<3,3>(ANGULAR,ANGULAR) = res.template
// block<3,3>(LINEAR,LINEAR) - res.template block<3,3>(LINEAR,LINEAR).transpose();
res.template block<3, 3>(ANGULAR, ANGULAR) =
-skewSquare(mv.linear(), lever()) - skewSquare(lever(), mv.linear());
res.template block<3, 3>(LINEAR, LINEAR) =
(inertia() - AlphaSkewSquare(mass(), lever())).matrix();
res.template block<3, 3>(ANGULAR, ANGULAR) -=
res.template block<3, 3>(LINEAR, LINEAR) * skew(v.angular());
res.template block<3, 3>(ANGULAR, ANGULAR) +=
cross(v.angular(), res.template block<3, 3>(LINEAR, LINEAR));
res.template block<3, 3>(LINEAR, LINEAR).setZero();
return res;
}
template<typename MotionDerived, typename M6>
static void vxi_impl(
const MotionDense<MotionDerived> & v,
const InertiaTpl & I,
const Eigen::MatrixBase<M6> & Iout)
{
EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(M6, 6, 6);
M6 & Iout_ = PINOCCHIO_EIGEN_CONST_CAST(M6, Iout);
// Block 1,1
alphaSkew(I.mass(), v.angular(), Iout_.template block<3, 3>(LINEAR, LINEAR));
// Iout_.template block<3,3>(LINEAR,LINEAR) = alphaSkew(I.mass(),v.angular());
const Vector3 mc(I.mass() * I.lever());
// Block 1,2
skewSquare(-v.angular(), mc, Iout_.template block<3, 3>(LINEAR, ANGULAR));
alphaSkew(I.mass(), v.linear(), Iout_.template block<3, 3>(ANGULAR, LINEAR));
Iout_.template block<3, 3>(ANGULAR, LINEAR) -= Iout_.template block<3, 3>(LINEAR, ANGULAR);
skewSquare(-v.linear(), mc, Iout_.template block<3, 3>(ANGULAR, ANGULAR));
// TODO: I do not why, but depending on the CPU, these three lines can give
// wrong output.
// typename Symmetric3::AlphaSkewSquare mcxcx(I.mass(),I.lever());
// const Symmetric3 I_mcxcx(I.inertia() - mcxcx);
// Iout_.template block<3,3>(ANGULAR,ANGULAR) += I_mcxcx.vxs(v.angular());
Symmetric3 mcxcx(typename Symmetric3::AlphaSkewSquare(I.mass(), I.lever()));
Iout_.template block<3, 3>(ANGULAR, ANGULAR) += I.inertia().vxs(v.angular());
Iout_.template block<3, 3>(ANGULAR, ANGULAR) -= mcxcx.vxs(v.angular());
}
template<typename MotionDerived, typename M6>
static void ivx_impl(
const MotionDense<MotionDerived> & v,
const InertiaTpl & I,
const Eigen::MatrixBase<M6> & Iout)
{
EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(M6, 6, 6);
M6 & Iout_ = PINOCCHIO_EIGEN_CONST_CAST(M6, Iout);
// Block 1,1
alphaSkew(I.mass(), v.angular(), Iout_.template block<3, 3>(LINEAR, LINEAR));
// Block 2,1
const Vector3 mc(I.mass() * I.lever());
skewSquare(mc, v.angular(), Iout_.template block<3, 3>(ANGULAR, LINEAR));
// Block 1,2
alphaSkew(I.mass(), v.linear(), Iout_.template block<3, 3>(LINEAR, ANGULAR));
// Block 2,2
cross(
-I.lever(), Iout_.template block<3, 3>(ANGULAR, LINEAR),
Iout_.template block<3, 3>(ANGULAR, ANGULAR));
Iout_.template block<3, 3>(ANGULAR, ANGULAR) += I.inertia().svx(v.angular());
for (int k = 0; k < 3; ++k)
Iout_.template block<3, 3>(ANGULAR, ANGULAR).col(k) +=
I.lever().cross(Iout_.template block<3, 3>(LINEAR, ANGULAR).col(k));
// Block 1,2
Iout_.template block<3, 3>(LINEAR, ANGULAR) -= Iout_.template block<3, 3>(ANGULAR, LINEAR);
}
// Getters
Scalar mass() const
{
return m_mass;
}
const Vector3 & lever() const
{
return m_com;
}
const Symmetric3 & inertia() const
{
return m_inertia;
}
Scalar & mass()
{
return m_mass;
}
Vector3 & lever()
{
return m_com;
}
Symmetric3 & inertia()
{
return m_inertia;
}
template<typename S2, int O2>
InertiaTpl se3Action_impl(const SE3Tpl<S2, O2> & M) const
{
/* The multiplication RIR' has a particular form that could be used, however it
* does not seems to be more efficient, see http://stackoverflow.m_comom/questions/
* 13215467/eigen-best-way-to-evaluate-asa-transpose-and-store-the-result-in-a-symmetric .*/
return InertiaTpl(
mass(), M.translation() + M.rotation() * lever(), inertia().rotate(M.rotation()));
}
template<typename S2, int O2>
InertiaTpl se3ActionInverse_impl(const SE3Tpl<S2, O2> & M) const
{
return InertiaTpl(
mass(), M.rotation().transpose() * (lever() - M.translation()),
inertia().rotate(M.rotation().transpose()));
}
template<typename MotionDerived>
Force vxiv(const MotionDense<MotionDerived> & v) const
{
const Vector3 & mcxw = mass() * lever().cross(v.angular());
const Vector3 & mv_mcxw = mass() * v.linear() - mcxw;
return Force(
v.angular().cross(mv_mcxw),
v.angular().cross(lever().cross(mv_mcxw) + inertia() * v.angular())
- v.linear().cross(mcxw));
}
void disp_impl(std::ostream & os) const
{
os << " m = " << mass() << "\n"
<< " c = " << lever().transpose() << "\n"
<< " I = \n"
<< inertia().matrix() << "";
}
template<typename NewScalar>
InertiaTpl<NewScalar, Options> cast() const
{
return InertiaTpl<NewScalar, Options>(
pinocchio::cast<NewScalar>(mass()), lever().template cast<NewScalar>(),
inertia().template cast<NewScalar>());
}
// TODO: adjust code
// /// \brief Check whether *this is a valid inertia, resulting from a positive mass
// distribution bool isValid() const
// {
// return
// (m_mass > Scalar(0) && m_inertia.isValid())
// || (m_mass == Scalar(0) && (m_inertia.data().array() == Scalar(0)).all());
// }
protected:
Scalar m_mass;
Vector3 m_com;
Symmetric3 m_inertia;
}; // class InertiaTpl
template<typename _Scalar, int _Options>
struct PseudoInertiaTpl
{
typedef _Scalar Scalar;
enum
{
Options = _Options,
};
typedef Eigen::Matrix<Scalar, 4, 4, Options> Matrix4;
typedef Eigen::Matrix<Scalar, 3, 1, Options> Vector3;
typedef Eigen::Matrix<Scalar, 3, 3, Options> Matrix3;
typedef Eigen::Matrix<Scalar, 10, 1, Options> Vector10;
typedef LogCholeskyParametersTpl<Scalar, Options> LogCholeskyParameters;
Scalar mass;
Vector3 h;
Matrix3 sigma;
PseudoInertiaTpl(Scalar mass, const Vector3 & h, const Matrix3 & sigma)
: mass(mass)
, h(h)
, sigma(sigma)
{
}
Matrix4 toMatrix() const
{
Matrix4 pseudo_inertia = Matrix4::Zero();
pseudo_inertia.template block<3, 3>(0, 0) = sigma;
pseudo_inertia.template block<3, 1>(0, 3) = h;
pseudo_inertia.template block<1, 3>(3, 0) = h.transpose();
pseudo_inertia(3, 3) = mass;
return pseudo_inertia;
}
static PseudoInertiaTpl FromMatrix(const Matrix4 & pseudo_inertia)
{
Scalar mass = pseudo_inertia(3, 3);
Vector3 h = pseudo_inertia.template block<3, 1>(0, 3);
Matrix3 sigma = pseudo_inertia.template block<3, 3>(0, 0);
return PseudoInertiaTpl(mass, h, sigma);
}
template<typename Vector10Like>
static PseudoInertiaTpl
FromDynamicParameters(const Eigen::MatrixBase<Vector10Like> & dynamic_params)
{
PINOCCHIO_ASSERT_MATRIX_SPECIFIC_SIZE(Vector10Like, dynamic_params, 10, 1);
Scalar mass = dynamic_params[0];
Vector3 h = dynamic_params.template segment<3>(1);
Matrix3 I_bar;
// clang-format off
I_bar << dynamic_params[4], dynamic_params[5], dynamic_params[7],
dynamic_params[5], dynamic_params[6], dynamic_params[8],
dynamic_params[7], dynamic_params[8], dynamic_params[9];
// clang-format on
Matrix3 sigma = 0.5 * I_bar.trace() * Matrix3::Identity() - I_bar;
return PseudoInertiaTpl(mass, h, sigma);
}
Vector10 toDynamicParameters() const
{
Matrix3 I_bar = sigma.trace() * Matrix3::Identity() - sigma;
Vector10 dynamic_params;
dynamic_params[0] = mass;
dynamic_params.template segment<3>(1) = h;
dynamic_params[4] = I_bar(0, 0);
dynamic_params[5] = I_bar(0, 1);
dynamic_params[6] = I_bar(1, 1);
dynamic_params[7] = I_bar(0, 2);
dynamic_params[8] = I_bar(1, 2);
dynamic_params[9] = I_bar(2, 2);
return dynamic_params;
}
static PseudoInertiaTpl FromInertia(const InertiaTpl<Scalar, Options> & inertia)
{
Vector10 dynamic_params = inertia.toDynamicParameters();
return FromDynamicParameters(dynamic_params);
}
InertiaTpl<Scalar, Options> toInertia() const
{
Vector10 dynamic_params = toDynamicParameters();
return InertiaTpl<Scalar, Options>::FromDynamicParameters(dynamic_params);
}
static PseudoInertiaTpl FromLogCholeskyParameters(const LogCholeskyParameters & log_cholesky)
{
Vector10 dynamic_params = log_cholesky.toDynamicParameters();
return FromDynamicParameters(dynamic_params);
}
template<typename NewScalar>
PseudoInertiaTpl<NewScalar, Options> cast() const
{
return PseudoInertiaTpl<NewScalar, Options>(
pinocchio::cast<NewScalar>(mass), h.template cast<NewScalar>(),
sigma.template cast<NewScalar>());
}
void disp_impl(std::ostream & os) const
{
os << " m = " << mass << "\n"
<< " h = " << h.transpose() << "\n"
<< " sigma = \n"
<< sigma << "";
}
friend std::ostream & operator<<(std::ostream & os, const PseudoInertiaTpl & pi)
{
pi.disp_impl(os);
return os;
}
};
template<typename _Scalar, int _Options>
struct LogCholeskyParametersTpl
{
typedef _Scalar Scalar;
enum
{
Options = _Options,
};
typedef Eigen::Matrix<Scalar, 10, 1, Options> Vector10;
typedef Eigen::Matrix<Scalar, 10, 10, Options> Matrix10;
typedef PseudoInertiaTpl<Scalar, Options> PseudoInertia;
Vector10 parameters;
explicit LogCholeskyParametersTpl(const Vector10 & log_cholesky)
: parameters(log_cholesky)
{
}
Vector10 toDynamicParameters() const
{
using math::exp;
using math::pow;
// clang-format off
Vector10 dynamic_params;
const Scalar alpha = parameters[0];
const Scalar d1 = parameters[1];
const Scalar d2 = parameters[2];
const Scalar d3 = parameters[3];
const Scalar s12 = parameters[4];
const Scalar s23 = parameters[5];
const Scalar s13 = parameters[6];
const Scalar t1 = parameters[7];
const Scalar t2 = parameters[8];
const Scalar t3 = parameters[9];
const Scalar exp_d1 = exp(d1);
const Scalar exp_d2 = exp(d2);
const Scalar exp_d3 = exp(d3);
dynamic_params[0] = 1;
dynamic_params[1] = t1;
dynamic_params[2] = t2;
dynamic_params[3] = t3;
dynamic_params[4] = pow(s23, 2) + pow(t2, 2) + pow(t3, 2) + pow(exp_d2, 2) + pow(exp_d3, 2);
dynamic_params[5] = -s12 * exp_d2 - s13 * s23 - t1 * t2;
dynamic_params[6] = pow(s12, 2) + pow(s13, 2) + pow(t1, 2) + pow(t3, 2) + pow(exp_d1, 2) + pow(exp_d3, 2);
dynamic_params[7] = -s13 * exp_d3 - t1 * t3;
dynamic_params[8] = -s23 * exp_d3 - t2 * t3;
dynamic_params[9] = pow(s12, 2) + pow(s13, 2) + pow(s23, 2) + pow(t1, 2) + pow(t2, 2) + pow(exp_d1, 2) + pow(exp_d2, 2);
const Scalar exp_2_alpha = exp(2 * alpha);
dynamic_params *= exp_2_alpha;
// clang-format on
return dynamic_params;
}
PseudoInertia toPseudoInertia() const
{
Vector10 dynamic_params = toDynamicParameters();
return PseudoInertia::FromDynamicParameters(dynamic_params);
}
InertiaTpl<Scalar, Options> toInertia() const
{
Vector10 dynamic_params = toDynamicParameters();
return InertiaTpl<Scalar, Options>::FromDynamicParameters(dynamic_params);
}
Matrix10 calculateJacobian() const
{
using math::exp;
using math::pow;
Matrix10 jacobian = Matrix10::Zero();
const Scalar alpha = parameters[0];
const Scalar d1 = parameters[1];
const Scalar d2 = parameters[2];
const Scalar d3 = parameters[3];
const Scalar s12 = parameters[4];
const Scalar s23 = parameters[5];
const Scalar s13 = parameters[6];
const Scalar t1 = parameters[7];
const Scalar t2 = parameters[8];
const Scalar t3 = parameters[9];
const Scalar exp_2alpha = exp(2 * alpha);
const Scalar exp_2d1 = exp(2 * d1);
const Scalar exp_2d2 = exp(2 * d2);
const Scalar exp_2d3 = exp(2 * d3);
const Scalar exp_d1 = exp(d1);
const Scalar exp_d2 = exp(d2);
const Scalar exp_d3 = exp(d3);
// clang-format off
jacobian(0, 0) = 2 * exp_2alpha;
jacobian(1, 0) = 2 * t1 * exp_2alpha;
jacobian(1, 7) = exp_2alpha;
jacobian(2, 0) = 2 * t2 * exp_2alpha;
jacobian(2, 8) = exp_2alpha;
jacobian(3, 0) = 2 * t3 * exp_2alpha;
jacobian(3, 9) = exp_2alpha;
jacobian(4, 0) = 2 * (pow(s23, 2) + pow(t2, 2) + pow(t3, 2) + exp_2d2 + exp_2d3) * exp_2alpha;
jacobian(4, 2) = 2 * exp_2alpha * exp_2d2;
jacobian(4, 3) = 2 * exp_2alpha * exp_2d3;
jacobian(4, 5) = 2 * s23 * exp_2alpha;
jacobian(4, 8) = 2 * t2 * exp_2alpha;
jacobian(4, 9) = 2 * t3 * exp_2alpha;
jacobian(5, 0) = -2 * (s12 * exp_d2 + s13 * s23 + t1 * t2) * exp_2alpha;
jacobian(5, 2) = -s12 * exp_2alpha * exp_d2;
jacobian(5, 4) = -exp_2alpha * exp_d2;
jacobian(5, 5) = -s13 * exp_2alpha;
jacobian(5, 6) = -s23 * exp_2alpha;
jacobian(5, 7) = -t2 * exp_2alpha;
jacobian(5, 8) = -t1 * exp_2alpha;
jacobian(6, 0) = 2 * (pow(s12, 2) + pow(s13, 2) + pow(t1, 2) + pow(t3, 2) + exp_2d1 + exp_2d3) * exp_2alpha;
jacobian(6, 1) = 2 * exp_2alpha * exp_2d1;
jacobian(6, 3) = 2 * exp_2alpha * exp_2d3;
jacobian(6, 4) = 2 * s12 * exp_2alpha;
jacobian(6, 6) = 2 * s13 * exp_2alpha;
jacobian(6, 7) = 2 * t1 * exp_2alpha;
jacobian(6, 9) = 2 * t3 * exp_2alpha;
jacobian(7, 0) = -2 * (s13 * exp_d3 + t1 * t3) * exp_2alpha;
jacobian(7, 3) = -s13 * exp_2alpha * exp_d3;
jacobian(7, 6) = -exp_2alpha * exp_d3;
jacobian(7, 7) = -t3 * exp_2alpha;
jacobian(7, 9) = -t1 * exp_2alpha;
jacobian(8, 0) = -2 * (s23 * exp_d3 + t2 * t3) * exp_2alpha;
jacobian(8, 3) = -s23 * exp_2alpha * exp_d3;
jacobian(8, 5) = -exp_2alpha * exp_d3;
jacobian(8, 8) = -t3 * exp_2alpha;
jacobian(8, 9) = -t2 * exp_2alpha;
jacobian(9, 0) = 2 * (pow(s12, 2) + pow(s13, 2) + pow(s23, 2) + pow(t1, 2) + pow(t2, 2) + exp_2d1 + exp_2d2) * exp_2alpha;
jacobian(9, 1) = 2 * exp_2alpha * exp_2d1;
jacobian(9, 2) = 2 * exp_2alpha * exp_2d2;
jacobian(9, 4) = 2 * s12 * exp_2alpha;
jacobian(9, 5) = 2 * s23 * exp_2alpha;
jacobian(9, 6) = 2 * s13 * exp_2alpha;
jacobian(9, 7) = 2 * t1 * exp_2alpha;
jacobian(9, 8) = 2 * t2 * exp_2alpha;
// clang-format on
return jacobian;
}
template<typename NewScalar>
LogCholeskyParametersTpl<NewScalar, Options> cast() const
{
return LogCholeskyParametersTpl<NewScalar, Options>(parameters.template cast<NewScalar>());
}
void disp_impl(std::ostream & os) const
{
os << " alpha: " << parameters[0] << "\n"
<< " d1: " << parameters[1] << "\n"
<< " d2: " << parameters[2] << "\n"
<< " d3: " << parameters[3] << "\n"
<< " s12: " << parameters[4] << "\n"
<< " s23: " << parameters[5] << "\n"
<< " s13: " << parameters[6] << "\n"
<< " t1: " << parameters[7] << "\n"
<< " t2: " << parameters[8] << "\n"
<< " t3: " << parameters[9] << "";
}
friend std::ostream & operator<<(std::ostream & os, const LogCholeskyParametersTpl & lcp)
{
lcp.disp_impl(os);
return os;
}
};
} // namespace pinocchio
#endif // ifndef __pinocchio_spatial_inertia_hpp__