Drake

This class is used to represent the spatial momentum of a particle, system of particles or body (whether rigid or soft.) The linear momentum l_NS
of a system of particles S in a reference frame N is defined by:
l_NS = ∑l_NQi = ∑mᵢv_NQi
where mᵢ
and v_NQi
are the mass and linear velocity (in frame N) of the ith particle in the system, respectively. Their product l_NQi = mᵢv_NQi
is the linear momentum of the ith particle in the N reference frame. The angular momentum h_NSp
of a system of particles S in a reference frame N about an arbitrary point P is defined by:
h_NSp = ∑ p_PQi x l_NQi
where p_PQi
is the position vector from point P to the ith particle position Qi
. The definitions above extend to a continuum of particles as:
h_NSp = ∫p_PQ(r) x v_NQ(r) ρ(r)d³r l_NS = ∫v_NQ(r) ρ(r)d³r
where ρ(r)
is the density of the body at each material location r
. In particular, the continuum version above also applies to rigid bodies.
Spatial momenta are elements of F⁶ (see [Featherstone 2008]) that combine both rotational (angular momentum) and translational (linear momentum) components. Spatial momenta are 6element quantities that are pairs of ordinary 3vectors. Elements 02 are the angular momentum component while elements 35 are the linear momentum component. As with any other spatial vector, both vector components must be expressed in the same frame.
Neither the expressedin frame nor the aboutpoint are stored with a SpatialMomentum object; they must be understood from context. It is the responsibility of the user to keep track of the aboutpoint and the expressedin frame. That is best accomplished through disciplined notation. In source code we use monogram notation where L is used to designate a spatial momentum quantity. We write a point P fixed to body (or frame) B as \(B_P\) which appears in code and comments as Bp
. Then we write as \([^NL^{S/B_P}]_E\), which appears in code as L_NBp_E
, the spatial momentum of a body B in a reference frame N, about a point P and, expressed in frame E. Very often the aboutpoint will be the body origin Bo
; if no point is shown the origin is understood, thus L_NB_E
means L_NBo_E
. For a more detailed introduction on spatial vectors and the monogram notation please refer to section Spatial Vectors.
T  The underlying scalar type. Must be a valid Eigen scalar. 
#include <drake/multibody/math/spatial_velocity.h>
Public Member Functions  
SpatialMomentum ()  
Default constructor. More...  
SpatialMomentum (const Eigen::Ref< const Vector3< T >> &h, const Eigen::Ref< const Vector3< T >> &l)  
SpatialMomentum constructor from an angular momentum h and a linear momentum l. More...  
template<typename Derived >  
SpatialMomentum (const Eigen::MatrixBase< Derived > &L)  
SpatialMomentum constructor from an Eigen expression that represents a sixdimensional vector. More...  
SpatialMomentum< T > &  ShiftInPlace (const Vector3< T > &p_PQ_E) 
Inplace shift of a SpatialMomentum from one "aboutpoint" to another. More...  
SpatialMomentum< T >  Shift (const Vector3< T > &p_PQ_E) const 
Shift of a SpatialMomentum from one application point to another. More...  
SpatialMomentum< T > &  operator+= (const SpatialMomentum< T > &L_NSp_E) 
Adds in a spatial momentum to this spatial momentum. More...  
SpatialMomentum< T > &  operator= (const SpatialMomentum< T > &L_NSp_E) 
Subtracts a spatial momentum from this spatial momentum. More...  
T  dot (const SpatialVelocity< T > &V_NBp_E) const 
Given this spatial momentum L_NBp_E of a rigid body B, about point P and, expressed in a frame E, this method computes the dot product with the spatial velocity V_NBp_E of body B frame shifted to point P, measured in an inertial (or Newtonian) frame N and expressed in the same frame E in which the spatial momentum is expressed. More...  
Implements CopyConstructible, CopyAssignable, MoveConstructible, MoveAssignable  
SpatialMomentum (const SpatialMomentum &)=default  
SpatialMomentum &  operator= (const SpatialMomentum &)=default 
SpatialMomentum (SpatialMomentum &&)=default  
SpatialMomentum &  operator= (SpatialMomentum &&)=default 
Public Member Functions inherited from SpatialVector< SpatialMomentum, T >  
SpatialVector ()  
Default constructor. More...  
SpatialVector (const Eigen::Ref< const Vector3< T >> &w, const Eigen::Ref< const Vector3< T >> &v)  
SpatialVector constructor from an rotational component w and a linear component v . More...  
SpatialVector (const Eigen::MatrixBase< OtherDerived > &V)  
SpatialVector constructor from an Eigen expression that represents a sixdimensional vector. More...  
int  size () const 
The total size of the concatenation of the angular and linear components. More...  
const T &  operator[] (int i) const 
Const access to the ith component of this spatial vector. More...  
T &  operator[] (int i) 
Mutable access to the ith component of this spatial vector. More...  
const Vector3< T > &  rotational () const 
Const access to the rotational component of this spatial vector. More...  
Vector3< T > &  rotational () 
Mutable access to the rotational component of this spatial vector. More...  
const Vector3< T > &  translational () const 
Const access to the translational component of this spatial vector. More...  
Vector3< T > &  translational () 
Mutable access to the translational component of this spatial vector. More...  
const T *  data () const 
Returns a (const) bare pointer to the underlying data. More...  
T *  mutable_data () 
Returns a (mutable) bare pointer to the underlying data. More...  
std::tuple< const T, const T >  GetMaximumAbsoluteDifferences (const SpatialQuantity &other) const 
Returns the maximum absolute values of the differences in the rotational and translational components of this and other (i.e., the infinity norms of the difference in rotational and translational components). More...  
decltype(T()< T())  IsNearlyEqualWithinAbsoluteTolerance (const SpatialQuantity &other, double rotational_tolerance, double translational_tolerance) const 
Compares the rotational and translational parts of this and other to check if they are the same to within specified absolute differences. More...  
decltype(T()< T())  IsApprox (const SpatialQuantity &other, double tolerance=std::numeric_limits< double >::epsilon()) const 
Compares this spatial vector to the provided spatial vector other within a specified tolerance. More...  
void  SetNaN () 
Sets all entries in this SpatialVector to NaN. More...  
SpatialQuantity &  SetZero () 
Sets both rotational and translational components of this SpatialVector to zero. More...  
CoeffsEigenType &  get_coeffs () 
Returns a reference to the underlying storage. More...  
const CoeffsEigenType &  get_coeffs () const 
Returns a constant reference to the underlying storage. More...  
SpatialQuantity  operator () const 
Unary minus operator. More...  
SpatialVector (const SpatialVector &)=default  
SpatialVector (SpatialVector &&)=default  
SpatialVector &  operator= (const SpatialVector &)=default 
SpatialVector &  operator= (SpatialVector &&)=default 
Additional Inherited Members  
Public Types inherited from SpatialVector< SpatialMomentum, T >  
enum  
Sizes for spatial quantities and its components in three dimensions. More...  
using  SpatialQuantity = SpatialMomentum< T > 
The more specialized spatial vector class templated on the scalar type T. More...  
typedef Vector6< T >  CoeffsEigenType 
The type of the underlying inmemory representation using an Eigen vector. More...  
typedef T  ScalarType 
Static Public Member Functions inherited from SpatialVector< SpatialMomentum, T >  
static SpatialQuantity  Zero () 
Factory to create a zero SpatialVector, i.e. More...  
Related Functions inherited from SpatialVector< SpatialMomentum, T >  
std::ostream &  operator<< (std::ostream &o, const SpatialVector< SpatialQuantity, T > &V) 
Stream insertion operator to write SpatialVector objects into a std::ostream . More...  

