Drake

This class is used to represent a spatial acceleration that combines rotational (angular acceleration) and translational (linear acceleration) components.
While a SpatialVelocity V_XY
represents the motion of a "moving frame" Y measured with respect to a "measuredin" frame X, the SpatialAcceleration A_XY
represents the rate of change of this spatial velocity V_XY
in frame X. That is \(^XA^Y = \frac{^Xd}{dt}\,{^XV^Y} \) where \(\frac{^Xd}{dt} \) denotes the time derivative taken in frame X. That is, to compute an acceleration we need to specify in what frame the time derivative is taken, see [Mitiguy 2016, §6.1] for a more in depth discussion on this. Time derivatives can be taken in different frames, and they transform according to the "Transport Theorem", which is in Drake is implemented in drake::math::ConvertTimeDerivativeToOtherFrame(). In source code comments we write A_XY = DtX(V_XY)
, where DtX()
is the operator that takes the time derivative in the X frame. By convention, and unless otherwise stated, we assume that the frame in which the time derivative is taken is the "measuredin" frame, i.e. the time derivative used in A_XY
is in frame X by default (i.e. DtX()). To perform numerical computations, we need to specify an "expressedin" frame E (which may be distinct from either X or Y), so that components can be expressed as real numbers. Only the vector values are stored in a SpatialAcceleration object; the frames must be understood from context and it is the responsibility of the user to keep track of them. That is best accomplished through disciplined notation. In source code we use monogram notation where capital A is used to designate a spatial acceleration quantity. The same monogram notation rules for SpatialVelocity are also used for SpatialAcceleration. That is, the spatial acceleration of a frame Y measured in X and expressed in E is denoted with A_XY_E
. For a more detailed introduction on spatial vectors and the monogram notation please refer to section Spatial Vectors.
[Mitiguy 2016] Mitiguy, P., 2016. Advanced Dynamics & Motion Simulation.
T  The underlying scalar type. Must be a valid Eigen scalar. 
#include <drake/multibody/math/spatial_acceleration.h>
Public Member Functions  
SpatialAcceleration ()  
Default constructor. More...  
SpatialAcceleration (const Eigen::Ref< const Vector3< T >> &alpha, const Eigen::Ref< const Vector3< T >> &a)  
SpatialAcceleration constructor from an angular acceleration alpha and a linear acceleration a . More...  
template<typename Derived >  
SpatialAcceleration (const Eigen::MatrixBase< Derived > &A)  
SpatialAcceleration constructor from an Eigen expression that represents a sixdimensional vector. More...  
SpatialAcceleration< T > &  ShiftInPlace (const Vector3< T > &p_PoQ_E, const Vector3< T > &w_WP_E) 
Inplace shift of this spatial acceleration A_WP of a frame P into the spatial acceleration A_WPq of a frame Pq which is an offset frame rigidly aligned with P, but with its origin shifted to a point Q by an offset p_PoQ . More...  
SpatialAcceleration< T >  Shift (const Vector3< T > &p_PoQ_E, const Vector3< T > &w_WP_E) const 
Shifts this spatial acceleration A_WP of a frame P into the spatial acceleration A_WPq of a frame Pq which is an offset frame rigidly aligned with P, but with its origin shifted to a point Q by an offset p_PoQ. More...  
SpatialAcceleration< T >  Shift (const Vector3< T > &p_PoQ_E) const 
(Advanced) Given this spatial acceleration A_WP of a frame P in a second frame W, this operation is only valid when the angular velocity w_WP of P in W is zero. More...  
SpatialAcceleration< T >  ComposeWithMovingFrameAcceleration (const Vector3< T > &p_PoQ_E, const Vector3< T > &w_WP_E, const SpatialVelocity< T > &V_PB_E, const SpatialAcceleration< T > &A_PB_E) const 
This method composes this spatial acceleration A_WP of a frame P measured in a frame W, with that of a third frame B moving in P with spatial acceleration A_PB . More...  
Implements CopyConstructible, CopyAssignable, MoveConstructible, MoveAssignable  
SpatialAcceleration (const SpatialAcceleration &)=default  
SpatialAcceleration &  operator= (const SpatialAcceleration &)=default 
SpatialAcceleration (SpatialAcceleration &&)=default  
SpatialAcceleration &  operator= (SpatialAcceleration &&)=default 
Public Member Functions inherited from SpatialVector< SpatialAcceleration, 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< SpatialAcceleration, T >  
enum  
Sizes for spatial quantities and its components in three dimensions. More...  
using  SpatialQuantity = SpatialAcceleration< 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< SpatialAcceleration, T >  
static SpatialQuantity  Zero () 
Factory to create a zero SpatialVector, i.e. More...  
Related Functions inherited from SpatialVector< SpatialAcceleration, 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 
Default constructor.
In Release builds the elements of the newly constructed spatial acceleration 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 acceleration fail fast, allowing fast bug detection.
SpatialAcceleration  (  const Eigen::Ref< const Vector3< T >> &  alpha, 
const Eigen::Ref< const Vector3< T >> &  a  
) 
SpatialAcceleration constructor from an angular acceleration alpha
and a linear acceleration a
.

