Drake
Drake C++ Documentation
Frame< T > Class Template Referenceabstract

Detailed Description

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

Frame is an abstract class representing a material frame (also called a physical frame) of its underlying RigidBody.

The Frame's origin is a material point of its RigidBody, and its axes have fixed directions in that body. A Frame's pose (position and orientation) with respect to its RigidBodyFrame may be parameterized, but is fixed (not time or state dependent) once parameters have been set.

An important characteristic of a Frame is that forces or torques applied to a Frame are applied to the Frame's underlying RigidBody. Force-producing elements like joints, actuators, and constraints usually employ two Frames, with one Frame connected to one body and the other connected to a different body. Every Frame F can report the RigidBody B to which it is attached and its pose X_BF with respect to B's RigidBodyFrame.

A Frame's pose in World (or relative to other frames) is always calculated starting with its pose relative to its underlying RigidBodyFrame. Subclasses derived from Frame differ in how kinematic calculations are performed. For example, the angular velocity of a FixedOffsetFrame or RigidBodyFrame is identical to the angular velocity of its underlying body, whereas the translational velocity of a FixedOffsetFrame differs from that of a RigidBodyFrame.

Frame provides methods for obtaining its current orientation, position, motion, etc. from a Context passed to those methods.

Template Parameters
TThe scalar type, which must be one of the default scalars.

#include <drake/multibody/tree/multibody_tree.h>

Public Member Functions

 ~Frame () override
 
const RigidBody< T > & body () const
 Returns a const reference to the body associated to this Frame. More...
 
bool is_world_frame () const
 Returns true if this is the world frame. More...
 
bool is_body_frame () const
 Returns true if this is the body frame. More...
 
const std::string & name () const
 Returns the name of this frame. The name will never be empty. More...
 
ScopedName scoped_name () const
 Returns scoped name of this frame. More...
 
const math::RigidTransform< T > & EvalPoseInBodyFrame (const systems::Context< T > &context) const
 Returns a reference to the body-relative pose X_BF giving the pose of this Frame with respect to its body's RigidBodyFrame. More...
 
math::RigidTransform< T > CalcPoseInBodyFrame (const systems::Context< T > &context) const
 Returns the pose X_BF of this frame F in the body frame B associated with this frame. More...
 
math::RotationMatrix< T > CalcRotationMatrixInBodyFrame (const systems::Context< T > &context) const
 Returns the rotation matrix R_BF that relates body frame B to this frame F (B is the body frame to which this frame F is attached). More...
 
virtual math::RigidTransform< T > GetFixedPoseInBodyFrame () const
 Variant of CalcPoseInBodyFrame() that returns the fixed pose X_BF of this frame F in the body frame B associated with this frame. More...
 
virtual math::RotationMatrix< T > GetFixedRotationMatrixInBodyFrame () const
 Returns the rotation matrix R_BF that relates body frame B to this frame F (B is the body frame to which this frame F is attached). More...
 
math::RigidTransform< T > CalcOffsetPoseInBody (const systems::Context< T > &context, const math::RigidTransform< T > &X_FQ) const
 Given the offset pose X_FQ of a frame Q in this frame F, this method computes the pose X_BQ of frame Q in the body frame B to which this frame is attached. More...
 
math::RotationMatrix< T > CalcOffsetRotationMatrixInBody (const systems::Context< T > &context, const math::RotationMatrix< T > &R_FQ) const
 Calculates and returns the rotation matrix R_BQ that relates body frame B to frame Q via this intermediate frame F, i.e., R_BQ = R_BF * R_FQ (B is the body frame to which this frame F is attached). More...
 
virtual math::RigidTransform< T > GetFixedOffsetPoseInBody (const math::RigidTransform< T > &X_FQ) const
 Variant of CalcOffsetPoseInBody() that given the offset pose X_FQ of a frame Q in this frame F, returns the pose X_BQ of frame Q in the body frame B to which this frame is attached. More...
 
virtual math::RotationMatrix< T > GetFixedRotationMatrixInBody (const math::RotationMatrix< T > &R_FQ) const
 Calculates and returns the rotation matrix R_BQ that relates body frame B to frame Q via this intermediate frame F, i.e., R_BQ = R_BF * R_FQ (B is the body frame to which this frame F is attached). More...
 
math::RigidTransform< T > CalcPoseInWorld (const systems::Context< T > &context) const
 Computes and returns the pose X_WF of this frame F in the world frame W as a function of the state of the model stored in context. More...
 
math::RigidTransform< T > CalcPose (const systems::Context< T > &context, const Frame< T > &frame_M) const
 Computes and returns the pose X_MF of this frame F in measured in frame_M as a function of the state of the model stored in context. More...
 
math::RotationMatrix< T > CalcRotationMatrix (const systems::Context< T > &context, const Frame< T > &frame_M) const
 Calculates and returns the rotation matrix R_MF that relates frame_M and this frame F as a function of the state stored in context. More...
 
