iDynTree::SpatialInertia class
#include <iDynTree/SpatialInertia.h>

Class representing a six dimensional inertia.

Public static functions

static auto combine(const SpatialInertia& op1, const SpatialInertia& op2) -> SpatialInertia
static auto Zero() -> SpatialInertia
static auto momentumRegressor(const iDynTree::Twist& v) -> Matrix6x10
Get the momentum inertial parameters regressor.
static auto momentumDerivativeRegressor(const iDynTree::Twist& v, const iDynTree::SpatialAcc& a) -> Matrix6x10
Get the momentum derivative inertial parameters regressor.
static auto momentumDerivativeSlotineLiRegressor(const iDynTree::Twist& v, const iDynTree::Twist& vRef, const iDynTree::SpatialAcc& aRef) -> Matrix6x10
Get the momentum derivative inertial parameters regressor.

Constructors, destructors, conversion operators

SpatialInertia()
Default constructor.
SpatialInertia(const double mass, const Position& com, const RotationalInertia& rotInertia)
SpatialInertia(const SpatialInertia& other)

Public functions

void fromRotationalInertiaWrtCenterOfMass(const double mass, const Position& com, const RotationalInertia& rotInertia)
Helper constructor-like function that takes mass, center of mass and the rotational inertia expressed in the center of mass.
auto getMass() const -> double
multiplication operator
auto getCenterOfMass() const -> Position
auto getRotationalInertiaWrtFrameOrigin() const -> const RotationalInertia&
auto getRotationalInertiaWrtCenterOfMass() const -> RotationalInertia
auto multiply(const SpatialMotionVector& op) const -> SpatialForceVector
Multiplication function.
void zero()
reset to zero (i.e.
auto asMatrix() const -> Matrix6x6
Get the SpatialInertia as a 6x6 matrix.
auto applyInverse(const SpatialMomentum& mom) const -> Twist
auto getInverse() const -> Matrix6x6
auto operator+(const SpatialInertia& other) const -> SpatialInertia
auto operator*(const SpatialMotionVector& other) const -> SpatialForceVector
auto operator*(const Twist& other) const -> SpatialMomentum
auto operator*(const SpatialAcc& other) const -> Wrench
auto biasWrench(const Twist& V) const -> Wrench
Return the bias wrench v.cross(M*v).
auto biasWrenchDerivative(const Twist& V) const -> Matrix6x6
Return the derivative of the bias wrench with respect to the link 6D velocity.
auto asVector() const -> Vector10
Get the Rigid Body Inertia as a vector of 10 inertial parameters.
void fromVector(const Vector10& inertialParams)
Set the Rigid Body Inertia from the inertial parameters in the vector.
auto isPhysicallyConsistent() const -> bool
Check if the Rigid Body Inertia is physically consistent.

Protected variables

double m_mass
Mass.
double m_mcom
First moment of mass (i.e. mass * center of mass).
RotationalInertia m_rotInertia
Three dimensional rotational inertia.

Function documentation

static Matrix6x10 iDynTree::SpatialInertia::momentumRegressor(const iDynTree::Twist& v)

Get the momentum inertial parameters regressor.

Get the matrix

\[ Y(\mathrm{v}) \in \mathbb{R}^{6 \times 10} \]

such that:

\[ \mathbb{M} \mathrm{v} = Y(\mathrm{v}) \alpha \]

If $ \alpha \in \mathbb{R}^{10} $ is the inertial parameters representation of $ \mathbb{M} $ , as returned by the asVector method.

static Matrix6x10 iDynTree::SpatialInertia::momentumDerivativeRegressor(const iDynTree::Twist& v, const iDynTree::SpatialAcc& a)

Get the momentum derivative inertial parameters regressor.

Get the matrix

\[ Y(\mathrm{v},a) \in \mathbb{R}^{6 \times 10} \]

such that:

\[ \mathbb{M} a + \mathrm{v} \overline{\times}^{*} \mathbb{M} \mathrm{v} = Y(\mathrm{v}, a)\alpha \]

If $ \alpha \in \mathbb{R}^{10} $ is the inertial parameters representation of $ \mathbb{M} $ , as returned by the asVector method.

This is also the regressor of the net wrench acting on a rigid body. As such, it is the building block of all other algorithms to compute dynamics regressors.

static Matrix6x10 iDynTree::SpatialInertia::momentumDerivativeSlotineLiRegressor(const iDynTree::Twist& v, const iDynTree::Twist& vRef, const iDynTree::SpatialAcc& aRef)

Get the momentum derivative inertial parameters regressor.

Get the matrix

\[ Y(\mathrm{v},\mathrm{v}_r,a_r) \in \mathbb{R}^{6\times10} \]

such that:

\[ \mathbb{M} a_r + (\mathrm{v} \overline{\times}^{*} \mathbb{M} - \mathbb{M} \mathrm{v} \times) \mathrm{v}_r = Y(\mathrm{v},\mathrm{v}_r,a_r)\alpha \]

If $ \alpha \in \mathbb{R}^{10} $ is the inertial parameters representation of $ \mathbb{M} $ , as returned by the asVector method.

Notice that if $ \mathrm{v} = \mathrm{v}_r $ , this regressor reduces to the one computed by momentumDerivativeRegressor. The main difference is that (assuming constant $ \mathbb{M} $ ) this regressor respect the passivity condition and thus is the basic building block for building Slotine Li style regressors.

For more on this, please check:

Garofalo, G.; Ott, C.; Albu-Schaffer, A., "On the closed form computation of the dynamic matrices and their differentiations," in Intelligent Robots and Systems (IROS), 2013 IEEE/RSJ International Conference on doi: 10.1109/IROS.2013.6696688 URL: http://ieeexplore.ieee.org/stamp/stamp.jsp?tp=&arnumber=6696688&isnumber=6696319

iDynTree::SpatialInertia::SpatialInertia()

Default constructor.

The data is not reset to zero for perfomance reason. Please initialize the data in the vector before any use.

double iDynTree::SpatialInertia::getMass() const

multiplication operator

overloading happens on proper classes Getter functions

void iDynTree::SpatialInertia::zero()

reset to zero (i.e.

the inertia of body with zero mass) the SpatialInertia

Matrix6x6 iDynTree::SpatialInertia::asMatrix() const

Get the SpatialInertia as a 6x6 matrix.

If $ m \in \mathbb{R} $ is the mass, $ c \in \mathbb{R}^3 $ is the center of mass, $ I \in \mathbb{R}^{3 \times 3} $ is the 3d inertia, and $ 1_3 \in \mathbb{R}^{3 \times 3} $ is the 3d identity matrix this method returns the $ \mathbb{M} \in \mathbb{R}^{6 \times 6} $ matrix such that:

\[ \mathbb{M} = \begin{bmatrix} m 1_3 & -m c \times \\ m c \times & I \end{bmatrix} \]

.

Wrench iDynTree::SpatialInertia::biasWrench(const Twist& V) const

Return the bias wrench v.cross(M*v).

Defining $ \mathbb{M} $ as this inertia, return the bias wrench v.cross(M*v), defined in math as:

\[ \mathrm{v} \bar\times^* \mathbb{M} \mathrm{v} = \\ \begin{bmatrix} \omega \times & 0 \\ v \times & \omega \times \end{bmatrix} \begin{bmatrix} m 1_3 & -mc\times \\ mc\times & I \end{bmatrix} \begin{bmatrix} v \\ \omega \end{bmatrix} = \\ \begin{bmatrix} m \omega \times v - \omega \times ( m c \times \omega) \\ m c \times ( \omega \times v ) + \omega \times I \omega \end{bmatrix} \]

Matrix6x6 iDynTree::SpatialInertia::biasWrenchDerivative(const Twist& V) const

Return the derivative of the bias wrench with respect to the link 6D velocity.

Defining $ \mathbb{M} \in \mathbb{R}^{6 \times 6} $ as this inertia, return the derivative with respect to $ \mathrm{v} = \begin{bmatrix} v \\ \omega \end{bmatrix} \in \mathbb{R}^6 $ of the bias wrench $ \mathrm{v} \bar\times^* \mathbb{M} \mathrm{v} $ (i.e. v.cross(M*v)).

The bias wrench is:

\[ \mathrm{v} \bar\times^* \mathbb{M} \mathrm{v} = \\ \begin{bmatrix} m \omega \times v - \omega \times ( m c \times \omega) \\ m c \times ( \omega \times v ) + \omega \times I \omega \end{bmatrix} \]

So the derivative with respect to the twist V is :

\[ \partial_\mathrm{v} ( \mathrm{v} \bar\times^* \mathbb{M} \mathrm{v} ) = \\ \begin{bmatrix} m \omega \times & - m v \times + ( m c \times \omega) \times - (\omega \times) (mc \times) \\ (m c \times) ( \omega \times) & - (m c \times )(v \times) + \omega \times I - (I \omega) \times \end{bmatrix} \]

Vector10 iDynTree::SpatialInertia::asVector() const

Get the Rigid Body Inertia as a vector of 10 inertial parameters.

Return the rigid body inertia inertial parameters, defined as:

ElementsSymbolDescription
0 $ m $ The mass of the rigid body
1-3 $ m c $ The first moment of mass of the rigid body
4-9 $ \mathop{vech}(I) $ The 6 independent elements of the 3d inertia matrix, i.e. $ \begin{bmatrix} I_{xx} \\ I_{xy} \\ I_{xz} \\ I_{yy} \\ I_{yz} \\ I_{zz} \end{bmatrix} $ .

The first moment of mass is the center of mass ( $ c \in \mathbb{R}^3 $ ) w.r.t. to the frame where this rigid body inertia is expressed multiplied by the rigid body mass $ m $ .

The 3d rigid body inertia $ I \in \mathbb{R}^{3 \times 3} $ is expressed with the orientation of the frame in which this rigid body inertia is expressed, and with respect to the frame origin.

void iDynTree::SpatialInertia::fromVector(const Vector10& inertialParams)

Set the Rigid Body Inertia from the inertial parameters in the vector.

The serialization assumed in the inertialParams is the same used in the asVector method.

bool iDynTree::SpatialInertia::isPhysicallyConsistent() const

Check if the Rigid Body Inertia is physically consistent.

This method will check:

  • if the mass is positive,
  • if the 3d inertia at the COM is positive semidefinite, (semidefinite to cover also the case of the inertia of a point mass),
  • if the moment of inertia along the principal axes at the COM respect the triangle inequality.

It will return true if all this check will pass, or false otherwise.