Drake

This class represents the physical concept of a Spatial Inertia.
A spatial inertia (or spatial mass matrix) encapsulates the mass, center of mass, and rotational inertia of the mass distribution of a body or composite body S, where with "composite body" we mean a collection of bodies welded together containing at least one body (throughout this documentation "body" is many times used instead of "composite body" but the same concepts apply to a collection of bodies as well.) A spatial inertia is an element of ℝ⁶ˣ⁶ that is symmetric, and positive semidefinite. It logically consists of 3x3
submatrices arranged like so, [Jain 2010]:
Spatial mass matrix   0    1  I_SP  m p_PScm×  2      3    4  m p_PScm×  m Id  5      Symbol: M
where, with the monogram notation described in Spatial Mass Matrix (Spatial Inertia), I_SP
is the rotational inertia of body or composite body S computed about a point P, m is the mass of this composite body, p_PScm
is the position vector from point P to the center of mass Scm
of the composite body S with p_PScm×
denoting its skewsymmetric cross product matrix (defined such that a× b = a.cross(b)
), and Id
is the identity matrix in ℝ³ˣ³. See Section 2.1, p. 17 of [Jain 2010]. The logical arrangement as shown above is chosen to be consistent with our logical arrangement for spatial vectors as documented in Spatial Algebra for which the rotational component comes first followed by the translational component.
In typeset material we use the symbol \( [M^{S/P}]_E \) to represent the spatial inertia of a body or composite body S about point P, expressed in frame E. For this inertia, the monogram notation reads M_SP_E
. If the point P is fixed to a body B, we write that point as \( B_P \) which appears in code and comments as Bp
. So if the body or composite body is B and the about point is Bp
, the monogram notation reads M_BBp_E
, which can be abbreviated to M_Bp_E
since the about point Bp
also identifies the body. Common cases are that the about point is the origin Bo
of the body, or it's the center of mass Bcm
for which the rotational inertia in monogram notation would read as I_Bo_E
and I_Bcm_E
, respectively. Given M_BP_E
( \([M^{B/P}]_E\)), the rotational inertia of this spatial inertia is I_BP_E
( \([I^{B/P}]_E\)) and the position vector of the center of mass measured from point P and expressed in E is p_PBcm_E
( \([^Pp^{B_{cm}}]_E\)).
true
. For instance, validity checks are not performed when T is symbolic::Expression.T  The scalar type, which must be one of the default scalars. 
#include <drake/multibody/tree/spatial_inertia.h>
Public Member Functions  
SpatialInertia ()  
Default SpatialInertia constructor initializes mass, center of mass and rotational inertia to invalid NaN's for a quick detection of uninitialized values. More...  
SpatialInertia (const T &mass, const Vector3< T > &p_PScm_E, const UnitInertia< T > &G_SP_E, const bool skip_validity_check=false)  
Constructs a spatial inertia for a physical body or composite body S about a point P from a given mass, center of mass and rotational inertia. More...  
template<typename Scalar >  
SpatialInertia< Scalar >  cast () const 
Returns a new SpatialInertia object templated on Scalar initialized from the value of this spatial inertia. More...  
const T &  get_mass () const 
Get a constant reference to the mass of this spatial inertia. More...  
const Vector3< T > &  get_com () const 
Get a constant reference to the position vector p_PScm_E from the about point P to the center of mass Scm of the body or composite body S, expressed in frame E. More...  
Vector3< T >  CalcComMoment () const 
Computes the center of mass moment vector mass * p_PScm_E given the position vector p_PScm_E from the about point P to the center of mass Scm of the body or composite body S, expressed in frame E. More...  
const UnitInertia< T > &  get_unit_inertia () const 
Get a constant reference to the unit inertia G_SP_E of this spatial inertia, computed about point P and expressed in frame E. More...  
RotationalInertia< T >  CalcRotationalInertia () const 
Computes the rotational inertia I_SP_E = mass * G_SP_E of this spatial inertia, computed about point P and expressed in frame E. More...  
boolean< T >  IsNaN () const 
Returns true if any of the elements in this spatial inertia is NaN and false otherwise. More...  
boolean< T >  IsPhysicallyValid () const 
Performs a number of checks to verify that this is a physically valid spatial inertia. More...  
Matrix6< T >  CopyToFullMatrix6 () const 
Copy to a full 6x6 matrix representation. More...  
void  SetNaN () 
Sets this spatial inertia to have NaN entries. More...  
SpatialInertia &  operator+= (const SpatialInertia< T > &M_BP_E) 
Adds in a spatial inertia to this spatial inertia. More...  
SpatialInertia &  ReExpressInPlace (const math::RotationMatrix< T > &R_AE) 
Given this spatial inertia M_SP_E for some body or composite body S, taken about a point P and expressed in frame E, this method computes the same inertia reexpressed in another frame A. More...  
SpatialInertia  ReExpress (const math::RotationMatrix< T > &R_AE) const 
Given this spatial inertia M_SP_E for some body or composite body S, taken about a point P and expressed in frame E, this method computes the same inertia reexpressed in another frame A. More...  
SpatialInertia &  ShiftInPlace (const Vector3< T > &p_PQ_E) 
Given this spatial inertia M_SP_E for some body or composite body S, computed about point P, and expressed in frame E, this method uses the Parallel Axis Theorem for spatial inertias to compute the same spatial inertia about a new point Q. More...  
SpatialInertia  Shift (const Vector3< T > &p_PQ_E) const 
Given this spatial inertia M_SP_E for some body or composite body S, computed about point P, and expressed in frame E, this method uses the Parallel Axis Theorem for spatial inertias to compute the same spatial inertia about a new point Q. More...  
SpatialForce< T >  operator * (const SpatialAcceleration< T > &A_WB_E) const 
Multiplies this spatial inertia M_Bo_E of a body B about its frame origin Bo by the spatial acceleration of the body frame B in a frame W. More...  
SpatialMomentum< T >  operator * (const SpatialVelocity< T > &V_WBp_E) const 
Multiplies this spatial inertia M_BP_E of a body B about a point P by the spatial velocity V_WBp , in a frame W, of the body frame B shifted to point P. More...  
template<typename Derived >  
Eigen::Matrix< T, 6, Derived::ColsAtCompileTime, 0, 6, Derived::MaxColsAtCompileTime >  operator * (const Eigen::MatrixBase< Derived > &Mmatrix) const 
Multiplies this spatial inertia by a set of spatial vectors in M⁶ stored as columns of input matrix Mmatrix . More...  
Implements CopyConstructible, CopyAssignable, MoveConstructible, MoveAssignable  
SpatialInertia (const SpatialInertia &)=default  
SpatialInertia &  operator= (const SpatialInertia &)=default 
SpatialInertia (SpatialInertia &&)=default  
SpatialInertia &  operator= (SpatialInertia &&)=default 
Static Public Member Functions  
static SpatialInertia  MakeFromCentralInertia (const T &mass, const Vector3< T > &p_PScm_E, const RotationalInertia< T > &I_SScm_E) 
Creates a spatial inertia for a physical body or composite body S about a point P from a given mass, center of mass, and central rotational inertia. More...  
Related Functions  
(Note that these are not member functions.)  
template<typename T >  
std::ostream &  operator<< (std::ostream &o, const SpatialInertia< T > &M) 
Insertion operator to write SpatialInertia objects into a std::ostream . More...  

