Drake
SpatialInertia< T > Class Template Reference

This class represents the physical concept of a Spatial Inertia. More...

#include <multibody/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)
 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...
 
bool IsNaN () const
 Returns true if any of the elements in this spatial inertia is NaN and false otherwise. More...
 
bool 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...
 
SpatialInertiaoperator+= (const SpatialInertia< T > &M_BP_E)
 Adds in a spatial inertia to this spatial inertia. More...
 
SpatialInertiaReExpressInPlace (const Matrix3< 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 re-expressed in another frame A. More...
 
SpatialInertia ReExpress (const Matrix3< 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 re-expressed in another frame A. More...
 
SpatialInertiaShiftInPlace (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...
 
Implements CopyConstructible, CopyAssignable, MoveConstructible, MoveAssignable
 SpatialInertia (const SpatialInertia &)=default
 
SpatialInertiaoperator= (const SpatialInertia &)=default
 
 SpatialInertia (SpatialInertia &&)=default
 
SpatialInertiaoperator= (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...
 

Detailed Description

template<typename T>
class drake::multibody::SpatialInertia< T >

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 semi-definite. It logically consists of 3x3 sub-matrices 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 skew-symmetric 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\)).

Note
This class does not implement any mechanism to track the frame E in which a spatial inertia is expressed or about what point is computed. Methods and operators on this class have no means to determine frame consistency through operations. It is therefore the responsibility of users of this class to keep track of frames in which operations are performed. We suggest doing that using disciplined notation, as described above.
  • [Jain 2010] Jain, A., 2010. Robot and multibody dynamics: analysis and algorithms. Springer Science & Business Media.
Template Parameters
TThe underlying scalar type. Must be a valid Eigen scalar.

Instantiated templates for the following kinds of T's are provided:

  • double
  • AutoDiffXd

They are already available to link against in the containing library.

Constructor & Destructor Documentation

SpatialInertia ( const SpatialInertia< T > &  )
default
SpatialInertia ( SpatialInertia< T > &&  )
default
SpatialInertia ( )
inline

Default SpatialInertia constructor initializes mass, center of mass and rotational inertia to invalid NaN's for a quick detection of uninitialized values.

Here is the caller graph for this function:

SpatialInertia ( const T &  mass,
const Vector3< T > &  p_PScm_E,
const UnitInertia< T > &  G_SP_E 
)
inline

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.

Note
The third argument of this constructor is unusual in that it is an UnitInertia (not a traditional RotationalInertia) and its inertia is about the arbitrary point P (not Scm – S's center of mass).
See also
MakeFromCentralInertia a factory method with traditional utility.

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 non-physically viable spatial inertia.

Parameters
[in]massThe mass of the body or composite body S.
[in]p_PScm_EThe position vector from point P to the center of mass of body or composite body S expressed in frame E.
[in]G_SP_EUnitInertia of the body or composite body S computed about origin point P and expressed in frame E.

Member Function Documentation

Vector3<T> CalcComMoment ( ) const
inline

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.

Here is the caller graph for this function:

RotationalInertia<T> CalcRotationalInertia ( ) const
inline

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.

Here is the caller graph for this function:

SpatialInertia<Scalar> cast ( ) const
inline

Returns a new SpatialInertia object templated on Scalar initialized from the value of this spatial inertia.

Template Parameters
ScalarThe scalar type on which the new spatial inertia will be templated.
Note
SpatialInertia<From>::cast<To>() creates a new SpatialInertia<To> from a SpatialInertia<From> but only if type To is constructible from type From. As an example of this, SpatialInertia<double>::cast<AutoDiffXd>() is valid since AutoDiffXd a(1.0) is valid. However, SpatialInertia<AutoDiffXd>::cast<double>() is not.

Here is the caller graph for this function:

Matrix6<T> CopyToFullMatrix6 ( ) const
inline

Copy to a full 6x6 matrix representation.

const Vector3<T>& get_com ( ) const
inline

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.

Here is the caller graph for this function:

const T& get_mass ( ) const
inline

Get a constant reference to the mass of this spatial inertia.

Here is the caller graph for this function:

const UnitInertia<T>& get_unit_inertia ( ) const
inline

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.

Here is the caller graph for this function:

bool IsNaN ( ) const
inline

Returns true if any of the elements in this spatial inertia is NaN and false otherwise.

Here is the caller graph for this function:

bool IsPhysicallyValid ( ) const
inline

Performs a number of checks to verify that this is a physically valid spatial inertia.

The checks performed are:

  • No NaN entries.
  • Non-negative mass.
  • Non-negative principal moments about the center of mass.
  • Principal moments about the center of mass must satisfy the triangle inequality:

Here is the caller graph for this function:

static SpatialInertia MakeFromCentralInertia ( const T &  mass,
const Vector3< T > &  p_PScm_E,
const RotationalInertia< T > &  I_SScm_E 
)
inlinestatic

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 non-physically viable spatial inertia.

Parameters
[in]massThe mass of the body or composite body S.
[in]p_PScm_EThe position vector from point P to point Scm (S's center of mass), expressed in a frame E.
[in]I_SScm_ES's RotationalInertia about Scm, expressed in frame E.
Return values
M_SP_ES's spatial inertia about point P, expressed in frame E.

Here is the caller graph for this function:

SpatialForce<T> operator* ( const SpatialAcceleration< T > &  A_WB_E) const
inline

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.

Note
The term F_Bo_E computed by this operator appears in the equations of motion for a rigid body which, when writen 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_Bo
where Ftot_BBo is the total spatial force applied on body B at 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).
SpatialInertia& operator+= ( const SpatialInertia< T > &  M_BP_E)
inline

Adds in a spatial inertia to this spatial inertia.

Parameters
[in]M_BP_EA 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.
Returns
A reference to this spatial inertia, which has been updated to include the given spatial inertia M_BP_E.
Note
This operation aborts if the mass of the resulting spatial inertia is zero since in that case the position vector from the about point to the center of mass is not well defined.
Warning
This operation is only valid if both spatial inertias are computed about the same point P and expressed in the same frame E. Considering 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.
SpatialInertia& operator= ( SpatialInertia< T > &&  )
default
SpatialInertia& operator= ( const SpatialInertia< T > &  )
default
SpatialInertia ReExpress ( const Matrix3< T > &  R_AE) const
inline

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 re-expressed in another frame A.

Parameters
[in]R_AERotation matrix from frame E to frame A.
Return values
M_SP_AThe same spatial inertia of S about P but now re-expressed in frame A.
See also
ReExpressInPlace() for details.

Here is the caller graph for this function:

SpatialInertia& ReExpressInPlace ( const Matrix3< T > &  R_AE)
inline

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 re-expressed in another frame A.

This operation is performed in-place modifying the original object.

Parameters
[in]R_AERotation matrix from frame E to frame A.
Returns
A reference to this rotational inertia about the same point P but now re-expressed in frame A, that is, M_SP_A.
Warning
This method does not check whether the input matrix R_AE represents a valid rotation or not. It is the resposibility of users to provide valid rotation matrices.
void SetNaN ( )
inline

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
inline

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.

See also
ShiftInPlace() for more details.
Parameters
[in]p_PQ_EVector from the original about point P to the new about point Q, expressed in the same frame E this spatial inertia is expressed in.
Return values
<tt>M_SQ_E</tt>This same spatial inertia for body or composite body S but computed about about a new point Q.
SpatialInertia& ShiftInPlace ( const Vector3< T > &  p_PQ_E)
inline

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 in-place modifying the original object.

See also
Shift() which does not modify this object.

For details see Section 2.1.2, p. 20 of [Jain 2010].

Parameters
[in]p_PQ_EVector from the original about point P to the new about point Q, expressed in the same frame E this spatial inertia is expressed in.
Returns
A reference to this spatial inertia for body or composite body S but now computed about about a new point Q.

Friends And Related Function Documentation

std::ostream & operator<< ( std::ostream &  o,
const SpatialInertia< T > &  M 
)
related

Insertion operator to write SpatialInertia objects into a std::ostream.

Especially useful for debugging.

Here is the call graph for this function:


The documentation for this class was generated from the following file: