Drake
SpatialForce< T > Class Template Reference

This class is used to represent a spatial force (also called a wrench) that combines both rotational (torque) and translational force components. More...

#include <multibody/multibody_tree/math/spatial_force.h>

Inheritance diagram for SpatialForce< T >:
[legend]
Collaboration diagram for SpatialForce< T >:
[legend]

Public Member Functions

 SpatialForce ()
 Default constructor. More...
 
 SpatialForce (const Eigen::Ref< const Vector3< T >> &tau, const Eigen::Ref< const Vector3< T >> &f)
 SpatialForce constructor from a torque tau and a force f. More...
 
template<typename Derived >
 SpatialForce (const Eigen::MatrixBase< Derived > &V)
 SpatialForce constructor from an Eigen expression that represents a six-dimensional vector. More...
 
SpatialForce< T > & ShiftInPlace (const Vector3< T > &p_BpBq_E)
 In-place shift of a SpatialForce from one application point to another. More...
 
SpatialForce< T > Shift (const Vector3< T > &p_BpBq_E) const
 Shift of a SpatialForce from one application point to another. More...
 
SpatialForce< T > & operator+= (const SpatialForce< T > &F_Sp_E)
 Adds in a spatial force to this spatial force. More...
 
SpatialForce< T > & operator-= (const SpatialForce< T > &F_Sp_E)
 Subtracts a spatial force from this spatial force. More...
 
dot (const SpatialVelocity< T > &V_IBp_E) const
 Given this spatial force F_Bp_E applied at point P of body B and expressed in a frame E, this method computes the 6-dimensional dot product with the spatial velocity V_IBp_E of body B at point P, measured in an inertial frame I and expressed in the same frame E in which the spatial force is expressed. More...
 
Implements CopyConstructible, CopyAssignable, MoveConstructible, MoveAssignable
 SpatialForce (const SpatialForce &)=default
 
SpatialForceoperator= (const SpatialForce &)=default
 
 SpatialForce (SpatialForce &&)=default
 
SpatialForceoperator= (SpatialForce &&)=default
 
- Public Member Functions inherited from SpatialVector< SpatialForce, 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 six-dimensional 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 i-th component of this spatial vector. More...
 
T & operator[] (int i)
 Mutable access to the i-th 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...
 
