|
Drake
|
|
Drake
|
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.
| T | The scalar type, which must be one of the default scalars. |
#include <drake/multibody/tree/articulated_body_inertia.h>
Public Member Functions | |
| ArticulatedBodyInertia ()=default | |
| Default ArticulatedBodyInertia constructor initializes all matrix values to NaN for a quick detection of uninitialized values. | |
| 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. | |
| template<typename Derived> | |
| ArticulatedBodyInertia (const Eigen::MatrixBase< Derived > &matrix) | |
| Constructs an articulated body inertia from an input matrix. | |
| template<typename Scalar> | |
| ArticulatedBodyInertia< Scalar > | cast () const |
| Returns a new ArticulatedBodyInertia object templated on Scalar with casted values of this articulated body inertia. | |
| 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. | |
| 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. | |
| Matrix6< T > | CopyToFullMatrix6 () const |
| Copy to a full 6x6 matrix representation. | |
| 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. | |
| 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. | |
| 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. | |
| ArticulatedBodyInertia< T > & | operator-= (const ArticulatedBodyInertia< T > &P_BQ_E) |
| Subtracts P_BQ_E from this articulated body inertia. | |
| 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. | |
| SpatialForce< T > | operator* (const SpatialAcceleration< T > &A_WB_E) const |
| Multiplies this articulated body inertia on the right by a spatial acceleration. | |
Implements CopyConstructible, CopyAssignable, MoveConstructible, MoveAssignable | |
| ArticulatedBodyInertia (const ArticulatedBodyInertia &)=default | |
| ArticulatedBodyInertia & | operator= (const ArticulatedBodyInertia &)=default |
| ArticulatedBodyInertia (ArticulatedBodyInertia &&)=default | |
| ArticulatedBodyInertia & | operator= (ArticulatedBodyInertia &&)=default |
Friends | |
| template<typename> | |
| class | ArticulatedBodyInertia |
| template<typename OtherDerived> | |
| const Eigen::Product< OtherDerived, Eigen::SelfAdjointView< const Matrix6< T >, Eigen::Lower > > | operator* (const Eigen::MatrixBase< OtherDerived > &lhs, const ArticulatedBodyInertia &rhs) |
| Multiplies this articulated body inertia on the left by a matrix or vector. | |
|
default |
|
default |
|
default |
Default ArticulatedBodyInertia constructor initializes all matrix values to NaN for a quick detection of uninitialized values.
|
explicit |
Constructs an articulated body inertia for an articulated body consisting of a single rigid body given its spatial inertia.
From an input spatial inertia M_SQ_E for a body or composite body S, about point Q, and expressed in a frame E, this constructor creates an articulated body inertia about the same point Q and expressed in the same frame E.
| [in] | M_SQ_E | The spatial inertia of a body or composite body S about point Q and expressed in frame E. |
|
explicit |
Constructs an articulated body inertia from an input matrix.
In Debug, this constructor checks for the physical validity of the resulting ArticulatedBodyInertia with IsPhysicallyValid() and throws a std::runtime_error exception in the event the provided input matrix leads to a non-physically viable articulated body inertia.
| [in] | matrix | A matrix or matrix expression representing the articulated body inertia. Only the lower triangular region is used and the strictly upper part is ignored. |
| std::exception | in Debug builds if IsPhysicallyValid() for this inertia is false. |
Returns a new ArticulatedBodyInertia object templated on Scalar with casted values of this articulated body inertia.
| Scalar | The scalar type on which the new articulated body inertia will be templated. |
| Matrix6< T > CopyToFullMatrix6 | ( | ) | const |
Copy to a full 6x6 matrix representation.
| 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.
The checks performed are:
| std::enable_if_t<!scalar_predicate< T1 >::is_bool, bool > IsPhysicallyValid | ( | ) | const |
IsPhysicallyValid() for non-numeric scalar types is not supported.
| 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.
| SpatialForce< T > operator* | ( | const SpatialAcceleration< T > & | A_WB_E | ) | const |
Multiplies this articulated body inertia on the right by a spatial acceleration.
See Eq. (2) for an example.
| 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.
The result is equivalent to the articulated body inertia P_CQ_E for the composite articulated body C which has at its base a rigid body composed of the bases of A and B welded together [Featherstone 2008, example 7.1]. The composite articulated body inertia P_CQ_E is also about the same point Q and expressed in the same frame E as the addends.
| [in] | P_BQ_E | An articulated body inertia of some articulated body B to be added to this articulated body inertia. It must be defined about the same point Q as this inertia, and expressed in the same frame E. |
| ArticulatedBodyInertia< T > & operator-= | ( | const ArticulatedBodyInertia< T > & | P_BQ_E | ) |
Subtracts P_BQ_E from this articulated body inertia.
P_BQ_E must be for the same articulated body B as this ABI (about the same point Q and expressed in the same frame E). The resulting inertia will have the same properties.
|
default |
|
default |
| 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.
The result still is expressed in frame E.
| [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. |
| P_AR_E | This same articulated body inertia for articulated body A but now computed about a new point R. |
| 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.
For details see Section 6.2.5, Page 105 of [Jain 2010].
| [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. |
|
friend |
Multiplies this articulated body inertia on the left by a matrix or vector.