default 

default 
SpatialMomentum  (  ) 
Default constructor.
In Release builds the elements of the newly constructed spatial momentum are left uninitialized resulting in a zero cost operation. However in Debug builds those entries are set to NaN so that operations using this uninitialized spatial momentum fail fast, allowing fast bug detection.
SpatialMomentum  (  const Eigen::Ref< const Vector3< T >> &  h, 
const Eigen::Ref< const Vector3< T >> &  l  
) 
SpatialMomentum constructor from an angular momentum h and a linear momentum l.

explicit 
SpatialMomentum constructor from an Eigen expression that represents a sixdimensional vector.
This constructor will assert the size of L is six (6) at compiletime for fixed sized Eigen expressions and at runtime for dynamic sized Eigen expressions.
T dot  (  const SpatialVelocity< T > &  V_NBp_E  )  const 
Given this
spatial momentum L_NBp_E
of a rigid body B, about point P and, expressed in a frame E, this method computes the dot product with the spatial velocity V_NBp_E
of body B frame shifted to point P, measured in an inertial (or Newtonian) frame N and expressed in the same frame E in which the spatial momentum is expressed.
This dotproduct is twice the kinetic energy ke_NB
of body B in reference frame N. The kinetic energy ke_NB
is independent of the aboutpoint P and so is this dot product. Therefore it is always true that:
ke_NB = 1/2 (L_NBp⋅V_NBp) = 1/2 (L_NBcm⋅V_NBcm)
where L_NBcm
is the spatial momentum about the center of mass of body B and V_NBcm
is the spatial velocity of frame B shifted to its center of mass. The above is true due to how spatial momentum and velocity shift when changing point P, see SpatialMomentum::Shift() and SpatialVelocity::Shift().
SpatialMomentum<T>& operator+=  (  const SpatialMomentum< T > &  L_NSp_E  ) 
Adds in a spatial momentum to this
spatial momentum.
[in]  L_NSp_E  A spatial momentum to be added to this spatial momentum. It must be about the same point P as this spatial momentum and expressed in the same frame E. 
this
spatial momentum, which has been updated to include the given spatial momentum L_NSp_E
.SpatialMomentum<T>& operator=  (  const SpatialMomentum< T > &  L_NSp_E  ) 
Subtracts a spatial momentum from this
spatial momentum.
[in]  L_NSp_E  A spatial momentum to be subtracted from this spatial momentum. It must be about the same point P as this spatial momentum and expressed in the same frame E. 
this
spatial momentum, which has been updated to exclude the given spatial momentum L_NSp_E
.