default 

default 
SpatialInertia  (  ) 
Default SpatialInertia constructor initializes mass, center of mass and rotational inertia to invalid NaN's for a quick detection of uninitialized values.
SpatialInertia  (  const T &  mass, 
const Vector3< T > &  p_PScm_E,  
const UnitInertia< T > &  G_SP_E,  
const bool  skip_validity_check = false 

) 
Constructs a spatial inertia for a physical body or composite body S about a point P from a given mass, center of mass and rotational inertia.
The center of mass is specified by the position vector p_PScm_E
from point P to the center of mass point Scm
, expressed in a frame E. The rotational inertia is provided as the UnitInertia G_SP_E
of the body or composite body S computed about point P and expressed in frame E.
This constructor checks for the physical validity of the resulting SpatialInertia with IsPhysicallyValid() and throws a std::runtime_error exception in the event the provided input parameters lead to nonphysically viable spatial inertia. Since this check has nonnegligable runtime costs, it can be disabled by setting the optional argument skip_validity_check
to true
.
[in]  mass  The mass of the body or composite body S. 
[in]  p_PScm_E  The position vector from point P to the center of mass of body or composite body S expressed in frame E. 
[in]  G_SP_E  UnitInertia of the body or composite body S computed about origin point P and expressed in frame E. 
[in]  skip_validity_check  If true, skips the validity check described above. Defaults to false. 
Vector3<T> CalcComMoment  (  )  const 
Computes the center of mass moment vector mass * p_PScm_E
given the position vector p_PScm_E
from the about point P to the center of mass Scm
of the body or composite body S, expressed in frame E.
See the documentation of this class for details.
RotationalInertia<T> CalcRotationalInertia  (  )  const 
Computes the rotational inertia I_SP_E = mass * G_SP_E
of this spatial inertia, computed about point P and expressed in frame E.
See the documentation of this class for details.
SpatialInertia<Scalar> cast  (  )  const 
Returns a new SpatialInertia object templated on Scalar
initialized from the value of this
spatial inertia.
Scalar  The scalar type on which the new spatial inertia will be templated. 
SpatialInertia<From>::cast<To>()
creates a new SpatialInertia<To>
from a SpatialInertia<From>
but only if type To
is constructible from type From
. This cast method works in accordance with Eigen's cast method for Eigen's objects that underlie this SpatialInertia. For example, Eigen currently allows cast from type double to AutoDiffXd, but not viceversa. Matrix6<T> CopyToFullMatrix6  (  )  const 
Copy to a full 6x6 matrix representation.
const Vector3<T>& get_com  (  )  const 
Get a constant reference to the position vector p_PScm_E
from the about point P to the center of mass Scm
of the body or composite body S, expressed in frame E.
See the documentation of this class for details.
const T& get_mass  (  )  const 
Get a constant reference to the mass of this spatial inertia.
const UnitInertia<T>& get_unit_inertia  (  )  const 
Get a constant reference to the unit inertia G_SP_E
of this spatial inertia, computed about point P and expressed in frame E.
See the documentation of this class for details.
boolean<T> IsNaN  (  )  const 
Returns true
if any of the elements in this spatial inertia is NaN and false
otherwise.
boolean<T> IsPhysicallyValid  (  )  const 
Performs a number of checks to verify that this is a physically valid spatial inertia.
The checks performed are:
Ixx + Iyy >= Izz
Ixx + Izz >= Iyy
Iyy + Izz >= Ixx
These are the tests performed by RotationalInertia::CouldBePhysicallyValid() which become a sufficient condition when performed on a rotational inertia about a body's center of mass.