explicit 
SpatialAcceleration constructor from an Eigen expression that represents a sixdimensional vector.
Under the hood, spatial accelerations are 6element quantities that are pairs of ordinary 3vectors. Elements 02 constitute the angular acceleration component while elements 35 constitute the translational acceleration. The argument A
in this constructor is the concatenation of the rotational 3D component followed by the translational 3D component. This constructor will assert the size of A
is six (6) at compiletime for fixed sized Eigen expressions and at runtime for dynamic sized Eigen expressions.
SpatialAcceleration<T> ComposeWithMovingFrameAcceleration  (  const Vector3< T > &  p_PoQ_E, 
const Vector3< T > &  w_WP_E,  
const SpatialVelocity< T > &  V_PB_E,  
const SpatialAcceleration< T > &  A_PB_E  
)  const 
This method composes this
spatial acceleration A_WP
of a frame P measured in a frame W, with that of a third frame B moving in P with spatial acceleration A_PB
.
The result is the spatial acceleration A_WB
of frame B measured in W. At the instant in which the accelerations are composed, frame B is located with its origin Q at p_PoQ
from P's origin Po.
This operation can be written in a more compact fom in terms of the rigid shift operator Φᵀ(p_PoQ)
(see SpatialVelocity::Shift()) as:
A_WB = Φᵀ(p_PoQ)A_WP + Ac_WB(w_WP, V_PB) + A_PB_W
where Φᵀ(p_PoQ)A_WP
denotes the application of the rigid shift operation as in SpatialVelocity::Shift() and Ac_WB(w_WP, V_PB)
contains the centrifugal and Coriolis terms:
Ac_WB(w_WP, V_PB) =  w_WP x w_PB_W   w_WP x w_WP x p_PoQ_W + 2 w_WP x v_PB_W  ^^^ ^^^ centrifugal Coriolis
The equation above shows that composing spatial accelerations cannot be simply accomplished by adding A_WP
with A_PB
(this is the reason why this class does not overload the operator+()
and provides this method instead). Moreover, we see that, unlike with angular velocities, angular accelerations cannot be added in order to compose them. That is w_AC = w_AB + w_BC
but alpha_AC ≠ alpha_AB + alpha_BC
due to the cross term w_AC x w_BC
. See the derivation below for more details.
V_PB
and A_PB
are both zero. In other words the results from Shift() equal the results from this method when V_PB
and A_PB
are both zero.[in]  p_PoQ_E  Shift vector from P's origin to B's origin, expressed in frame E. The "from" point Po must be the point whose acceleration is currently represented in this spatial acceleration, and E must be the same expressedin frame as for this spatial acceleration. 
[in]  w_WP_E  Angular velocity of frame P measured in frame A and expressed in frame E. 
[in]  V_PB_E  The spatial velocity of a third frame B in motion with respect to P, expressed in the same frame E as this spatial acceleration. 
[in]  A_PB_E  The spatial acceleration of a third frame B in motion with respect to P, expressed in the same frame E as this spatial acceleration. 
A_WB_E  The spatial acceleration of frame B in W, expressed in frame E. 
The spatial velocity of frame B in W can be obtained by composing V_WP
with V_PB
:
V_WB = V_WPq + V_PB = V_WP.Shift(p_PoQ) + V_PB (1)
This operation can be performed with the SpatialVelocity method ComposeWithMovingFrameVelocity().
The translational velocity v_WQ
of point Q in W corresponds to the translational component in Eq. (1):
v_WQ = v_WPo + w_WP x p_PoQ + v_PQ (2)
Therefore, for the translational acceleration we have:
a_WQ = DtW(v_WQ) = DtW(v_WPo + w_WP x p_PoQ + v_PQ) = DtW(v_WPo) + DtW(w_WP x p_PoQ) + DtW(v_PQ) = a_WPo + DtW(w_WP) x p_PoQ + w_WP x DtW(p_PoQ) + DtW(v_PQ) = a_WPo + alpha_WP x p_PoQ + w_WP x DtW(p_PoQ) + DtW(v_PQ) (3)
with a_WPo = DtW(v_WPo)
and alpha_WP = DtW(w_WP)
by definition. The term DtW(p_PoQ) in Eq. (3) is obtained by converting the vector time derivative from DtW()
to DtP()
, see drake::math::ConvertTimeDerivativeToOtherFrame():
DtW(p_PoQ) = DtP(p_PoQ) + w_WP x p_PoQ = v_PQ + w_WP x p_PoQ (4)
since v_PQ = DtP(p_PoQ)
by definition. Similarly, the term DtW(v_PQ)
in Eq. (3) is also obtained by converting the time derivative from DtW()
to DtP()
:
DtW(v_PQ) = DtP(v_PQ) + w_WP x v_PQ = a_PQ + w_WP x v_PQ (5)
with a_PQ = DtP(v_PQ)
by definition. Using Eqs. (4) and (5) in Eq. (3) yields for the translational acceleration:
a_WQ = a_WPo + alpha_WP x p_PoQ + w_WP x (v_PQ + w_WP x p_PoQ) + a_PQ + w_WP x v_PQ
and finally, by grouping terms together:
a_WQ = a_WPo + alpha_WP x p_PoQ + w_WP x w_WP x p_PoQ + 2 * w_WP x v_PQ + a_PQ (6)
which includes the effect of angular acceleration of P in W alpha_WP x p_PoQ
, the centrifugual acceleration w_WP x w_WP x p_PoQ
, the Coriolis acceleration 2 * w_WP x v_PQ
due to the motion of Q in P and, the additional acceleration of Q in P a_PQ
.
The rotational velocity w_WB
of frame B in W corresponds to the rotational component in Eq. (1):
w_WB = w_WP + w_PB (7)
Therefore, the rotational acceleration of B in W corresponds to:
alpha_WB = DtW(w_WB) = DtW(w_WP) + DtW(w_PB) = alpha_WP + DtW(w_PB) (8)
where the last term in Eq. (8) can be converted to a time derivative in P as:
DtW(w_PB) = DtP(w_PB) + w_WP x w_PB = alpha_PB + w_WP x w_PB (9)
where alpha_PB = DtP(w_PB)
by definition. Thus, the final expression for alpha_WB
is obtained by using Eq. (9) into Eq. (8):
alpha_WB = alpha_WP + alpha_PB + w_WP x w_PB (10)
Equation (10) shows that angular accelerations cannot be simply added as angular velocities can but there exists an additional term w_WP x w_PB
.
The rotational and translational components of the spatial acceleration are given by Eqs. (10) and (6) respectively:
A_WB.rotational() = alpha_WB = {alpha_WP} + alpha_PB + w_WP x w_PB (11) A_WB.translational() = a_WQ = {a_WPo + alpha_WP x p_PoQ + w_WP x w_WP x p_PoQ} + 2 * w_WP x v_PQ + a_PQ (12)
where we have placed within curly brackets {}
all the terms that also appear in the Shift() operation, which is equivalent to this method when V_PB
and A_PB
are both zero. In the equations above alpha_WP = A_WP.rotational()
and a_WPo = A_WP.translational()
. The above expression can be written in a more compact fom in terms of the rigid shift operator Φᵀ(p_PoQ)
(see SpatialVelocity::Shift()) as presented in the main body of this documentation:
A_WB = Φᵀ(p_PoQ)A_WP + Ac_WB(w_WP, V_PB) + A_PB_W (13)
where Ac_WB(w_WP, V_PB)
contains the centrifugal and Coriolis terms:
Ac_WB(w_WP, V_PB) =  w_WP x w_PB_W   w_WP x w_WP x p_PoQ_W + 2 w_WP x v_PB_W  ^^^ ^^^ centrifugal Coriolis
As usual, for computation, all quantities above must be expressed in a common frame E; we add an _E
suffix to each symbol to indicate that.

