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\)).
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 (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 > | IsZero () const | ||
Returns true if all of the elements in this spatial inertia are zero and false otherwise. More... | |||
boolean< T > | IsPhysicallyValid () const | ||
Performs a number of checks to verify that this is a physically valid spatial inertia. More... | |||
std::string | CriticizeNotPhysicallyValid () const | ||
Performs the same checks as the boolean typed IsPhysicallyValid(). 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< T > & | operator+= (const SpatialInertia< T > &M_BP_E) | ||
Adds in a spatial inertia to this spatial inertia. More... | |||
void | 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 re-expressed in another frame A. More... | |||
SpatialInertia< T > | 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 re-expressed in another frame A. More... | |||
void | 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< T > | 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 | ||
Spatial inertia equivalent shapes | |||
Calculates principal semi-diameters (half-lengths), principal axes orientations, and the position of a uniform-density object whose spatial inertia is equal to Example: Consider an oddly-shaped rigid body B with a known spatial inertia M_BBo_B about B's origin Bo, expressed in frame B. These functions return an easily visualized simple shape whose spatial inertial is equal to M_BBo_B. The simple shape is defined by a frame A with origin Ao at Bcm (B's center of mass), has principal dimensions [a b c], and has unit vectors Ax, Ay, Az parallel to the simple shape's principal directions. When the simple shape is a uniform-density solid ellipsoid, proceed as follows to form [a b c], the rotation matrix R_BA describing Ax, Ay, Az, and the position vector p_BoAo_B from Bo to Ao (ellipsoid center of mass). const SpatialInertia<double>& M_BBo_B = B.default_spatial_inertia(); auto [abc, X_BA] = M_BBo_B.CalcPrincipalSemiDiametersAndPoseForSolidEllipsoid();
| |||
std::pair< Vector3< double >, drake::math::RigidTransform< double > > | CalcPrincipalSemiDiametersAndPoseForSolidEllipsoid () const | ||
Returns 3 principal semi-diameters [lmax lmed lmin] sorted in descending order (lmax ≥ lmed ≥ lmin), orientation, and position of a solid ellipsoid whose spatial inertia is equal to this spatial inertia. More... | |||
std::pair< Vector3< double >, drake::math::RigidTransform< double > > | CalcPrincipalHalfLengthsAndPoseForSolidBox () const | ||
Returns three ½-lengths [lmax lmed lmin] sorted in descending order (lmax ≥ lmed ≥ lmin), orientation, and position of a solid box whose spatial inertia is equal to this spatial inertia. More... | |||
std::pair< Vector3< double >, drake::math::RigidTransform< double > > | CalcPrincipalHalfLengthsAndPoseForMinimumBoundingBox () const | ||
Returns three ½-lengths [lmax lmed lmin] sorted in descending order (lmax ≥ lmed ≥ lmin), orientation, and position of a box whose mass is concentrated in 8 particles at the box's corners and whose spatial inertia is equal to this spatial inertia. More... | |||
T | CalcMinimumPhysicalLength () const | ||
Returns the minimum possible length for the physical extent of the massive object that underlies this spatial inertia. More... | |||
Static Public Member Functions | |
static SpatialInertia< T > | 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... | |
static SpatialInertia< T > | MakeUnitary () |
(Internal use only) Creates a spatial inertia whose mass is 1, position vector to center of mass is zero, and whose rotational inertia has moments of inertia of 1 and products of inertia of 0. More... | |
static SpatialInertia< T > | PointMass (const T &mass, const Vector3< T > &position) |
Creates the spatial inertia for a particle Q of mass m about a point P. More... | |
static SpatialInertia< T > | SolidBoxWithDensity (const T &density, const T &lx, const T &ly, const T &lz) |
Creates a spatial inertia for a uniform density solid box B about its geometric center Bo (which is coincident with B's center of mass Bcm). More... | |
static SpatialInertia< T > | SolidBoxWithMass (const T &mass, const T &lx, const T &ly, const T &lz) |
Creates a spatial inertia for a uniform density solid box B about its geometric center Bo (which is coincident with B's center of mass Bcm). More... | |
static SpatialInertia< T > | SolidCubeWithDensity (const T &density, const T &length) |
Creates a spatial inertia for a uniform density solid cube B about its geometric center Bo (which is coincident with B's center of mass Bcm). More... | |
static SpatialInertia< T > | SolidCubeWithMass (const T &mass, const T &length) |
Creates a spatial inertia for a uniform density solid cube B about its geometric center Bo (which is coincident with B's center of mass Bcm). More... | |
static SpatialInertia< T > | SolidCapsuleWithDensity (const T &density, const T &radius, const T &length, const Vector3< T > &unit_vector) |
Creates a spatial inertia for a uniform density solid capsule B about its geometric center Bo (which is coincident with B's center of mass Bcm). More... | |
static SpatialInertia< T > | SolidCapsuleWithMass (const T &mass, const T &radius, const T &length, const Vector3< T > &unit_vector) |
Creates a spatial inertia for a uniform density solid capsule B about its geometric center Bo (which is coincident with B's center of mass Bcm). More... | |
static SpatialInertia< T > | SolidCylinderWithDensity (const T &density, const T &radius, const T &length, const Vector3< T > &unit_vector) |
Creates a spatial inertia for a uniform density solid cylinder B about its geometric center Bo (which is coincident with B's center of mass Bcm). More... | |
static SpatialInertia< T > | SolidCylinderWithMass (const T &mass, const T &radius, const T &length, const Vector3< T > &unit_vector) |
Creates a spatial inertia for a uniform density solid cylinder B about its geometric center Bo (which is coincident with B's center of mass Bcm). More... | |
static SpatialInertia< T > | SolidCylinderWithDensityAboutEnd (const T &density, const T &radius, const T &length, const Vector3< T > &unit_vector) |
Creates a spatial inertia for a uniform-density solid cylinder B about an end-point Bp of the cylinder's axis (see below for more about Bp). More... | |
static SpatialInertia< T > | SolidCylinderWithMassAboutEnd (const T &mass, const T &radius, const T &length, const Vector3< T > &unit_vector) |
Creates a spatial inertia for a uniform-density solid cylinder B about an end-point Bp of the cylinder's axis (see below for more about Bp). More... | |
static SpatialInertia< T > | ThinRodWithMass (const T &mass, const T &length, const Vector3< T > &unit_vector) |
Creates a spatial inertia for a uniform-density thin rod B about its center of mass Bcm. More... | |
static SpatialInertia< T > | ThinRodWithMassAboutEnd (const T &mass, const T &length, const Vector3< T > &unit_vector) |
Creates a spatial inertia for a uniform-density thin rod B about one of its ends. More... | |
static SpatialInertia< T > | SolidEllipsoidWithDensity (const T &density, const T &a, const T &b, const T &c) |
Creates a spatial inertia for a uniform density solid ellipsoid B about its geometric center Bo (which is coincident with B's center of mass Bcm). More... | |
static SpatialInertia< T > | SolidEllipsoidWithMass (const T &mass, const T &a, const T &b, const T &c) |
Creates a spatial inertia for a uniform density solid ellipsoid B about its geometric center Bo (which is coincident with B's center of mass Bcm). More... | |
static SpatialInertia< T > | SolidSphereWithDensity (const T &density, const T &radius) |
Creates a spatial inertia for a uniform density solid sphere B about its geometric center Bo (which is coincident with B's center of mass Bcm). More... | |
static SpatialInertia< T > | SolidSphereWithMass (const T &mass, const T &radius) |
Creates a spatial inertia for a uniform density solid sphere B about its geometric center Bo (which is coincident with B's center of mass Bcm). More... | |
static SpatialInertia< T > | HollowSphereWithDensity (const T &area_density, const T &radius) |
Creates a spatial inertia for a uniform density thin hollow sphere B about its geometric center Bo (which is coincident with B's center of mass Bcm). More... | |
static SpatialInertia< T > | HollowSphereWithMass (const T &mass, const T &radius) |
Creates a spatial inertia for a uniform density hollow sphere B about its geometric center Bo (which is coincident with B's center of mass Bcm). More... | |
static SpatialInertia< T > | SolidTetrahedronAboutPointWithDensity (const T &density, const Vector3< T > &p0, const Vector3< T > &p1, const Vector3< T > &p2, const Vector3< T > &p3) |
Creates a spatial inertia for a uniform density solid tetrahedron B about a point A, from which position vectors to B's 4 vertices B0, B1, B2, B3 are measured (position vectors are all expressed in a common frame E). More... | |
static SpatialInertia< T > | SolidTetrahedronAboutVertexWithDensity (const T &density, const Vector3< T > &p1, const Vector3< T > &p2, const Vector3< T > &p3) |
(Advanced) Creates a spatial inertia for a uniform density solid tetrahedron B about its vertex B0, from which position vectors to B's other 3 vertices B1, B2, B3 are measured (position vectors are all expressed in a common frame E). More... | |
static SpatialInertia | Zero () |
Initializes mass, center of mass and rotational inertia to zero. More... | |
static SpatialInertia | NaN () |
Initializes mass, center of mass and rotational inertia to invalid NaN's for a quick detection of uninitialized values. More... | |
Related Functions | |
(Note that these are not member functions.) | |
template<typename T > | |
std::ostream & | operator<< (std::ostream &out, const SpatialInertia< T > &M) |
Writes an instance of SpatialInertia into a std::ostream. More... | |
|
default |
|
default |
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 non-physically viable spatial inertia. Since this check has non-negligable 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.
T CalcMinimumPhysicalLength | ( | ) | const |
Returns the minimum possible length for the physical extent of the massive object that underlies this spatial inertia.
In other words, the underlying physical object must have at least two particles whose distance between each other is greater than or equal to the minimum possible length.
this
spatial inertia, which happens to be equal to √(2 * trace of the central unit inertia associated with this
). this
spatial inertia (maybe due to inertia conversion errors, e.g., factor of 10⁷ from kg m² to g cm² or 10⁹ from kg m² to g mm²). To assess whether the minimum possible length is reasonable, it helps to have comparable sizes, e.g., the world's largest aircraft carrier has a space-diagonal ≈ 355 m (length ≈ 337 m, width ≈ 78 m, height ≈ 76 m), the largest land vehicle (Bagger bucket-wheel excavator) is ≈ 224 m long, the largest human object in space (International Space Station) is 109 m long and 75 m wide, the USA space shuttle is ≈ 37 m long and can carry a 15.2 m Canadarm, the world's largest humanoid robot (Mononofu) is ≈ 8.5 m tall. Also, minimum possible length can be compared to known physical geometry (e.g., realistic collision geometry, visual geometry, or physical extents associated with body connectivity data and topology), and this comparison can be used to warn that a spatial inertia may be physically impossible (e.g., underlying geometry is smaller than the minimum possible length). std::pair<Vector3<double>, drake::math::RigidTransform<double> > CalcPrincipalHalfLengthsAndPoseForMinimumBoundingBox | ( | ) | const |
Returns three ½-lengths [lmax lmed lmin] sorted in descending order (lmax ≥ lmed ≥ lmin), orientation, and position of a box whose mass is concentrated in 8 particles at the box's corners and whose spatial inertia is equal to this
spatial inertia.
The physical geometry of the actual underlying object must be larger than this box, as this box is the minimum bounding box for the actual geometry. See Spatial inertia equivalent shapes for more details.
std::exception | if the elements of this spatial inertia cannot be converted to a real finite double. For example, an exception is thrown if this contains an erroneous NaN or if scalar type T is symbolic. |
std::pair<Vector3<double>, drake::math::RigidTransform<double> > CalcPrincipalHalfLengthsAndPoseForSolidBox | ( | ) | const |
Returns three ½-lengths [lmax lmed lmin] sorted in descending order (lmax ≥ lmed ≥ lmin), orientation, and position of a solid box whose spatial inertia is equal to this
spatial inertia.
See Spatial inertia equivalent shapes for more details.
std::exception | if the elements of this spatial inertia cannot be converted to a real finite double. For example, an exception is thrown if this contains an erroneous NaN or if scalar type T is symbolic. |
std::pair<Vector3<double>, drake::math::RigidTransform<double> > CalcPrincipalSemiDiametersAndPoseForSolidEllipsoid | ( | ) | const |
Returns 3 principal semi-diameters [lmax lmed lmin] sorted in descending order (lmax ≥ lmed ≥ lmin), orientation, and position of a solid ellipsoid whose spatial inertia is equal to this
spatial inertia.
See Spatial inertia equivalent shapes for more details.
std::exception | if the elements of this spatial inertia cannot be converted to a real finite double. For example, an exception is thrown if this contains an erroneous NaN or if scalar type T is symbolic. |
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 vice-versa. Matrix6<T> CopyToFullMatrix6 | ( | ) | const |
Copy to a full 6x6 matrix representation.
std::string CriticizeNotPhysicallyValid | ( | ) | const |
Performs the same checks as the boolean typed IsPhysicallyValid().
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.
|
static |
Creates a spatial inertia for a uniform density thin hollow sphere B about its geometric center Bo (which is coincident with B's center of mass Bcm).
[in] | area_density | mass per unit area (kg/m²). |
[in] | radius | sphere's radius in meters (the hollow sphere is regarded as an infinitesimally thin shell of uniform density). |
M_BBo_B | B's spatial inertia about Bo, expressed in B. Since B's rotational inertia is triaxially symmetric, M_BBo_B = M_BBo_E, i.e., M_BBo expressed in frame B is equal to M_BBo expressed in an arbitrary frame E. |
std::exception | if area_density or radius is not positive and finite. |
|
static |
Creates a spatial inertia for a uniform density hollow sphere B about its geometric center Bo (which is coincident with B's center of mass Bcm).
[in] | mass | mass of the hollow sphere (kg). |
[in] | radius | sphere's radius in meters (the hollow sphere is regarded as an infinitesimally thin shell of uniform density). |
M_BBo | B's spatial inertia about Bo. Since B's rotational inertia is triaxially symmetric, M_BBo_B = M_BBo_E, i.e., M_BBo expressed in frame B is equal to M_BBo expressed in an arbitrary frame E. |
std::exception | if mass or radius is not positive and finite. |
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.
boolean<T> IsZero | ( | ) | const |
Returns true
if all of the elements in this spatial inertia are zero and false
otherwise.
|
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 non-physically 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. |
|
static |
(Internal use only) Creates a spatial inertia whose mass is 1, position vector to center of mass is zero, and whose rotational inertia has moments of inertia of 1 and products of inertia of 0.
|
static |
Initializes mass, center of mass and rotational inertia to invalid NaN's for a quick detection of uninitialized values.
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 j-th column stores the spatial vector in F⁶ result of multiplying this
spatial inertia with the j-th column of Mmatrix
.
SpatialInertia<T>& 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 |
|
static |
Creates the spatial inertia for a particle Q of mass m about a point P.
[in] | mass | mass of the single particle (units of kg). |
[in] | position | vector from point P to Q, expressed in a frame B. |
M_QP_B | particle Q's spatial inertia about P, expressed in frame B. |
std::exception | if mass is not positive and finite. |
SpatialInertia<T> 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 re-expressed 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 re-expressed in frame A. |
void 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 re-expressed in another frame A.
This operation is performed in-place modifying the original object. On return, this
is now re-expressed in frame A, that is, M_SP_A
.
[in] | R_AE | Rotation matrix from frame E to frame A. |
void SetNaN | ( | ) |
Sets this
spatial inertia to have NaN entries.
Typically used for quick detection of uninitialized values.
SpatialInertia<T> 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. |
void 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 in-place modifying the original object. On return, this
is now computed about a new point Q.
For details see Section 2.1.2, p. 20 of [Jain 2010].
[in] | p_PQ_E | position vector from the original about-point P to the new about-point Q, expressed in the same frame E that this spatial inertia is expressed in. |
|
static |
Creates a spatial inertia for a uniform density solid box B about its geometric center Bo (which is coincident with B's center of mass Bcm).
[in] | density | mass per volume (kg/m³). |
[in] | lx | length of the box in the Bx direction (meters). |
[in] | ly | length of the box in the By direction (meters). |
[in] | lz | length of the box in the Bz direction (meters). |
M_BBo_B | B's spatial inertia about Bo, expressed in B. |
std::exception | if density, lx, ly, or lz is not positive and finite. |
|
static |
Creates a spatial inertia for a uniform density solid box B about its geometric center Bo (which is coincident with B's center of mass Bcm).
[in] | mass | mass of the solid box (kg). |
[in] | lx | length of the box in the Bx direction (meters). |
[in] | ly | length of the box in the By direction (meters). |
[in] | lz | length of the box in the Bz direction (meters). |
M_BBo_B | B's spatial inertia about Bo, expressed in B. |
std::exception | if mass, lx, ly, or lz is not positive and finite. |
|
static |
Creates a spatial inertia for a uniform density solid capsule B about its geometric center Bo (which is coincident with B's center of mass Bcm).
[in] | density | mass per volume (kg/m³). |
[in] | radius | radius of the cylinder/half-sphere parts of the capsule. |
[in] | length | length of the cylindrical part of the capsule. |
[in] | unit_vector | unit vector defining the axial direction of the cylindrical part of the capsule, expressed in B. |
M_BBo_B | B's spatial inertia about Bo, expressed in B. |
std::exception | if density, radius, or length is not positive and finite or if ‖unit_vector‖ is not within 1.0E-14 of 1.0. |
|
static |
Creates a spatial inertia for a uniform density solid capsule B about its geometric center Bo (which is coincident with B's center of mass Bcm).
[in] | mass | mass of the solid capsule (kg). |
[in] | radius | radius of the cylinder/half-sphere parts of the capsule. |
[in] | length | length of the cylindrical part of the capsule. |
[in] | unit_vector | unit vector defining the axial direction of the cylindrical part of the capsule, expressed in B. |
M_BBo_B | B's spatial inertia about Bo, expressed in B. |
std::exception | if mass, radius, or length is not positive and finite or if ‖unit_vector‖ is not within 1.0E-14 of 1.0. |
|
static |
Creates a spatial inertia for a uniform density solid cube B about its geometric center Bo (which is coincident with B's center of mass Bcm).
[in] | density | mass per volume (kg/m³). |
[in] | length | The length of each of the cube's sides (meters). |
M_BBo_B | B's spatial inertia about Bo, expressed in B. Since B's rotational inertia is triaxially symmetric, M_BBo_B = M_BBo_E, i.e., M_BBo expressed in frame B is equal to M_BBo expressed in an arbitrary frame E. |
std::exception | if density or length is not positive and finite. |
|
static |
Creates a spatial inertia for a uniform density solid cube B about its geometric center Bo (which is coincident with B's center of mass Bcm).
[in] | mass | mass of the solid cube (kg). |
[in] | length | The length of each of the cube's sides (meters). |
M_BBo_B | B's spatial inertia about Bo, expressed in B. Since B's rotational inertia is triaxially symmetric, M_BBo_B = M_BBo_E, i.e., M_BBo expressed in frame B is equal to M_BBo expressed in an arbitrary frame E. |
std::exception | if mass or length is not positive and finite. |
|
static |
Creates a spatial inertia for a uniform density solid cylinder B about its geometric center Bo (which is coincident with B's center of mass Bcm).
[in] | density | mass per volume (kg/m³). |
[in] | radius | radius of the cylinder (meters). |
[in] | length | length of cylinder in unit_vector direction (meters). |
[in] | unit_vector | unit vector defining the axial direction of the cylinder, expressed in B. |
M_BBo_B | B's spatial inertia about Bo, expressed in B. |
std::exception | if density, radius, or length is not positive and finite or if ‖unit_vector‖ is not within 1.0E-14 of 1.0. |
|
static |
Creates a spatial inertia for a uniform-density solid cylinder B about an end-point Bp of the cylinder's axis (see below for more about Bp).
[in] | density | mass per volume (kg/m³). |
[in] | radius | radius of cylinder (meters). |
[in] | length | length of cylinder in unit_vector direction (meters). |
[in] | unit_vector | unit vector parallel to the axis of the cylinder and directed from Bp to Bcm (B's center of mass), expressed in B. |
M_BBp_B | B's spatial inertia about Bp, expressed in B. |
std::exception | if density, radius, or length is not positive and finite or if ‖unit_vector‖ is not within 1.0E-14 of 1.0. |
|
static |
Creates a spatial inertia for a uniform density solid cylinder B about its geometric center Bo (which is coincident with B's center of mass Bcm).
[in] | mass | mass of the solid cylinder (kg). |
[in] | radius | radius of the cylinder (meters). |
[in] | length | length of cylinder in unit_vector direction (meters). |
[in] | unit_vector | unit vector defining the axial direction of the cylinder, expressed in B. |
M_BBo_B | B's spatial inertia about Bo, expressed in B. |
std::exception | if mass, radius, or length is not positive and finite or if ‖unit_vector‖ is not within 1.0E-14 of 1.0. |
|
static |
Creates a spatial inertia for a uniform-density solid cylinder B about an end-point Bp of the cylinder's axis (see below for more about Bp).
[in] | mass | mass of the solid cylinder (kg). |
[in] | radius | radius of cylinder (meters). |
[in] | length | length of cylinder in unit_vector direction (meters). |
[in] | unit_vector | unit vector parallel to the axis of the cylinder and directed from Bp to Bcm (B's center of mass), expressed in B. |
M_BBp_B | B's spatial inertia about Bp, expressed in B. |
std::exception | if density, radius, or length is not positive and finite or if ‖unit_vector‖ is not within 1.0E-14 of 1.0. |
|
static |
Creates a spatial inertia for a uniform density solid ellipsoid B about its geometric center Bo (which is coincident with B's center of mass Bcm).
[in] | density | mass per volume (kg/m³). |
[in] | a | length of ellipsoid semi-axis in the ellipsoid Bx direction. |
[in] | b | length of ellipsoid semi-axis in the ellipsoid By direction. |
[in] | c | length of ellipsoid semi-axis in the ellipsoid Bz direction. |
M_BBo_B | B's spatial inertia about Bo, expressed in B. |
std::exception | if density, a, b, or c is not positive and finite. |
|
static |
Creates a spatial inertia for a uniform density solid ellipsoid B about its geometric center Bo (which is coincident with B's center of mass Bcm).
[in] | mass | mass of the solid ellipsoid (kg). |
[in] | a | length of ellipsoid semi-axis in the ellipsoid Bx direction. |
[in] | b | length of ellipsoid semi-axis in the ellipsoid By direction. |
[in] | c | length of ellipsoid semi-axis in the ellipsoid Bz direction. |
M_BBo_B | B's spatial inertia about Bo, expressed in B. |
std::exception | if mass, a, b, or c is not positive and finite. |
|
static |
Creates a spatial inertia for a uniform density solid sphere B about its geometric center Bo (which is coincident with B's center of mass Bcm).
[in] | density | mass per volume (kg/m³). |
[in] | radius | sphere's radius (meters). |
M_BBo | B's spatial inertia about Bo. Since B's rotational inertia is triaxially symmetric, M_BBo_B = M_BBo_E, i.e., M_BBo expressed in frame B is equal to M_BBo expressed in an arbitrary frame E. |
std::exception | if density or radius is not positive and finite. |
|
static |
Creates a spatial inertia for a uniform density solid sphere B about its geometric center Bo (which is coincident with B's center of mass Bcm).
[in] | mass | mass of the solid sphere (kg). |
[in] | radius | sphere's radius (meters). |
M_BBo | B's spatial inertia about Bo. Since B's rotational inertia is triaxially symmetric, M_BBo_B = M_BBo_E, i.e., M_BBo expressed in frame B is equal to M_BBo expressed in an arbitrary frame E. |
std::exception | if mass or radius is not positive and finite. |
|
static |
Creates a spatial inertia for a uniform density solid tetrahedron B about a point A, from which position vectors to B's 4 vertices B0, B1, B2, B3 are measured (position vectors are all expressed in a common frame E).
[in] | density | mass per volume (kg/m³). |
[in] | p0 | position vector p_AB0_E from point A to B0, expressed in E. |
[in] | p1 | position vector p_AB1_E from point A to B1, expressed in E. |
[in] | p2 | position vector p_AB2_E from point A to B2, expressed in E. |
[in] | p3 | position vector p_AB3_E from point A to B3, expressed in E. |
M_BA_E | B's spatial inertia about point A, expressed in E. |
std::exception | if density is not positive and finite. double density = 1000; Vector3<double> p_WoB0_W(1, 0, 0); Vector3<double> p_WoB1_W(2, 0, 0); Vector3<double> p_WoB2_W(1, 1, 0); Vector3<double> p_WoB3_W(1, 0, 1); SpatialInertia<double> M_BWo_W = density, p_WoB0_W, p_WoB1_W, p_WoB2_W, p_WoB3_W); |
|
static |
(Advanced) Creates a spatial inertia for a uniform density solid tetrahedron B about its vertex B0, from which position vectors to B's other 3 vertices B1, B2, B3 are measured (position vectors are all expressed in a common frame E).
[in] | density | mass per volume (kg/m³). |
[in] | p1 | position vector p_B0B1_E from B0 to B1, expressed in E. |
[in] | p2 | position vector p_B0B2_E from B0 to B2, expressed in E. |
[in] | p3 | position vector p_B0B3_E from B0 to B3, expressed in E. |
M_BB0_E | B's spatial inertia about its vertex B0, expressed in E. |
std::exception | if density is not positive and finite. |
|
static |
Creates a spatial inertia for a uniform-density thin rod B about its center of mass Bcm.
[in] | mass | mass of the rod (units of kg). |
[in] | length | length of the rod (units of meters). |
[in] | unit_vector | unit vector defining the rod's axial direction, expressed in B. |
M_BBcm_B | B's spatial inertia about Bcm, expressed in B. |
std::exception | if mass or length is not positive and finite or if ‖unit_vector‖ is not within 1.0E-14 of 1.0. |
|
static |
Creates a spatial inertia for a uniform-density thin rod B about one of its ends.
[in] | mass | mass of the rod (units of kg). |
[in] | length | length of the rod (units of meters). |
[in] | unit_vector | unit vector defining the rod's axial direction, expressed in B. |
M_BBp_B | B's spatial inertia about Bp, expressed in B. |
std::exception | if mass or length is not positive and finite or if ‖unit_vector‖ is not within 1.0E-14 of 1.0. |
|
static |
Initializes mass, center of mass and rotational inertia to zero.
|
related |
Writes an instance of SpatialInertia into a std::ostream.