static 
Creates a spatial inertia for a physical body or composite body S about a point P from a given mass, center of mass, and central rotational inertia.
For example, this method creates a body's SpatialInertia about its body origin Bo from the body's mass, position vector from Bo to the body's center of mass, and rotational inertia about the body's center of mass.
This method checks for the physical validity of the resulting SpatialInertia with IsPhysicallyValid() and throws a std::runtime_error exception in the event the provided input parameters lead to a nonphysically viable spatial inertia.
[in]  mass  The mass of the body or composite body S. 
[in]  p_PScm_E  The position vector from point P to point Scm (S's center of mass), expressed in a frame E. 
[in]  I_SScm_E  S's RotationalInertia about Scm, expressed in frame E. 
M_SP_E  S's spatial inertia about point P, expressed in frame E. 
SpatialForce<T> operator *  (  const SpatialAcceleration< T > &  A_WB_E  )  const 
Multiplies this
spatial inertia M_Bo_E
of a body B about its frame origin Bo
by the spatial acceleration of the body frame B in a frame W.
Mathematically:
F_Bo_E = M_Bo_E * A_WB_E
or, in terms of its rotational and translational components (see this class's documentation for the block form of a rotational inertia):
t_Bo = I_Bo * alpha_WB + m * p_BoBcm x a_WBo f_Bo = m * p_BoBcm x alpha_WB + m * a_WBo
where alpha_WB
and a_WBo
are the rotational and translational components of the spatial acceleration A_WB
, respectively.
F_Bo_E
computed by this operator appears in the equations of motion for a rigid body which, when written about the origin Bo
of the body frame B (which does not necessarily need to coincide with the body's center of mass), read as: Ftot_BBo = M_Bo_W * A_WB + b_Bowhere
Ftot_BBo
is the total spatial force applied on body B at Bo
that corresponds to the body spatial acceleration A_WB
and b_Bo
contains the velocity dependent gyroscopic terms (see Eq. 2.26, p. 27, in A. Jain's book). SpatialMomentum<T> operator *  (  const SpatialVelocity< T > &  V_WBp_E  )  const 
Multiplies this
spatial inertia M_BP_E
of a body B about a point P by the spatial velocity V_WBp
, in a frame W, of the body frame B shifted to point P.
Mathematically:
L_WBp_E = M_BP_E * V_WBp_E
or, in terms of its rotational and translational components (see this class's documentation for the block form of a rotational inertia):
h_WB = I_Bp * w_WB + m * p_BoBcm x v_WP l_WBp = m * p_BoBcm x w_WB + m * v_WP
where w_WB
and v_WP
are the rotational and translational components of the spatial velocity V_WBp
, respectively and, h_WB
and l_WBp
are the angular and linear components of the spatial momentum L_WBp
, respectively.
M_BP_E.Shift(p_PQ_E) * V_WBp_E.Shift(p_PQ_E)
exactly equals L_WBp_E.Shift(p_PQ_E)
. Eigen::Matrix<T, 6, Derived::ColsAtCompileTime, 0, 6, Derived::MaxColsAtCompileTime> operator *  (  const Eigen::MatrixBase< Derived > &  Mmatrix  )  const 
Multiplies this
spatial inertia by a set of spatial vectors in M⁶ stored as columns of input matrix Mmatrix
.
The top three rows of Mmatrix are expected to store the rotational components while the bottom three rows are expected to store the translational components. The output matrix is of the same size as Mmatrix
and each jth column stores the spatial vector in F⁶ result of multiplying this
spatial inertia with the jth column of Mmatrix
.
SpatialInertia& operator+=  (  const SpatialInertia< T > &  M_BP_E  ) 
Adds in a spatial inertia to this
spatial inertia.
[in]  M_BP_E  A spatial inertia of some body B to be added to this spatial inertia. It must be defined about the same point P as this inertia, and expressed in the same frame E. 
this
spatial inertia, which has been updated to include the given spatial inertia M_BP_E
.this
spatial inertia to be M_SP_E
for some body or composite body S, about some point P, the supplied spatial inertia M_BP_E
must be for some other body or composite body B about the same point P; B's inertia is then included in S.

default 

default 
SpatialInertia ReExpress  (  const math::RotationMatrix< T > &  R_AE  )  const 
Given this
spatial inertia M_SP_E
for some body or composite body S, taken about a point P and expressed in frame E, this method computes the same inertia reexpressed in another frame A.
[in]  R_AE  RotationMatrix relating frames A and E. 
M_SP_A  The same spatial inertia of S about P but now reexpressed in frame A. 
SpatialInertia& ReExpressInPlace  (  const math::RotationMatrix< T > &  R_AE  ) 
Given this
spatial inertia M_SP_E
for some body or composite body S, taken about a point P and expressed in frame E, this method computes the same inertia reexpressed in another frame A.
This operation is performed inplace modifying the original object.
[in]  R_AE  Rotation matrix from frame E to frame A. 
this
rotational inertia about the same point P but now reexpressed in frame A, that is, M_SP_A
. void SetNaN  (  ) 
Sets this
spatial inertia to have NaN entries.
Typically used for quick detection of uninitialized values.
SpatialInertia Shift  (  const Vector3< T > &  p_PQ_E  )  const 
Given this
spatial inertia M_SP_E
for some body or composite body S, computed about point P, and expressed in frame E, this method uses the Parallel Axis Theorem for spatial inertias to compute the same spatial inertia about a new point Q.
The result still is expressed in frame E.
[in]  p_PQ_E  Vector from the original about point P to the new about point Q, expressed in the same frame E this spatial inertia is expressed in. 
M_SQ_E  This same spatial inertia for body or composite body S but computed about a new point Q. 
SpatialInertia& ShiftInPlace  (  const Vector3< T > &  p_PQ_E  ) 
Given this
spatial inertia M_SP_E
for some body or composite body S, computed about point P, and expressed in frame E, this method uses the Parallel Axis Theorem for spatial inertias to compute the same spatial inertia about a new point Q.
The result still is expressed in frame E. This operation is performed inplace modifying the original object.
For details see Section 2.1.2, p. 20 of [Jain 2010].
[in]  p_PQ_E  Vector from the original about point P to the new about point Q, expressed in the same frame E this spatial inertia is expressed in. 
this
spatial inertia for body or composite body S but now computed about a new point Q.

related 
Insertion operator to write SpatialInertia objects into a std::ostream
.
Especially useful for debugging.