bool IsNearlyEqualWithinAbsoluteTolerance (const SpatialQuantity &other, const T &rotational_tolerance, const T &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...
 
bool IsApprox (const SpatialQuantity &other, double tolerance=Eigen::NumTraits< T >::epsilon()) const
 Compares this spatial vector to the provided spatial vector other within a specified precision. More...
 
void SetNaN ()
 Sets all entries in this SpatialVector to NaN. More...
 
SpatialQuantitySetZero ()
 Sets both rotational and translational components of this SpatialVector to zero. More...
 
CoeffsEigenTypeget_coeffs ()
 Returns a reference to the underlying storage. More...
 
const CoeffsEigenTypeget_coeffs () const
 Returns a constant reference to the underlying storage. More...
 
 SpatialVector (const SpatialVector &)=default
 
 SpatialVector (SpatialVector &&)=default
 
SpatialVectoroperator= (const SpatialVector &)=default
 
SpatialVectoroperator= (SpatialVector &&)=default
 

Additional Inherited Members

- Public Types inherited from SpatialVector< SpatialForce, T >
enum  
 Sizes for spatial quantities and its components in three dimensions. More...
 
using SpatialQuantity = SpatialForce< T >
 The more specialized spatial vector class templated on the scalar type T. More...
 
typedef Vector6< T > CoeffsEigenType
 The type of the underlying in-memory representation using an Eigen vector. More...
 
typedef T ScalarType
 
- Static Public Member Functions inherited from SpatialVector< SpatialForce, T >
static SpatialQuantity Zero ()
 Factory to create a zero SpatialVector, i.e. More...
 

Detailed Description

template<typename T>
class drake::multibody::SpatialForce< T >

This class is used to represent a spatial force (also called a wrench) that combines both rotational (torque) and translational force components.

Spatial forces are 6-element quantities that are pairs of ordinary 3-vectors. Elements 0-2 are the torque component while elements 3-5 are the force component. Both vectors must be expressed in the same frame, and the translational force is applied to a particular point of a body, but neither the frame nor the point are stored with a SpatialForce object; they must be understood from context. It is the responsibility of the user to keep track of the application point and the expressed-in frame. That is best accomplished through disciplined notation. In source code we use monogram notation where capital F is used to designate a spatial force 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 a particular spatial force as F_Bp_E where the _E suffix indicates that the expressed-in frame is E. This symbol represents a torque applied to body B, and a force applied to point P on B, with both vectors expressed in E. Very often the application point will be the body origin Bo; if no point is shown the origin is understood, so F_B_E means F_Bo_E. For a more detailed introduction on spatial vectors and the monogram notation please refer to section Spatial Vectors.

Template Parameters
TThe underlying scalar type. Must be a valid Eigen scalar.

Constructor & Destructor Documentation

SpatialForce ( const SpatialForce< T > &  )
default
SpatialForce ( SpatialForce< T > &&  )
default
SpatialForce ( )
inline

Default constructor.

In Release builds the elements of the newly constructed spatial force 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 force fail fast, allowing fast bug detection.

SpatialForce ( const Eigen::Ref< const Vector3< T >> &  tau,
const Eigen::Ref< const Vector3< T >> &  f 
)
inline

SpatialForce constructor from a torque tau and a force f.

SpatialForce ( const Eigen::MatrixBase< Derived > &  V)
inlineexplicit

SpatialForce constructor from an Eigen expression that represents a six-dimensional vector.

This constructor will assert the size of V is six (6) at compile-time for fixed sized Eigen expressions and at run-time for dynamic sized Eigen expressions.

Member Function Documentation

T dot ( const SpatialVelocity< T > &  V_IBp_E) const

Given this spatial force F_Bp_E applied at point P of body B and expressed in a frame E, this method computes the 6-dimensional dot product with the spatial velocity V_IBp_E of body B at point P, measured in an inertial frame I and expressed in the same frame E in which the spatial force is expressed.

This dot-product represents the power generated by this spatial force when its body and application point have the given spatial velocity. Although the two spatial vectors must be expressed in the same frame, the result is independent of that frame.

Warning
The result of this method cannot be interpreted as power unless the spatial velocity is measured in an inertial frame I.

Here is the call graph for this function:

Here is the caller graph for this function:

SpatialForce<T>& operator+= ( const SpatialForce< T > &  F_Sp_E)
inline

Adds in a spatial force to this spatial force.

Parameters
[in]F_Sp_EA spatial force to be added to this spatial force. It must be on the same system or body S on which this spatial force is applied and at the same point P as this spatial force, and expressed in the same frame E.
Returns
A reference to this spatial force, which has been updated to include the given spatial force F_Sp_E.
Warning
This operation is only valid if both spatial forces are applied on the same system or body S, at the same point P and expressed in the same frame E.

Here is the call graph for this function:

SpatialForce<T>& operator-= ( const SpatialForce< T > &  F_Sp_E)
inline

Subtracts a spatial force from this spatial force.

Parameters
[in]F_Sp_EA spatial force to be subtracted from this spatial force. It must be on the same system or body S on which this spatial force is applied and at the same point P as this spatial force, and expressed in the same frame E.
Returns
A reference to this spatial force, which has been updated to exclude the given spatial force F_Sp_E.
Warning
This operation is only valid if both spatial forces are applied on the same system or body S, at the same point P and expressed in the same frame E.

Here is the call graph for this function:

SpatialForce& operator= ( SpatialForce< T > &&  )
default
SpatialForce& operator= ( const SpatialForce< T > &  )
default
SpatialForce<T> Shift ( const Vector3< T > &  p_BpBq_E) const
inline

Shift of a SpatialForce from one application point to another.

This is an alternate signature for shifting a spatial force's application point that does not change the original object. See ShiftInPlace() for more information.

Parameters
[in]p_BpBq_EShift vector from point P of body B to point Q of B, expressed in frame E. The "from" point Bp must be the current application point of this spatial force, and E must be the same expressed-in frame as for this spatial force.
Return values
F_Bq_EThe equivalent shifted spatial force, now applied at point Q rather than P.
See also
ShiftInPlace() to compute the shifted spatial force in-place modifying the original object.

Here is the call graph for this function:

Here is the caller graph for this function:

SpatialForce<T>& ShiftInPlace ( const Vector3< T > &  p_BpBq_E)
inline

In-place shift of a SpatialForce from one application point to another.

this spatial force F_Bp_E, which applies its translational force component to point P of body B, is modified to become the equivalent spatial force F_Bq_E that considers the force to be applied to point Q of body B instead (see class comment for more about this notation). This requires adjusting the torque component to account for the change in moment caused by the force shift.

We are given the vector from point P to point Q, as a position vector p_BpBq_E (or p_PQ_E) expressed in the same frame E as the spatial force. The operation performed, in coordinate-free form, is:

  τ_B  = τ_B -  p_BpBq x f_Bp
  f_Bq = f_Bp,  i.e. the force as applied to body B at Q is the
                same as was applied to B at P.

where τ and f represent the torque and force components respectively.

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 in-place modifying the original object.

Parameters
[in]p_BpBq_EShift vector from point P of body B to point Q of B, expressed in frame E. The "from" point Bp must be the current application point of this spatial force, and E must be the same expressed-in frame as for this spatial force.
Returns
A reference to this spatial force which is now F_Bq_E, that is, the force is now applied at point Q rather than P.
See also
Shift() to compute the shifted spatial force without modifying this original object.

Here is the call graph for this function:

Here is the caller graph for this function:


The documentation for this class was generated from the following files: