template<typename T>
class drake::multibody::ArticulatedBodyInertia< T >
Articulated Body Inertia is the inertia that a body appears to have when it is the base (or root) of a rigid-body system, also referred to as Articulated Body in the context of articulated body algorithms.
The Articulated Body Inertia is a very useful multibody dynamics concept that was introduced by Featherstone [Featherstone 1983] to develop the remarkable O(n)
Articulated Body Algorithm (ABA) for solving forward dynamics. Recall that the Newton-Euler equations allow us to describe the combined rotational and translational dynamics of a rigid body:
F_BBo_W = M_B_W * A_WB + Fb_Bo_W (1)
where the spatial inertia (see SpatialInertia) M_B_W
of body B expressed in the world frame W linearly relates the spatial acceleration (see SpatialAcceleration) of body B in the world frame with the total applied spatial forces (see SpatialForce) F_BBo
on body B and where Fb_Bo_W
contains the velocity dependent gyroscopic terms.
A similar relationship is found for an articulated body with a rigid body B at the base (or root). Even though the bodies in this multibody system are allowed to have relative motions among them, there still is a linear relationship between the spatial force F_BBo_W
applied on this body and the resulting acceleration A_WB
:
F_BBo_W = P_B_W * A_WB + Z_Bo_W (2)
where P_B_W
is the articulated body inertia of body B and Z_Bo_W
is a bias force that includes the gyroscopic and Coriolis forces and becomes zero when all body velocities and all applied generalized forces outboard from body B are zero [Jain 2010, §7.2.1]. The articulated body inertia P_B_W
is related to the multibody subsystem consisting only of bodies that are outboard of body B. We refer to this subsystem as the articulated body subsystem associated with body B. Equation (2) describes the acceleration response of body B, but also taking into account all outboard bodies connected to B. A special case is that of an articulated body composed of a single rigid body. For this special case, Eq. (2) reduces to Eq. (1) for the dynamics of rigid body B. In other words, the ABI for an articulated body consisting of a single rigid body exactly equals the spatial inertia of that body.
Articulated body inertias are elements of ℝ⁶ˣ⁶ that, as for spatial inertias, are symmetric and positive semi-definite. However, ABI objects are not spatial inertias. The spatial inertia of a rigid body can be described by a reduced set of ten parameters, namely the mass, center of mass and the six components of the rotational inertia for that body. However, this parametrization by ten parameters is just not possible for an ABI and the full 21 elements of the symmetric 6x6
matrix must be specified [Jain 2010, §6.4]. As a result ABI objects can have different properties than spatial inertia objects. As an example, the apparent mass of an articulated body will in general depend on the direction of the applied force. That is, the simple relationship F = m * a
is no longer valid for an articulated body's center of mass (refer to the excellent example 7.1 in [Featherstone 2008]).
We adopt the notation introduced by [Jain 2010] and generally we will use an uppercase P to represent an ABI. Thus, in typeset material we use the symbol \( [P^{A/Q}]_E \) to represent the spatial inertia of an articulated body A, about a point Q, expressed in a frame E. For this inertia, the monogram notation reads P_AQ_E
.
- Note
- This class does not implement any mechanism to track the frame E in which an articulated body 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.
- [Featherstone 1983] Featherstone, R., 1983. The calculation of robot dynamics using articulated-body inertias. The International Journal of Robotics Research, 2(1), pp.13-30.
- [Featherstone 2008] Featherstone, R., 2008. Rigid body dynamics algorithms. Springer.
- [Jain 2010] Jain, A., 2010. Robot and multibody dynamics: analysis and algorithms. Springer Science & Business Media.
- Template Parameters
-
|
| ArticulatedBodyInertia ()=default |
| Default ArticulatedBodyInertia constructor initializes all matrix values to NaN for a quick detection of uninitialized values. More...
|
|
| ArticulatedBodyInertia (const SpatialInertia< T > &M_SQ_E) |
| Constructs an articulated body inertia for an articulated body consisting of a single rigid body given its spatial inertia. More...
|
|
template<typename Derived > |
| ArticulatedBodyInertia (const Eigen::MatrixBase< Derived > &matrix) |
| Constructs an articulated body inertia from an input matrix. More...
|
|
template<typename Scalar > |
ArticulatedBodyInertia< Scalar > | cast () const |
| Returns a new ArticulatedBodyInertia object templated on Scalar with casted values of this articulated body inertia. More...
|
|
template<typename T1 = T> |
std::enable_if_t< scalar_predicate< T1 >::is_bool, bool > | IsPhysicallyValid () const |
| Performs a number of checks to verify that this is a physically valid articulated body inertia. More...
|
|
template<typename T1 = T> |
std::enable_if_t<!scalar_predicate< T1 >::is_bool, bool > | IsPhysicallyValid () const |
| IsPhysicallyValid() for non-numeric scalar types is not supported. More...
|
|
Matrix6< T > | CopyToFullMatrix6 () const |
| Copy to a full 6x6 matrix representation. More...
|
|
void | ShiftInPlace (const Vector3< T > &p_QR_E) |
| Given this articulated body inertia P_AQ_E for some articulated body A, computed about point Q, and expressed in frame E, this method uses the rigid body shift operator to compute the same articulated body inertia about a new point R. More...
|
|
ArticulatedBodyInertia< T > | Shift (const Vector3< T > &p_QR_E) const |
| Given this articulated body inertia P_AQ_E for some articulated body A, computed about point Q, and expressed in frame E, this method uses the rigid body shift operator to compute the same articulated body inertia about a new point R. More...
|
|
ArticulatedBodyInertia< T > & | operator+= (const ArticulatedBodyInertia< T > &P_BQ_E) |
| Adds in to this articulated body inertia P_AQ_E for an articulated body A about a point Q and expressed in a frame E the articulated body inertia P_BQ_E for a second articulated body B about the same point Q and expressed in the same frame E. More...
|
|
ArticulatedBodyInertia< T > & | operator-= (const ArticulatedBodyInertia< T > &P_BQ_E) |
| Subtracts P_BQ_E from this articulated body inertia. More...
|
|
template<typename OtherDerived > |
const Eigen::Product< Eigen::SelfAdjointView< const Matrix6< T >, Eigen::Lower >, OtherDerived > | operator * (const Eigen::MatrixBase< OtherDerived > &rhs) const |
| Multiplies this articulated body inertia on the right by a matrix or vector. More...
|
|
SpatialForce< T > | operator * (const SpatialAcceleration< T > &A_WB_E) const |
| Multiplies this articulated body inertia on the right by a spatial acceleration. More...
|
|
|
| ArticulatedBodyInertia (const ArticulatedBodyInertia &)=default |
|
ArticulatedBodyInertia & | operator= (const ArticulatedBodyInertia &)=default |
|
| ArticulatedBodyInertia (ArticulatedBodyInertia &&)=default |
|
ArticulatedBodyInertia & | operator= (ArticulatedBodyInertia &&)=default |
|
void ShiftInPlace |
( |
const Vector3< T > & |
p_QR_E | ) |
|
Given this
articulated body inertia P_AQ_E
for some articulated body A, computed about point Q, and expressed in frame E, this method uses the rigid body shift operator to compute the same articulated body inertia about a new point R.
The result still is expressed in frame E.
Mathematically, this is equivalent to:
P_AR_E = Φ(P_RQ_E) P_AQ_E Φ(p_RQ_E)ᵀ
where Φ(p_RQ_E)
is the rigid body shift operator as defined by [Jain 2010]. The definition of Φ(p_RQ_E)
uses p_QR_E×
, which is the skew-symmetric cross product matrix (defined such that a× b = a.cross(b)
).
Φ(p_RQ_E) =
| I₃ p_RQ_E× |
| 0 I₃ |
where p_RQ_E× = -p_QR_E×
.
This operation is performed in-place modifying the original object.
- See also
- Shift() which does not modify this object.
For details see Section 6.2.5, Page 105 of [Jain 2010].
- Parameters
-
[in] | p_QR_E | Vector from the original about point Q to the new about point R, expressed in the same frame E this articulated body inertia is expressed in. |