default 

default 
SpatialMomentum<T> Shift  (  const Vector3< T > &  p_PQ_E  )  const 
Shift of a SpatialMomentum from one application point to another.
This is an alternate signature for shifting a spatial momentum's aboutpoint that does not change the original object. See ShiftInPlace() for more information.
[in]  p_PQ_E  Shift vector from point P to point Q. 
L_NSq_E  The equivalent shifted spatial momentum, now applied at point Q rather than P. 
SpatialMomentum<T>& ShiftInPlace  (  const Vector3< T > &  p_PQ_E  ) 
Inplace shift of a SpatialMomentum from one "aboutpoint" to another.
this
spatial momentum L_NSp_E
for a system S in a reference frame N about a point P, and expressed in frame E, is modified to become the equivalent spatial momentum L_NSq_E
of the same system about another point Q.
We are given the vector from point P to point Q, as a position vector p_PQ_E
expressed in the same frame E as the spatial momentum. The operation performed, in coordinatefree form, is:
h_NSq = h_NSp  p_PQ x l_NSp l_NSq = l_NSp, i.e. the linear momentum about point Q is the same as the linear momentum about point P.
where h and l represent the angular and linear momentum components respectively. Notice that spatial momenta shift in the same way as spatial forces (see SpatialForce.)
The operation is linear, which [Jain 2010], (§2.1, page 22) writes using the "rigid body transformation operator" as:
L_NSq = Φ(p_QP)L_NSp = Φ(p_PQ)L_NSp
where Φ(p_PQ)
is the linear operator:
Φ(p_PQ) =  I₃ p_PQx   0 I₃ 
where p_PQx
denotes the cross product, skewsymmetric, matrix such that p_PQx v = p_PQ x v
. This same operator shifts spatial forces in analogous way (see SpatialForce::Shift()) while the transpose of this operator allow us to shift spatial velocities, see SpatialVelocity::Shift().
For computation, all quantities above must be expressed in a common frame E; we add an _E
suffix to each symbol to indicate that.
This operation is performed inplace modifying the original object.
[in]  p_PQ_E  Shift vector from point P to point Q, expressed in frame E. 
this
spatial momentum which is now L_NSq_E
, that is, the spatial momentum about point Q rather than P.