default 

default 
SpatialAcceleration<T> Shift  (  const Vector3< T > &  p_PoQ_E, 
const Vector3< T > &  w_WP_E  
)  const 
Shifts this
spatial acceleration A_WP
of a frame P into the spatial acceleration A_WPq
of a frame Pq
which is an offset frame rigidly aligned with P, but with its origin shifted to a point Q by an offset p_PoQ.
Frame Pq
is instantaneously moving together with frame P as if rigidly attached to it. As an example of application, this operation can be used to compute A_WPq
where P is a frame on a rigid body and Q is another point on that same body. Therefore P and Pq
move together with the spatial velocity V_PPq
being zero at all times. This is an alternate signature for shifting a spatial acceleration that does not change the original object. See ShiftInPlace() for more information and a description of the arguments.
SpatialAcceleration<T> Shift  (  const Vector3< T > &  p_PoQ_E  )  const 
(Advanced) Given this
spatial acceleration A_WP
of a frame P in a second frame W, this operation is only valid when the angular velocity w_WP
of P in W is zero.
Refer to Shift(const Vector3<T>&, const Vector3<T>&) for the full version that includes velocity terms. This method can be used to avoid unnecessary computation when shifting this
spatial acceleration of a frame P into the spatial acceleration of the shifted frame Pq
. The shift position vector is given by p_PoQ_E
, expresssed in the same frame E as this
spatialacceleration. Mathematically, this returns
A_WPq = Φᵀ(p_PoQ)A_WP, where
Φ(p_PoQ)` is the rigid shift operator, see SpatialVelocity::Shift().
SpatialAcceleration<T>& ShiftInPlace  (  const Vector3< T > &  p_PoQ_E, 
const Vector3< T > &  w_WP_E  
) 
Inplace shift of this
spatial acceleration A_WP
of a frame P into the spatial acceleration A_WPq
of a frame Pq
which is an offset frame rigidly aligned with P, but with its origin shifted to a point Q by an offset p_PoQ
.
Frame Pq
is instantaneously moving together with frame P as if rigidly attached to it. As an example of application, this operation can be used to compute A_WPq
where P is a frame on a rigid body and Q is another point on that same body. Therefore P and Pq
move together with the spatial velocity V_PPq
being zero at all times.
The shift operation modifies this
spatial acceleration A_WP_E
of a frame P measured in a frame W and expressed in a frame E, to become A_WPq_E
, representing the acceleration of a frame Pq
result of shifting frame P to point Q which instantaneously moves together with frame P. This requires adjusting the linear acceleration component to account for:
alpha_WP
of frame P in W.w_WP
of frame P in W.We are given the vector from the origin Po
of frame P to point Q, which becomes the origin of the shifted frame Pq
, as the position vector p_PoQ_E
expressed in the same frame E as this
spatial acceleration. The operation performed, in coordinatefree form, is:
alpha_WPq = alpha_WP, i.e. the angular acceleration is unchanged. a_WQ = a_WPo + alpha_WP x p_PoQ + w_WP x w_WP x p_PoQ
where alpha
and a
represent the angular and linear acceleration components respectively. See notes at the end of this documentation for a detailed derivation.
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_PoQ_E  Shift vector from the origin Po of frame P to point Q, expressed in frame E. The "from" frame P must be the frame whose acceleration is currently represented in this spatial acceleration, and E must be the same expressedin frame as for this spatial acceleration. 
[in]  w_WP_E  Angular velocity of frame P measured in frame W and expressed in frame E. 
this
spatial acceleration which is now A_WPq_E
, that is, the spatial acceleration of frame Pq
, still measured in frame W and expressed in frame E.Recall that frame Pq
is an offset frame rigidly aligned with P, but with its origin shifted to a point Q by an offset p_PoQ
. Frame Pq
is instantaneously moving together with frame P as if rigidly attached to it. The translational velocity v_WPq
of frame Pq
's origin, point Q, in W can be obtained by the shift operation as:
v_WPq = v_WPo + w_WP x p_PoQ (1)
Therefore, for the translational acceleration we have:
a_WQ = DtW(v_WPq) = DtW(v_WPo + w_WP x p_PoQ) = DtW(v_WPo) + DtW(w_WP x p_PoQ) = a_WPo + DtW(w_WP) x p_PoQ + w_WP x DtW(p_PoQ) = a_WPo + alpha_WP x p_PoQ + w_WP x DtW(p_PoQ) (2)
with a_WPo = DtW(v_WPo)
and alpha_WP = DtW(w_WP)
by definition. The last term in Eq. (2) is obtained by converting the vector time derivative from DtW()
to DtP()
, see drake::math::ConvertTimeDerivativeToOtherFrame():
DtW(p_PoQ) = DtP(p_PoQ) + w_WP x p_PoQ = w_WP x p_PoQ (3)
since v_PQ = DtP(p_PoQ) = 0
because the position of point Q is fixed in frame P. Using Eq. (3) in Eq. (2) finally yields for the translational acceleration:
a_WQ = a_WPo + alpha_WP x p_PoQ + w_WP x w_WP x p_PoQ (4)
The rotational velocity of frame Pq
simply equals that of frame P since they are moving together in rigid motion, therefore w_WPq = w_WP
. From this, the rotational acceleration of frame Pq
in W is obtained as:
alpha_WPq = DtW(w_WPq) = DtW(w_WP) = alpha_WP (5)
which should be immediately obvious considering that frame Pq
rotates together with frame P.
With the rotational, Eq. (5), and translational, Eq. (4), components of acceleration derived above, we can write for A_WPq
:
A_WPq.rotational() = alpha_WPq = alpha_WP A_WPq.translational() = a_WQ = a_WPo + alpha_WP x p_PoQ + w_WP x w_WP x p_PoQ
with alpha_WP = A_WP.rotational()
and a_WPo = A_WP.translational()
. As usual, for computation, all quantities above must be expressed in a common frame E; we add an _E
suffix to each symbol to indicate that.