math::RotationMatrix< T > CalcRotationMatrixInWorld (const systems::Context< T > &context) const
 Calculates and returns the rotation matrix R_WF that relates the world frame W and this frame F as a function of the state stored in context. More...
 
const Vector3< T > & EvalAngularVelocityInWorld (const systems::Context< T > &context) const
 Evaluates this frame F's angular velocity measured and expressed in the world frame W. More...
 
Vector3< T > CalcAngularVelocity (const systems::Context< T > &context, const Frame< T > &measured_in_frame, const Frame< T > &expressed_in_frame) const
 Calculates this frame F's angular velocity measured in a frame M, expressed in a frame E. More...
 
SpatialVelocity< T > CalcSpatialVelocityInWorld (const systems::Context< T > &context) const
 Calculates this frame F's spatial velocity measured and expressed in the world frame W. More...
 
SpatialVelocity< T > CalcSpatialVelocity (const systems::Context< T > &context, const Frame< T > &frame_M, const Frame< T > &frame_E) const
 Calculates this frame F's spatial velocity measured in a frame M, expressed in a frame E. More...
 
SpatialVelocity< T > CalcRelativeSpatialVelocityInWorld (const systems::Context< T > &context, const Frame< T > &other_frame) const
 Calculates this frame C's spatial velocity relative to another frame B, measured and expressed in the world frame W. More...
 
SpatialVelocity< T > CalcRelativeSpatialVelocity (const systems::Context< T > &context, const Frame< T > &other_frame, const Frame< T > &measured_in_frame, const Frame< T > &expressed_in_frame) const
 Calculates this frame C's spatial velocity relative to another frame B, measured in a frame M, expressed in a frame E. More...
 
SpatialAcceleration< T > CalcSpatialAccelerationInWorld (const systems::Context< T > &context) const
 Calculates this frame F's spatial acceleration measured and expressed in the world frame W. More...
 
SpatialAcceleration< T > CalcSpatialAcceleration (const systems::Context< T > &context, const Frame< T > &measured_in_frame, const Frame< T > &expressed_in_frame) const
 Calculates this frame F's spatial acceleration measured in a frame M, expressed in a frame E. More...
 
SpatialAcceleration< T > CalcRelativeSpatialAccelerationInWorld (const systems::Context< T > &context, const Frame< T > &other_frame) const
 Calculates this frame C's spatial acceleration relative to another frame B, measured and expressed in the world frame W. More...
 
SpatialAcceleration< T > CalcRelativeSpatialAcceleration (const systems::Context< T > &context, const Frame< T > &other_frame, const Frame< T > &measured_in_frame, const Frame< T > &expressed_in_frame) const
 Calculates this frame C's spatial acceleration relative to another frame B, measured in a frame M, expressed in a frame E. More...
 
template<typename ToScalar >
std::unique_ptr< Frame< ToScalar > > CloneToScalar (const internal::MultibodyTree< ToScalar > &tree_clone) const
 (Advanced) NVI to DoCloneToScalar() templated on the scalar type of the new clone to be created. More...
 
Does not allow copy, move, or assignment
 Frame (const Frame &)=delete
 
Frameoperator= (const Frame &)=delete
 
 Frame (Frame &&)=delete
 
Frameoperator= (Frame &&)=delete
 
Internal use only

These functions work directly with the frame body pose cache entry.

void set_body_pose_index_in_cache (int body_pose_index)
 (Internal use only) A Frame's pose-in-parent X_PF can be parameterized, the parent's pose may also be parameterized, and so on. More...
 
int get_body_pose_index_in_cache () const
 (Internal use only) Retrieve this Frame's body pose index in the cache. More...
 
const math::RigidTransform< T > & get_X_BF (const internal::FrameBodyPoseCache< T > &frame_body_poses) const
 (Internal use only) Given an already up-to-date frame body pose cache, extract X_BF for this Frame from it. More...
 
const math::RigidTransform< T > & get_X_FB (const internal::FrameBodyPoseCache< T > &frame_body_poses) const
 (Internal use only) Given an already up-to-date frame body pose cache, extract X_FB (=X_BF⁻¹) for this Frame from it. More...
 
- Public Member Functions inherited from FrameBase< T >
FrameIndex index () const
 Returns this element's unique index. More...
 
 ~FrameBase () override
 
- Public Member Functions inherited from MultibodyElement< T >
virtual ~MultibodyElement ()
 
ModelInstanceIndex model_instance () const
 Returns the ModelInstanceIndex of the model instance to which this element belongs. More...
 
template<typename MultibodyPlantDeferred = MultibodyPlant<T>>
const MultibodyPlantDeferred & GetParentPlant () const
 Returns the MultibodyPlant that owns this MultibodyElement. More...
 
