Gazebo Math

API Reference

7.4.0
Inertial< T > Class Template Reference

The Inertial object provides a representation for the mass and inertia matrix of a body B. The components of the inertia matrix are expressed in what we call the "inertial" frame Bi of the body, i.e. the frame in which these inertia components are measured. The inertial frame Bi must be located at the center of mass of the body, but not necessarily aligned with the body’s frame. In addition, this class allows users to specify a frame F for these inertial properties by specifying the pose X_FBi of the inertial frame Bi in the inertial object frame F. More...

#include <gz/math/Inertial.hh>

Public Member Functions

 Inertial ()
 Default Constructor. More...
 
 Inertial (const Inertial< T > &_inertial)=default
 Copy constructor. More...
 
 Inertial (const MassMatrix3< T > &_massMatrix, const Pose3< T > &_pose)
 Constructs an inertial object from the mass matrix for a body B, about its center of mass Bcm, and expressed in a frame that we’ll call the "inertial" frame Bi, i.e. the frame in which the components of the mass matrix are specified (see this class’s documentation for details). The pose object specifies the pose X_FBi of the inertial frame Bi in the frame F of this inertial object (see class’s documentation). More...
 
 Inertial (const MassMatrix3< T > &_massMatrix, const Pose3< T > &_pose, const Matrix6< T > &_addedMass)
 Constructs an inertial object from the mass matrix for a body B, about its center of mass Bcm, and expressed in a frame that we'll call the "inertial" frame Bi, i.e. the frame in which the components of the mass matrix are specified (see this class's documentation for details). The pose object specifies the pose X_FBi of the inertial frame Bi in the frame F of this inertial object (see class's documentation). The added mass matrix is expressed in the link origin frame F. More...
 
 ~Inertial ()=default
 Destructor. More...
 
Matrix6< T > BodyMatrix () const
 Spatial mass matrix for body B. It does not include fluid added mass. The matrix is expressed in the object's frame F, not to be confused with the center of mass frame Bi. More...
 
const std::optional< Matrix6< T > > & FluidAddedMass () const
 Get the fluid added mass matrix. More...
 
const MassMatrix3< T > & MassMatrix () const
 Get the mass and inertia matrix. More...
 
Matrix3< T > Moi () const
 Get the moment of inertia matrix computer about the body's center of mass and expressed in this Inertial object’s frame F. More...
 
bool operator!= (const Inertial< T > &_inertial) const
 Inequality test operator. More...
 
const Inertial< T > operator+ (const Inertial< T > &_inertial) const
 Adds inertial properties to current object. The mass, center of mass location, and inertia matrix are updated as long as the total mass is positive. More...
 
Inertial< T > & operator+= (const Inertial< T > &_inertial)
 Adds inertial properties to current object. The mass, center of mass location, and inertia matrix are updated as long as the total mass is positive. More...
 
const Inertial< T > operator- (const Inertial< T > &_inertial) const
 Subtracts inertial properties from current object. The mass, center of mass location, and inertia matrix are updated as long as the remaining mass is positive. More...
 
Inertial< T > & operator-= (const Inertial< T > &_inertial)
 Subtracts inertial properties from current object. The mass, center of mass location, and inertia matrix are updated as long as the remaining mass is positive. More...
 
Inertialoperator= (const Inertial< T > &_inertial)=default
 Equal operator. More...
 
bool operator== (const Inertial< T > &_inertial) const
 Equality comparison operator. More...
 
const Pose3< T > & Pose () const
 Get the pose of the center of mass reference frame. More...
 
bool SetFluidAddedMass (const Matrix6< T > &_m)
 Set the matrix representing the inertia of the fluid that is dislocated when the body moves. Added mass should be zero if the density of the surrounding fluid is negligible with respect to the body's density. More...
 
bool SetInertialRotation (const Quaternion< T > &_q)
 Set the inertial pose rotation without affecting the MOI in the base coordinate frame. More...
 
bool SetMassMatrix (const MassMatrix3< T > &_m, const T _tolerance=GZ_MASSMATRIX3_DEFAULT_TOLERANCE< T >)
 Set the mass and inertia matrix. More...
 
bool SetMassMatrixRotation (const Quaternion< T > &_q, const T _tol=1e-6)
 Set the MassMatrix rotation (eigenvectors of inertia matrix) without affecting the MOI in the base coordinate frame. Note that symmetries in inertia matrix may prevent the output of MassMatrix3::PrincipalAxesOffset to match this function's input _q, but it is guaranteed that the MOI in the base frame will not change. A negative value of _tol (such as -1e-6) can be passed to ensure that diagonal values are always sorted. More...
 
bool SetPose (const Pose3< T > &_pose)
 Set the pose of the center of mass reference frame. More...
 
Matrix6< T > SpatialMatrix () const
 Spatial mass matrix, which includes the body's inertia, as well as the inertia of the fluid that is dislocated when the body moves. The matrix is expressed in the object's frame F, not to be confused with the center of mass frame Bi. More...
 

Detailed Description

template<typename T>
class gz::math::Inertial< T >

The Inertial object provides a representation for the mass and inertia matrix of a body B. The components of the inertia matrix are expressed in what we call the "inertial" frame Bi of the body, i.e. the frame in which these inertia components are measured. The inertial frame Bi must be located at the center of mass of the body, but not necessarily aligned with the body’s frame. In addition, this class allows users to specify a frame F for these inertial properties by specifying the pose X_FBi of the inertial frame Bi in the inertial object frame F.

Throughout this documentation, the terms "body", "object" and "link" are used interchangeably.

For information about the X_FBi notation, see http://drake.mit.edu/doxygen_cxx/group__multibody__spatial__pose.html

Constructor & Destructor Documentation

◆ Inertial() [1/4]

Inertial ( )
inline

Default Constructor.

◆ Inertial() [2/4]

Inertial ( const MassMatrix3< T > &  _massMatrix,
const Pose3< T > &  _pose 
)
inline

Constructs an inertial object from the mass matrix for a body B, about its center of mass Bcm, and expressed in a frame that we’ll call the "inertial" frame Bi, i.e. the frame in which the components of the mass matrix are specified (see this class’s documentation for details). The pose object specifies the pose X_FBi of the inertial frame Bi in the frame F of this inertial object (see class’s documentation).

Parameters
[in]_massMatrixMass and inertia matrix.
[in]_posePose of center of mass reference frame.

◆ Inertial() [3/4]

Inertial ( const MassMatrix3< T > &  _massMatrix,
const Pose3< T > &  _pose,
const Matrix6< T > &  _addedMass 
)
inline

Constructs an inertial object from the mass matrix for a body B, about its center of mass Bcm, and expressed in a frame that we'll call the "inertial" frame Bi, i.e. the frame in which the components of the mass matrix are specified (see this class's documentation for details). The pose object specifies the pose X_FBi of the inertial frame Bi in the frame F of this inertial object (see class's documentation). The added mass matrix is expressed in the link origin frame F.

Parameters
[in]_massMatrixMass and inertia matrix.
[in]_posePose of center of mass reference frame.
[in]_addedMassCoefficients for fluid added mass.

◆ Inertial() [4/4]

Inertial ( const Inertial< T > &  _inertial)
default

Copy constructor.

Parameters
[in]_inertialInertial element to copy

◆ ~Inertial()

~Inertial ( )
default

Destructor.

Member Function Documentation

◆ BodyMatrix()

Matrix6<T> BodyMatrix ( ) const
inline

Spatial mass matrix for body B. It does not include fluid added mass. The matrix is expressed in the object's frame F, not to be confused with the center of mass frame Bi.

The matrix is arranged as follows:

| m 0 0 0 m * Pz -m * Py | | 0 m 0 -m * Pz 0 m * Px | | 0 0 m m * Py -m * Px 0 | | 0 -m * Pz m * Py Ixx Ixy Ixz | | m * Pz 0 -m * Px Ixy Iyy Iyz | | -m * Py m* Px 0 Ixz Iyz Izz |

Returns
The body's 6x6 inertial matrix.
See also
SpatialMatrix

References Inertial< T >::Moi(), Matrix6< T >::SetSubmatrix(), and Matrix6< T >::Transposed().

Referenced by Inertial< T >::SpatialMatrix().

◆ FluidAddedMass()

const std::optional< Matrix6<T> >& FluidAddedMass ( ) const
inline

Get the fluid added mass matrix.

Returns
The added mass matrix. It will be nullopt if the added mass was never set.

Referenced by Inertial< T >::operator==().

◆ MassMatrix()

const MassMatrix3<T>& MassMatrix ( ) const
inline

Get the mass and inertia matrix.

Returns
The mass matrix about the body’s center of mass and expressed in the inertial frame Bi as defined by this class’s documentation

Referenced by Inertial< T >::operator+=(), Inertial< T >::operator-=(), Inertial< T >::operator==(), and Inertial< T >::SetMassMatrixRotation().

◆ Moi()

Matrix3<T> Moi ( ) const
inline

Get the moment of inertia matrix computer about the body's center of mass and expressed in this Inertial object’s frame F.

Returns
The inertia matrix computed about the body’s center of mass and expressed in this Inertial object’s frame F, as defined in this class’s documentation.

Referenced by Inertial< T >::BodyMatrix(), Inertial< T >::operator+=(), Inertial< T >::operator-=(), and Inertial< T >::SetInertialRotation().

◆ operator!=()

bool operator!= ( const Inertial< T > &  _inertial) const
inline

Inequality test operator.

Parameters
[in]_inertialInertial<T> to test
Returns
True if not equal (using the default tolerance of 1e-6)

◆ operator+()

const Inertial<T> operator+ ( const Inertial< T > &  _inertial) const
inline

Adds inertial properties to current object. The mass, center of mass location, and inertia matrix are updated as long as the total mass is positive.

Parameters
[in]_inertialInertial to add.
Returns
Sum of inertials as new object.

◆ operator+=()

Inertial<T>& operator+= ( const Inertial< T > &  _inertial)
inline

Adds inertial properties to current object. The mass, center of mass location, and inertia matrix are updated as long as the total mass is positive.

Parameters
[in]_inertialInertial to add.
Returns
Reference to this object.

References Inertial< T >::MassMatrix(), Inertial< T >::Moi(), Inertial< T >::Pose(), std::pow(), Vector3< T >::X(), Vector3< T >::Y(), and Vector3< T >::Z().

◆ operator-()

const Inertial<T> operator- ( const Inertial< T > &  _inertial) const
inline

Subtracts inertial properties from current object. The mass, center of mass location, and inertia matrix are updated as long as the remaining mass is positive.

Parameters
[in]_inertialInertial to subtract.
Returns
Reference to this object.

◆ operator-=()

Inertial<T>& operator-= ( const Inertial< T > &  _inertial)
inline

Subtracts inertial properties from current object. The mass, center of mass location, and inertia matrix are updated as long as the remaining mass is positive.

Parameters
[in]_inertialInertial to subtract.
Returns
Reference to this object.

References Inertial< T >::MassMatrix(), Inertial< T >::Moi(), Inertial< T >::Pose(), std::pow(), Vector3< T >::X(), Vector3< T >::Y(), and Vector3< T >::Z().

◆ operator=()

Inertial& operator= ( const Inertial< T > &  _inertial)
default

Equal operator.

Parameters
[in]_inertialInertial to copy.
Returns
Reference to this object.

◆ operator==()

bool operator== ( const Inertial< T > &  _inertial) const
inline

Equality comparison operator.

Parameters
[in]_inertialInertial to copy.
Returns
true if each component is equal within a default tolerance, false otherwise

References Inertial< T >::FluidAddedMass(), Inertial< T >::MassMatrix(), and Inertial< T >::Pose().

◆ Pose()

const Pose3<T>& Pose ( ) const
inline

Get the pose of the center of mass reference frame.

Returns
The pose of the inertial frame Bi in the frame F of this Inertial object as defined by this class’s documentation.

Referenced by Inertial< T >::operator+=(), Inertial< T >::operator-=(), and Inertial< T >::operator==().

◆ SetFluidAddedMass()

bool SetFluidAddedMass ( const Matrix6< T > &  _m)
inline

Set the matrix representing the inertia of the fluid that is dislocated when the body moves. Added mass should be zero if the density of the surrounding fluid is negligible with respect to the body's density.

Parameters
[in]_mNew Matrix6 object, which must be a symmetric matrix.
Returns
True if the Matrix6 is symmetric.
See also
SpatialMatrix

◆ SetInertialRotation()

bool SetInertialRotation ( const Quaternion< T > &  _q)
inline

Set the inertial pose rotation without affecting the MOI in the base coordinate frame.

Parameters
[in]_qNew rotation for inertial pose.
Returns
True if the MassMatrix3 is valid.

References Inertial< T >::Moi().

◆ SetMassMatrix()

bool SetMassMatrix ( const MassMatrix3< T > &  _m,
const T  _tolerance = GZ_MASSMATRIX3_DEFAULT_TOLERANCE<T> 
)
inline

Set the mass and inertia matrix.

Parameters
[in]_mNew MassMatrix3 object.
[in]_toleranceTolerance is passed to MassMatrix3::IsValid and is the amount of error to accept when checking whether the MassMatrix3 _m is valid. Refer to MassMatrix3::Epsilon for detailed description of _tolerance.
Returns
True if the MassMatrix3 is valid.

◆ SetMassMatrixRotation()

bool SetMassMatrixRotation ( const Quaternion< T > &  _q,
const T  _tol = 1e-6 
)
inline

Set the MassMatrix rotation (eigenvectors of inertia matrix) without affecting the MOI in the base coordinate frame. Note that symmetries in inertia matrix may prevent the output of MassMatrix3::PrincipalAxesOffset to match this function's input _q, but it is guaranteed that the MOI in the base frame will not change. A negative value of _tol (such as -1e-6) can be passed to ensure that diagonal values are always sorted.

Parameters
[in]_qNew rotation.
[in]_tolRelative tolerance given by absolute value of _tol. This is passed to the MassMatrix3 PrincipalMoments and PrincipalAxesOffset functions.
Returns
True if the MassMatrix3 is valid.

References Quaternion::Inverse(), and Inertial< T >::MassMatrix().

◆ SetPose()

bool SetPose ( const Pose3< T > &  _pose)
inline

Set the pose of the center of mass reference frame.

Parameters
[in]_poseNew pose.
Returns
True if the MassMatrix3 is valid.

◆ SpatialMatrix()

Matrix6<T> SpatialMatrix ( ) const
inline

Spatial mass matrix, which includes the body's inertia, as well as the inertia of the fluid that is dislocated when the body moves. The matrix is expressed in the object's frame F, not to be confused with the center of mass frame Bi.

Returns
The spatial mass matrix.
See also
BodyMatrix
FluidAddedMass

References Inertial< T >::BodyMatrix().


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