void DeclareParameters (internal::MultibodyTreeSystem< T > *tree_system)
 Declares MultibodyTreeSystem Parameters at MultibodyTreeSystem::Finalize() time. More...
 
void SetDefaultParameters (systems::Parameters< T > *parameters) const
 Sets default values of parameters belonging to each MultibodyElement in parameters at a call to MultibodyTreeSystem::SetDefaultParameters(). More...
 
 MultibodyElement (const MultibodyElement &)=delete
 
MultibodyElementoperator= (const MultibodyElement &)=delete
 
 MultibodyElement (MultibodyElement &&)=delete
 
MultibodyElementoperator= (MultibodyElement &&)=delete
 

Protected Member Functions

 Frame (const std::string &name, const RigidBody< T > &body, std::optional< ModelInstanceIndex > model_instance={})
 Only derived classes can use this constructor. More...
 
virtual void DoDeclareFrameParameters (internal::MultibodyTreeSystem< T > *)
 Called by DoDeclareParameters(). More...
 
virtual void DoSetDefaultFrameParameters (systems::Parameters< T > *) const
 Called by DoSetDefaultParameters(). More...
 
virtual math::RigidTransform< T > DoCalcPoseInBodyFrame (const systems::Parameters< T > &parameters) const =0
 
virtual math::RotationMatrix< T > DoCalcRotationMatrixInBodyFrame (const systems::Parameters< T > &parameters) const =0
 
virtual math::RigidTransform< T > DoCalcOffsetPoseInBody (const systems::Parameters< T > &parameters, const math::RigidTransform< T > &X_FQ) const
 
virtual math::RotationMatrix< T > DoCalcOffsetRotationMatrixInBody (const systems::Parameters< T > &parameters, const math::RotationMatrix< T > &R_FQ) const
 
Methods to make a clone templated on different scalar types.

These methods are meant to be called by MultibodyTree::CloneToScalar() when making a clone of the entire tree or a new instance templated on a different scalar type.

The only const argument to these methods is the new MultibodyTree clone under construction. Specific Frame subclasses might specify a number of prerequisites on the cloned tree and therefore require it to be at a given state of cloning. See MultibodyTree::CloneToScalar() for a list of prerequisites that are guaranteed to be satisfied during the cloning process.

virtual std::unique_ptr< Frame< double > > DoCloneToScalar (const internal::MultibodyTree< double > &tree_clone) const =0
 Clones this Frame (templated on T) to a frame templated on double. More...
 
virtual std::unique_ptr< Frame< AutoDiffXd > > DoCloneToScalar (const internal::MultibodyTree< AutoDiffXd > &tree_clone) const =0
 Clones this Frame (templated on T) to a frame templated on AutoDiffXd. More...
 
virtual std::unique_ptr< Frame< symbolic::Expression > > DoCloneToScalar (const internal::MultibodyTree< symbolic::Expression > &) const =0
 
- Protected Member Functions inherited from FrameBase< T >
 FrameBase (ModelInstanceIndex model_instance)
 
- Protected Member Functions inherited from MultibodyElement< T >
 MultibodyElement ()
 Default constructor made protected so that sub-classes can still declare their default constructors if they need to. More...
 
 MultibodyElement (ModelInstanceIndex model_instance)
 Constructor which allows specifying a model instance. More...
 
 MultibodyElement (ModelInstanceIndex model_instance, int64_t index)
 Both the model instance and element index are specified. More...
 
template<typename ElementIndexType >
ElementIndexType index_impl () const
 Returns this element's unique index. More...
 
int ordinal_impl () const
 Returns this element's unique ordinal. More...
 
const internal::MultibodyTree< T > & get_parent_tree () const
 Returns a constant reference to the parent MultibodyTree that owns this element. More...
 
const internal::MultibodyTreeSystem< T > & GetParentTreeSystem () const
 Returns a constant reference to the parent MultibodyTreeSystem that owns the parent MultibodyTree that owns this element. More...
 
void SetTopology (const internal::MultibodyTreeTopology &tree)
 Gives MultibodyElement-derived objects the opportunity to retrieve their topology after MultibodyTree::Finalize() is invoked. More...
 
systems::NumericParameterIndex DeclareNumericParameter (internal::MultibodyTreeSystem< T > *tree_system, const systems::BasicVector< T > &model_vector)
 To be used by MultibodyElement-derived objects when declaring parameters in their implementation of DoDeclareParameters(). More...
 
systems::AbstractParameterIndex DeclareAbstractParameter (internal::MultibodyTreeSystem< T > *tree_system, const AbstractValue &model_value)
 To be used by MultibodyElement-derived objects when declaring parameters in their implementation of DoDeclareParameters(). More...
 

Constructor & Destructor Documentation

◆ Frame() [1/3]

Frame ( const Frame< T > &  )
delete

◆ Frame() [2/3]

Frame ( Frame< T > &&  )
delete

◆ ~Frame()

~Frame ( )
override

◆ Frame() [3/3]

Frame ( const std::string &  name,
const RigidBody< T > &  body,
std::optional< ModelInstanceIndex model_instance = {} 
)
explicitprotected

Only derived classes can use this constructor.

It creates a Frame object attached to body and puts the frame in the body's model instance.

Member Function Documentation

◆ body()

const RigidBody<T>& body ( ) const

Returns a const reference to the body associated to this Frame.

◆ CalcAngularVelocity()

Vector3<T> CalcAngularVelocity ( const systems::Context< T > &  context,
const Frame< T > &  measured_in_frame,
const Frame< T > &  expressed_in_frame 
) const

Calculates this frame F's angular velocity measured in a frame M, expressed in a frame E.

Parameters
[in]contextcontains the state of the multibody system.
[in]measured_in_framewhich is frame M (the frame in which this angular velocity is to be measured).
[in]expressed_in_framewhich is frame E (the frame in which the returned angular velocity is to be expressed).
Returns
ω_MF_E, this frame F's angular velocity ω measured in frame M, expressed in frame E.
See also
EvalAngularVelocityInWorld() to evaluate ω_WF_W (this frame F's angular velocity ω measured and expressed in the world frame W).

◆ CalcOffsetPoseInBody()

math::RigidTransform<T> CalcOffsetPoseInBody ( const systems::Context< T > &  context,
const math::RigidTransform< T > &  X_FQ 
) const

Given the offset pose X_FQ of a frame Q in this frame F, this method computes the pose X_BQ of frame Q in the body frame B to which this frame is attached.

In other words, if the pose of this frame F in the body frame B is X_BF, this method computes the pose X_BQ of frame Q in the body frame B as X_BQ = X_BF * X_FQ. In particular, if this isthe body frame B, i.e.X_BFis the identity transformation, this method directly returnsX_FQ`. Specific frame subclasses can override this method to provide faster implementations if needed.

◆ CalcOffsetRotationMatrixInBody()

math::RotationMatrix<T> CalcOffsetRotationMatrixInBody ( const systems::Context< T > &  context,
const math::RotationMatrix< T > &  R_FQ 
) const

Calculates and returns the rotation matrix R_BQ that relates body frame B to frame Q via this intermediate frame F, i.e., R_BQ = R_BF * R_FQ (B is the body frame to which this frame F is attached).

Parameters
[in]R_FQrotation matrix that relates frame F to frame Q.

◆ CalcPose()

math::RigidTransform<T> CalcPose ( const systems::Context< T > &  context,
const Frame< T > &  frame_M 
) const

Computes and returns the pose X_MF of this frame F in measured in frame_M as a function of the state of the model stored in context.

See also
CalcPoseInWorld().

◆ CalcPoseInBodyFrame()

math::RigidTransform<T> CalcPoseInBodyFrame ( const systems::Context< T > &  context) const

Returns the pose X_BF of this frame F in the body frame B associated with this frame.

In particular, if this is the body frame B, this method directly returns the identity transformation. Note that this ONLY depends on the Parameters in the context; it does not depend on time, input, state, etc.

◆ CalcPoseInWorld()

math::RigidTransform<T> CalcPoseInWorld ( const systems::Context< T > &  context) const

Computes and returns the pose X_WF of this frame F in the world frame W as a function of the state of the model stored in context.

Note
RigidBody::EvalPoseInWorld() provides a more efficient way to obtain the pose for a body frame.

◆ CalcRelativeSpatialAcceleration()

SpatialAcceleration<T> CalcRelativeSpatialAcceleration ( const systems::Context< T > &  context,
const Frame< T > &  other_frame,
const Frame< T > &  measured_in_frame,
const Frame< T > &  expressed_in_frame 
) const

Calculates this frame C's spatial acceleration relative to another frame B, measured in a frame M, expressed in a frame E.

Parameters
[in]contextcontains the state of the multibody system.
[in]other_framewhich is frame B.
[in]measured_in_framewhich is frame M.
[in]expressed_in_framewhich is frame E.
Returns
A_M_BC_E = A_MC_E - A_MB_E, frame C's spatial acceleration relative to frame B, measured in frame M, expressed in frame E.

In general, A_M_BC = DtW(V_M_BC), the time-derivative in frame M of frame C's spatial velocity relative to frame B. The rotational part of the returned quantity is α_MC_E - α_MB_E = DtM(ω_BC)_E. Note: For 3D analysis, DtM(ω_BC) ≠ α_BC. The translational part of the returned quantity is a_M_BoCo_E (Co's translational acceleration relative to Bo, measured in frame M, expressed in frame E).

 α_MC_E - α_MB_E = DtM(ω_MC)_E - DtM(ω_MB)_E = DtM(ω_BC)_E
 a_M_BoCo_E = a_MCo_E - a_MBo_E = DtM(v_MCo) - DtM(v_MBo) = Dt²M(p_BoCo)_E

where Dt²M(p_BoCo)_E is the 2ⁿᵈ time-derivative in frame M of p_BoCo (the position vector from Bo to Co), and this result is expressed in frame E.

Note
The calculation of the 2ⁿᵈ time-derivative of the distance between Bo and Co can be done with relative translational acceleration, but this calculation does not depend on the measured-in-frame, hence in this case, consider CalcRelativeSpatialAccelerationInWorld() since it is faster.
See also
CalcSpatialAccelerationInWorld(), CalcSpatialAcceleration(), and CalcRelativeSpatialAccelerationInWorld().

◆ CalcRelativeSpatialAccelerationInWorld()

SpatialAcceleration<T> CalcRelativeSpatialAccelerationInWorld ( const systems::Context< T > &  context,
const Frame< T > &  other_frame 
) const

Calculates this frame C's spatial acceleration relative to another frame B, measured and expressed in the world frame W.

Parameters
[in]contextcontains the state of the multibody system.
[in]other_framewhich is frame B.
Returns
A_W_BC_W = A_WC_W - A_WB_W, frame C's spatial acceleration relative to frame B, measured and expressed in the world frame W.

In general, A_W_BC = DtW(V_W_BC), the time-derivative in the world frame W of frame C's spatial velocity relative to frame B. The rotational part of the returned quantity is α_WC_W - α_WB_W = DtW(ω_BC)_W. For 3D analysis, DtW(ω_BC) ≠ α_BC. The translational part of the returned quantity is a_W_BoCo_W (Co's translational acceleration relative to Bo, measured and expressed in world frame W).

 α_WC_W - α_WB_W = DtW(ω_WC)_W - DtW(ω_WB)_W = DtW(ω_BC)_W
 a_W_BoCo_W = a_WCo_W - a_WBo_W = DtW(v_WCo) - DtW(v_WBo) = Dt²W(p_BoCo)_W

where Dt²W(p_BoCo)_W is the 2ⁿᵈ time-derivative in frame W of p_BoCo (the position vector from Bo to Co), and this result is expressed in frame W.

Note
The method CalcSpatialAccelerationInWorld() is more efficient and coherent if any of this, other_frame, or the world frame W are the same.
See also
CalcSpatialAccelerationInWorld(), CalcRelativeSpatialAcceleration().

◆ CalcRelativeSpatialVelocity()

SpatialVelocity<T> CalcRelativeSpatialVelocity ( const systems::Context< T > &  context,
const Frame< T > &  other_frame,
const Frame< T > &  measured_in_frame,
const Frame< T > &  expressed_in_frame 
) const

Calculates this frame C's spatial velocity relative to another frame B, measured in a frame M, expressed in a frame E.

Parameters
[in]contextcontains the state of the multibody system.
[in]other_framewhich is frame B.
[in]measured_in_framewhich is frame M.
[in]expressed_in_framewhich is frame E.
Returns
V_M_BC_E = V_MC_E - V_MB_E, frame C's spatial velocity relative to frame B, measured in frame M, expressed in frame E. The rotational part of the returned quantity is ω_BC_E (C's angular velocity measured in B and expressed in E). The translational part is v_M_BoCo_E (Co's translational velocity relative to Bo, measured in M, and expressed in E).
 ω_BC_E = ω_MC_E - ω_MB_E
 v_M_BoCo_E = v_MCo_E - v_MBo_E = DtM(p_BoCo)
where DtM(p_BoCo) is the time-derivative in frame M of p_BoCo (position vector from Bo to Co), and this vector is expressed in frame E.
Note
The method CalcSpatialVelocity() is more efficient and coherent if any of this, other_frame, or measured_in_frame are the same. Also, the value of V_M_BoCo does not depend on the measured_in_frame if Bo and Co are coincident (i.e., p_BoCo = 0), in which case consider the more efficient method CalcRelativeSpatialVelocityInWorld(). Lastly, the calculation of elongation between Bo and Co can be done with relative translational velocity, but elongation does not depend on the measured-in-frame (hence consider CalcRelativeSpatialVelocityInWorld()).
See also
CalcSpatialVelocityInWorld(), CalcSpatialVelocity(), and CalcRelativeSpatialVelocityInWorld().

◆ CalcRelativeSpatialVelocityInWorld()

SpatialVelocity<T> CalcRelativeSpatialVelocityInWorld ( const systems::Context< T > &  context,
const Frame< T > &  other_frame 
) const

Calculates this frame C's spatial velocity relative to another frame B, measured and expressed in the world frame W.

Parameters
[in]contextcontains the state of the multibody system.
[in]other_framewhich is frame B.
Returns
V_W_BC_W = V_WC_W - V_WB_W, frame C's spatial velocity relative to frame B, measured and expressed in the world frame W. The rotational part of the returned quantity is ω_BC_W (C's angular velocity measured in B and expressed in W). The translational part is v_W_BoCo_W (Co's translational velocity relative to Bo, measured and expressed in world frame W).
    ω_BC_W  = ω_WC_W - ω_WB_W
 v_W_BoCo_W = v_WCo_W - v_WBo_W = DtW(p_BoCo)
where DtW(p_BoCo) is the time-derivative in frame W of p_BoCo (position vector from Bo to Co), and this vector is expressed in frame W.
Note
The method CalcSpatialVelocityInWorld() is more efficient and coherent if any of this, other_frame, or the world frame W are the same.
See also
CalcSpatialVelocityInWorld() and CalcRelativeSpatialVelocity().

◆ CalcRotationMatrix()

math::RotationMatrix<T> CalcRotationMatrix ( const systems::Context< T > &  context,
const Frame< T > &  frame_M 
) const

Calculates and returns the rotation matrix R_MF that relates frame_M and this frame F as a function of the state stored in context.

◆ CalcRotationMatrixInBodyFrame()

math::RotationMatrix<T> CalcRotationMatrixInBodyFrame ( const systems::Context< T > &  context) const

Returns the rotation matrix R_BF that relates body frame B to this frame F (B is the body frame to which this frame F is attached).

Note
If this is B, this method returns the identity RotationMatrix. Note that this ONLY depends on the Parameters in the context; it does not depend on time, input, state, etc.

◆ CalcRotationMatrixInWorld()

math::RotationMatrix<T> CalcRotationMatrixInWorld ( const systems::Context< T > &  context) const

Calculates and returns the rotation matrix R_WF that relates the world frame W and this frame F as a function of the state stored in context.

◆ CalcSpatialAcceleration()

SpatialAcceleration<T> CalcSpatialAcceleration ( const systems::Context< T > &  context,
const Frame< T > &  measured_in_frame,
const Frame< T > &  expressed_in_frame 
) const

Calculates this frame F's spatial acceleration measured in a frame M, expressed in a frame E.

Parameters
[in]contextcontains the state of the multibody system.
[in]measured_in_framewhich is frame M.
[in]expressed_in_framewhich is frame E.
Returns
A_MF_E, this frame F's spatial acceleration measured in frame M, expressed in frame E. The rotational part of the returned quantity is α_MF_E (frame F's angular acceleration α measured in frame M, expressed in frame E). The translational part is a_MFo_E (translational acceleration of frame F's origin point Fo, measured in frame M, expressed in frame E). Although α_MF is defined below in terms of DtM(ω_MF), the time-derivative in frame M of ω_MF, the actual calculation of α_MF avoids differentiation. Similarly for the definition vs. calculation for a_MFo.
 α_MF = DtM(ω_MF)           ω_MF is frame F's angular velocity in frame M.
 a_MFo = DtM(v_MFo)    v_MF is Fo's translational acceleration in frame M.
See also
CalcSpatialAccelerationInWorld() and CalcSpatialVelocity().

◆ CalcSpatialAccelerationInWorld()

SpatialAcceleration<T> CalcSpatialAccelerationInWorld ( const systems::Context< T > &  context) const

Calculates this frame F's spatial acceleration measured and expressed in the world frame W.

Parameters
[in]contextcontains the state of the multibody system.
Returns
A_WF_W, this frame F's spatial acceleration measured and expressed in the world frame W. The rotational part of the returned quantity is α_WF_E (frame F's angular acceleration α measured and expressed in the world frame W). The translational part is a_WFo_W (translational acceleration of frame F's origin point Fo, measured and expressed in the world frame W).
Note
RigidBody::EvalSpatialAccelerationInWorld() provides a more efficient way to obtain a body frame's spatial acceleration measured in the world frame.
When cached values are out of sync with the state stored in context, this method performs an expensive forward dynamics computation, whereas once evaluated, successive calls to this method are inexpensive.
See also
CalcSpatialAcceleration() and CalcSpatialVelocityInWorld().

◆ CalcSpatialVelocity()

SpatialVelocity<T> CalcSpatialVelocity ( const systems::Context< T > &  context,
const Frame< T > &  frame_M,
const Frame< T > &  frame_E 
) const

Calculates this frame F's spatial velocity measured in a frame M, expressed in a frame E.

Parameters
[in]contextcontains the state of the multibody system.
[in]frame_Mwhich is the measured_in_frame.
[in]frame_Ewhich is the expressed_in_frame.
Returns
V_MF_E, this frame F's spatial velocity measured in frame M, expressed in frame E. The rotational part of the returned quantity is ω_MF_E (frame F's angular velocity ω measured in frame M, expressed in frame E). The translational part is v_MFo_E (translational velocity v of frame F's origin point Fo, measured in frame M, expressed in frame E).
See also
CalcSpatialVelocityInWorld(), CalcRelativeSpatialVelocity(), and CalcSpatialAcceleration().

◆ CalcSpatialVelocityInWorld()

SpatialVelocity<T> CalcSpatialVelocityInWorld ( const systems::Context< T > &  context) const

Calculates this frame F's spatial velocity measured and expressed in the world frame W.

Parameters
[in]contextcontains the state of the multibody system.
Returns
V_WF_W, this frame F's spatial velocity measured and expressed in the world frame W. The rotational part of the returned quantity is ω_WF_W (frame F's angular velocity ω measured and expressed in the world frame W). The translational part is v_WFo_W (translational velocity v of frame F's origin point Fo, measured and expressed in the world frame W).
Note
RigidBody::EvalSpatialVelocityInWorld() provides a more efficient way to obtain a body frame's spatial velocity measured in the world frame.
See also
CalcSpatialVelocity(), CalcRelativeSpatialVelocityInWorld(), and CalcSpatialAccelerationInWorld().

◆ CloneToScalar()

std::unique_ptr<Frame<ToScalar> > CloneToScalar ( const internal::MultibodyTree< ToScalar > &  tree_clone) const

(Advanced) NVI to DoCloneToScalar() templated on the scalar type of the new clone to be created.

This method is mostly intended to be called by MultibodyTree::CloneToScalar(). Most users should not call this clone method directly but rather clone the entire parent MultibodyTree if needed.

See also
MultibodyTree::CloneToScalar()

◆ DoCalcOffsetPoseInBody()

virtual math::RigidTransform<T> DoCalcOffsetPoseInBody ( const systems::Parameters< T > &  parameters,
const math::RigidTransform< T > &  X_FQ 
) const
protectedvirtual

◆ DoCalcOffsetRotationMatrixInBody()

virtual math::RotationMatrix<T> DoCalcOffsetRotationMatrixInBody ( const systems::Parameters< T > &  parameters,
const math::RotationMatrix< T > &  R_FQ 
) const
protectedvirtual

◆ DoCalcPoseInBodyFrame()

virtual math::RigidTransform<T> DoCalcPoseInBodyFrame ( const systems::Parameters< T > &  parameters) const
protectedpure virtual

◆ DoCalcRotationMatrixInBodyFrame()

virtual math::RotationMatrix<T> DoCalcRotationMatrixInBodyFrame ( const systems::Parameters< T > &  parameters) const
protectedpure virtual

◆ DoCloneToScalar() [1/3]

virtual std::unique_ptr<Frame<double> > DoCloneToScalar ( const internal::MultibodyTree< double > &  tree_clone) const
protectedpure virtual

Clones this Frame (templated on T) to a frame templated on double.

Implemented in FixedOffsetFrame< T >, RigidBodyFrame< T >, and RigidBodyFrame< double >.

◆ DoCloneToScalar() [2/3]

virtual std::unique_ptr<Frame<AutoDiffXd> > DoCloneToScalar ( const internal::MultibodyTree< AutoDiffXd > &  tree_clone) const
protectedpure virtual

Clones this Frame (templated on T) to a frame templated on AutoDiffXd.

Implemented in FixedOffsetFrame< T >, RigidBodyFrame< T >, and RigidBodyFrame< double >.

◆ DoCloneToScalar() [3/3]

virtual std::unique_ptr<Frame<symbolic::Expression> > DoCloneToScalar ( const internal::MultibodyTree< symbolic::Expression > &  ) const
protectedpure virtual

◆ DoDeclareFrameParameters()

virtual void DoDeclareFrameParameters ( internal::MultibodyTreeSystem< T > *  )
protectedvirtual

Called by DoDeclareParameters().

Derived classes may choose to override to declare their sub-class specific parameters.

◆ DoSetDefaultFrameParameters()

virtual void DoSetDefaultFrameParameters ( systems::Parameters< T > *  ) const
protectedvirtual

Called by DoSetDefaultParameters().

Derived classes may choose to override to set their sub-class specific parameters.

◆ EvalAngularVelocityInWorld()

const Vector3<T>& EvalAngularVelocityInWorld ( const systems::Context< T > &  context) const

Evaluates this frame F's angular velocity measured and expressed in the world frame W.

Parameters
[in]contextcontains the state of the multibody system.
Returns
ω_WF_W (frame F's angular velocity ω measured and expressed in the world frame W).
See also
CalcAngularVelocity() to calculate ω_MF_E (this frame F's angular velocity ω measured in a frame M and expressed in a frame E).

◆ EvalPoseInBodyFrame()

const math::RigidTransform<T>& EvalPoseInBodyFrame ( const systems::Context< T > &  context) const

Returns a reference to the body-relative pose X_BF giving the pose of this Frame with respect to its body's RigidBodyFrame.

This may depend on parameters in the Context but not on time or state. The first time this is called after a parameter change will precalculate offset poses for all Frames into the Context's cache; subsequent calls on any Frame are very fast.

◆ get_body_pose_index_in_cache()

int get_body_pose_index_in_cache ( ) const

(Internal use only) Retrieve this Frame's body pose index in the cache.

◆ get_X_BF()

const math::RigidTransform<T>& get_X_BF ( const internal::FrameBodyPoseCache< T > &  frame_body_poses) const

(Internal use only) Given an already up-to-date frame body pose cache, extract X_BF for this Frame from it.

Note
Be sure you have called MultibodyTreeSystem::EvalFrameBodyPoses() since the last parameter change; we can't check here.
Return values
X_BFpose of this frame in its body's frame

◆ get_X_FB()

const math::RigidTransform<T>& get_X_FB ( const internal::FrameBodyPoseCache< T > &  frame_body_poses) const

(Internal use only) Given an already up-to-date frame body pose cache, extract X_FB (=X_BF⁻¹) for this Frame from it.

Note
Be sure you have called MultibodyTreeSystem::EvalFrameBodyPoses() since the last parameter change; we can't check here.
Return values
X_FBinverse of this frame's pose in its body's frame

◆ GetFixedOffsetPoseInBody()

virtual math::RigidTransform<T> GetFixedOffsetPoseInBody ( const math::RigidTransform< T > &  X_FQ) const
virtual

Variant of CalcOffsetPoseInBody() that given the offset pose X_FQ of a frame Q in this frame F, returns the pose X_BQ of frame Q in the body frame B to which this frame is attached.

Exceptions
std::exceptionif called on a Frame that does not have a fixed offset in the body frame.

Reimplemented in RigidBodyFrame< T >, and RigidBodyFrame< double >.

◆ GetFixedPoseInBodyFrame()

virtual math::RigidTransform<T> GetFixedPoseInBodyFrame ( ) const
virtual

Variant of CalcPoseInBodyFrame() that returns the fixed pose X_BF of this frame F in the body frame B associated with this frame.

Exceptions
std::exceptionif called on a Frame that does not have a fixed offset in the body frame.

Reimplemented in FixedOffsetFrame< T >, RigidBodyFrame< T >, and RigidBodyFrame< double >.

◆ GetFixedRotationMatrixInBody()

virtual math::RotationMatrix<T> GetFixedRotationMatrixInBody ( const math::RotationMatrix< T > &  R_FQ) const
virtual

Calculates and returns the rotation matrix R_BQ that relates body frame B to frame Q via this intermediate frame F, i.e., R_BQ = R_BF * R_FQ (B is the body frame to which this frame F is attached).

Parameters
[in]R_FQrotation matrix that relates frame F to frame Q.
Exceptions
std::exceptionif this frame F is a Frame that does not have a fixed offset in the body frame B (i.e., R_BF is not constant).

Reimplemented in RigidBodyFrame< T >, and RigidBodyFrame< double >.

◆ GetFixedRotationMatrixInBodyFrame()

virtual math::RotationMatrix<T> GetFixedRotationMatrixInBodyFrame ( ) const
virtual

Returns the rotation matrix R_BF that relates body frame B to this frame F (B is the body frame to which this frame F is attached).

Exceptions
std::exceptionif this frame F is a Frame that does not have a fixed offset in the body frame B (i.e., R_BF is not constant). Frame sub-classes that have a constant R_BF must override this method. An example of a frame sub-class not implementing this method would be that of a frame on a soft body, for which its pose in the body frame depends on the state of deformation of the body.

Reimplemented in FixedOffsetFrame< T >, RigidBodyFrame< T >, and RigidBodyFrame< double >.

◆ is_body_frame()

bool is_body_frame ( ) const

Returns true if this is the body frame.

◆ is_world_frame()

bool is_world_frame ( ) const

Returns true if this is the world frame.

◆ name()

const std::string& name ( ) const

Returns the name of this frame. The name will never be empty.

◆ operator=() [1/2]

Frame& operator= ( Frame< T > &&  )
delete

◆ operator=() [2/2]

Frame& operator= ( const Frame< T > &  )
delete

◆ scoped_name()

ScopedName scoped_name ( ) const

Returns scoped name of this frame.

Neither of the two pieces of the name will be empty (the scope name and the element name).

◆ set_body_pose_index_in_cache()

void set_body_pose_index_in_cache ( int  body_pose_index)

(Internal use only) A Frame's pose-in-parent X_PF can be parameterized, the parent's pose may also be parameterized, and so on.

Thus the calculation of this frame's pose in its body (X_BF) can be expensive. There is a cache entry that holds the calculated X_BF, evaluated whenever parameters change. This allows us to grab X_BF as a const reference rather than having to extract and reformat parameters, and compose with parent and ancestor poses at runtime.

At the time parameters are allocated we assign a slot in the body pose cache entry to each Frame and record its index using this function. (The index for a RigidBodyFrame will refer to an identity transform.) Note that the body pose index is not necessarily the same as the Frame index because all RigidBodyFrames can share an entry. (Of course if you know you are working with a RigidBodyFrame you don't need to ask about its body pose!)


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