Drake
Body< T > Class Template Referenceabstract

Detailed Description

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

Body provides the general abstraction of a body with an API that makes no assumption about whether a body is rigid or deformable and neither does it make any assumptions about the underlying physical model or approximation.

As an element or component of a MultibodyTree, a body is a MultibodyElement, and therefore it has a unique index of type BodyIndex within the multibody tree it belongs to.

A Body contains a unique BodyFrame; see BodyFrame class documentation for more information.

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

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

Public Member Functions

 Body (const std::string &name, ModelInstanceIndex model_instance, double default_mass)
 Creates a Body named name in model instance model_instance with a given default_mass and a BodyFrame associated with it. More...
 
const std::string & name () const
 Gets the name associated with this body. More...
 
virtual int get_num_flexible_positions () const =0
 Returns the number of generalized positions q describing flexible deformations for this body. More...
 
virtual int get_num_flexible_velocities () const =0
 Returns the number of generalized velocities v describing flexible deformations for this body. More...
 
const BodyFrame< T > & body_frame () const
 Returns a const reference to the associated BodyFrame. More...
 
void Lock (systems::Context< T > *context) const
 For a floating body, lock its implicit inboard joint. More...
 
void Unlock (systems::Context< T > *context) const
 For a floating body, unlock its implicit inboard joint. More...
 
bool is_locked (const systems::Context< T > &context) const
 
internal::BodyNodeIndex node_index () const
 (Advanced) Returns the index of the node in the underlying tree structure of the parent MultibodyTree to which this body belongs. More...
 
bool is_floating () const
 (Advanced) Returns true if this body is granted 6-dofs by a Mobilizer. More...
 
bool has_quaternion_dofs () const
 (Advanced) If true, this body is a floating body modeled with a quaternion floating mobilizer. More...
 
int floating_positions_start () const
 (Advanced) For floating bodies (see is_floating()) this method returns the index of the first generalized position in the state vector for a MultibodyPlant model. More...
 
int floating_velocities_start () const
 (Advanced) For floating bodies (see is_floating()) this method returns the index of the first generalized velocity in the state vector for a MultibodyPlant model. More...
 
double get_default_mass () const
 Returns the default mass (not Context dependent) for this body. More...
 
virtual const T & get_mass (const systems::Context< T > &context) const =0
 (Advanced) Returns the mass of this body stored in context. More...
 
virtual const Vector3< T > CalcCenterOfMassInBodyFrame (const systems::Context< T > &context) const =0
 (Advanced) Computes the center of mass p_BoBcm_B (or p_Bcm for short) of this body measured from this body's frame origin Bo and expressed in the body frame B. More...
 
virtual Vector3< T > CalcCenterOfMassTranslationalVelocityInWorld (const systems::Context< T > &context) const =0
 Calculates v_WBcm, Bcm's translational velocity in the world frame W. More...
 
virtual SpatialInertia< T > CalcSpatialInertiaInBodyFrame (const systems::Context< T > &context) const =0
 (Advanced) Computes the SpatialInertia I_BBo_B of this body about its frame origin Bo (not necessarily its center of mass) and expressed in its body frame B. More...
 
const math::RigidTransform< T > & EvalPoseInWorld (const systems::Context< T > &context) const
 Returns the pose X_WB of this body B in the world frame W as a function of the state of the model stored in context. More...
 
const SpatialVelocity< T > & EvalSpatialVelocityInWorld (const systems::Context< T > &context) const
 Evaluates V_WB, this body B's spatial velocity in the world frame W. More...
 
const SpatialAcceleration< T > & EvalSpatialAccelerationInWorld (const systems::Context< T > &context) const
 Evaluates A_WB, this body B's spatial acceleration in the world frame W. More...
 
const SpatialForce< T > & GetForceInWorld (const systems::Context< T > &, const MultibodyForces< T > &forces) const
 Gets the sptatial force on this body B from forces as F_BBo_W: applied at body B's origin Bo and expressed in world frame W. More...
 
void AddInForceInWorld (const systems::Context< T > &, const SpatialForce< T > &F_Bo_W, MultibodyForces< T > *forces) const
 Adds the spatial force on this body B, applied at body B's origin Bo and expressed in the world frame W into forces. More...
 
void AddInForce (const systems::Context< T > &context, const Vector3< T > &p_BP_E, const SpatialForce< T > &F_Bp_E, const Frame< T > &frame_E, MultibodyForces< T > *forces) const
 Adds the spatial force on this body B, applied at point P and expressed in a frame E into forces. More...
 
template<typename ToScalar >
std::unique_ptr< Body< ToScalar > > CloneToScalar (const internal::MultibodyTree< ToScalar > &tree_clone) const
 NVI (Non-Virtual Interface) to DoCloneToScalar() templated on the scalar type of the new clone to be created. More...
 
Does not allow copy, move, or assignment
 Body (const Body &)=delete
 
Bodyoperator= (const Body &)=delete
 
 Body (Body &&)=delete
 
Bodyoperator= (Body &&)=delete
 
- Public Member Functions inherited from MultibodyElement< Body, T, BodyIndex >
virtual ~MultibodyElement ()
 
BodyIndex index () const
 Returns this element's unique index. More...
 
ModelInstanceIndex model_instance () const
 Returns the ModelInstanceIndex of the model instance to which this element belongs. More...
 
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...
 
 MultibodyElement (const MultibodyElement &)=delete
 
 MultibodyElement (MultibodyElement &&)=delete
 
MultibodyElementoperator= (const MultibodyElement &)=delete
 
MultibodyElementoperator= (MultibodyElement &&)=delete
 

Protected Member Functions

void DoDeclareParameters (internal::MultibodyTreeSystem< T > *tree_system) override
 Implementation of the NVI DeclareParameters(). More...
 
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 Body subclasses might specify a number of prerequisites on the cloned tree and therefore require it to be at a given state of cloning (for instance requiring that the cloned tree already contains all the frames in the world as in the original tree.) See MultibodyTree::CloneToScalar() for a list of prerequisites that are guaranteed to be satisfied during the cloning process.

virtual std::unique_ptr< Body< double > > DoCloneToScalar (const internal::MultibodyTree< double > &tree_clone) const =0
 Clones this Body (templated on T) to a body templated on double. More...
 
virtual std::unique_ptr< Body< AutoDiffXd > > DoCloneToScalar (const internal::MultibodyTree< AutoDiffXd > &tree_clone) const =0
 Clones this Body (templated on T) to a body templated on AutoDiffXd. More...
 
virtual std::unique_ptr< Body< symbolic::Expression > > DoCloneToScalar (const internal::MultibodyTree< symbolic::Expression > &) const =0
 Clones this Body (templated on T) to a body templated on Expression. More...
 
- Protected Member Functions inherited from MultibodyElement< Body, T, BodyIndex >
 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...
 
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...
 

Friends

class internal::BodyAttorney< T >
 

Constructor & Destructor Documentation

◆ Body() [1/3]

Body ( const Body< T > &  )
delete

◆ Body() [2/3]

Body ( Body< T > &&  )
delete

◆ Body() [3/3]

Body ( const std::string &  name,
ModelInstanceIndex  model_instance,
double  default_mass 
)

Creates a Body named name in model instance model_instance with a given default_mass and a BodyFrame associated with it.

Member Function Documentation

◆ AddInForce()

void AddInForce ( const systems::Context< T > &  context,
const Vector3< T > &  p_BP_E,
const SpatialForce< T > &  F_Bp_E,
const Frame< T > &  frame_E,
MultibodyForces< T > *  forces 
) const

Adds the spatial force on this body B, applied at point P and expressed in a frame E into forces.

Parameters
[in]contextThe context containing the current state of the model.
[in]p_BP_EThe position of point P in B, expressed in a frame E.
[in]F_Bp_EThe spatial force to be applied on body B at point P, expressed in frame E.
[in]frame_EThe expressed-in frame E.
[out]forcesA multibody forces objects that on output will have F_Bp_E added.
Exceptions
std::exceptionif forces is nullptr or if it is not consistent with the model to which this body belongs.

◆ AddInForceInWorld()

void AddInForceInWorld ( const systems::Context< T > &  ,
const SpatialForce< T > &  F_Bo_W,
MultibodyForces< T > *  forces 
) const

Adds the spatial force on this body B, applied at body B's origin Bo and expressed in the world frame W into forces.

◆ body_frame()

const BodyFrame<T>& body_frame ( ) const

Returns a const reference to the associated BodyFrame.

◆ CalcCenterOfMassInBodyFrame()

virtual const Vector3<T> CalcCenterOfMassInBodyFrame ( const systems::Context< T > &  context) const
pure virtual

(Advanced) Computes the center of mass p_BoBcm_B (or p_Bcm for short) of this body measured from this body's frame origin Bo and expressed in the body frame B.

Implemented in RigidBody< T >.

◆ CalcCenterOfMassTranslationalVelocityInWorld()

virtual Vector3<T> CalcCenterOfMassTranslationalVelocityInWorld ( const systems::Context< T > &  context) const
pure virtual

Calculates v_WBcm, Bcm's translational velocity in the world frame W.

Parameters
[in]contextThe context contains the state of the model.
Return values
v_WBcm_WBcm's (this body's center of mass) translational velocity in the world frame W, expressed in W.

Implemented in RigidBody< T >.

◆ CalcSpatialInertiaInBodyFrame()

virtual SpatialInertia<T> CalcSpatialInertiaInBodyFrame ( const systems::Context< T > &  context) const
pure virtual

(Advanced) Computes the SpatialInertia I_BBo_B of this body about its frame origin Bo (not necessarily its center of mass) and expressed in its body frame B.

In general, the spatial inertia of a body is a function of state. Consider for instance the case of a flexible body for which its spatial inertia in the body frame depends on the generalized coordinates describing its state of deformation. As a particular case, the spatial inertia of a RigidBody in its body frame is constant.

Implemented in RigidBody< T >.

◆ CloneToScalar()

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

NVI (Non-Virtual Interface) 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()

◆ DoCloneToScalar() [1/3]

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

Clones this Body (templated on T) to a body templated on double.

Implemented in RigidBody< T >.

◆ DoCloneToScalar() [2/3]

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

Clones this Body (templated on T) to a body templated on AutoDiffXd.

Implemented in RigidBody< T >.

◆ DoCloneToScalar() [3/3]

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

Clones this Body (templated on T) to a body templated on Expression.

Implemented in RigidBody< T >.

◆ DoDeclareParameters()

void DoDeclareParameters ( internal::MultibodyTreeSystem< T > *  )
overrideprotectedvirtual

Implementation of the NVI DeclareParameters().

MultibodyElement-derived objects must override to declare their specific parameters. If an object is not a direct descendent of MultibodyElement, it must invoke its parent classes' DoDeclareParameters() before declaring its own parameters.

Reimplemented from MultibodyElement< Body, T, BodyIndex >.

Reimplemented in RigidBody< T >.

◆ EvalPoseInWorld()

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

Returns the pose X_WB of this body B in the world frame W as a function of the state of the model stored in context.

◆ EvalSpatialAccelerationInWorld()

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

Evaluates A_WB, this body B's spatial acceleration in the world frame W.

Parameters
[in]contextContains the state of the model.
Return values
A_WB_Wthis body B's spatial acceleration in the world frame W, expressed in W (for point Bo, the body's origin).
Note
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.

◆ EvalSpatialVelocityInWorld()

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

Evaluates V_WB, this body B's spatial velocity in the world frame W.

Parameters
[in]contextContains the state of the model.
Return values
V_WB_Wthis body B's spatial velocity in the world frame W, expressed in W (for point Bo, the body frame's origin).

◆ floating_positions_start()

int floating_positions_start ( ) const

(Advanced) For floating bodies (see is_floating()) this method returns the index of the first generalized position in the state vector for a MultibodyPlant model.

Positions for this body are then contiguous starting at this index. When a floating body is modeled with a quaternion mobilizer (see has_quaternion_dofs()), the four consecutive entries in the state starting at this index correspond to the quaternion that parametrizes this body's orientation.

Exceptions
std::exceptionif called pre-finalize, see MultibodyPlant::Finalize().

◆ floating_velocities_start()

int floating_velocities_start ( ) const

(Advanced) For floating bodies (see is_floating()) this method returns the index of the first generalized velocity in the state vector for a MultibodyPlant model.

Velocities for this body are then contiguous starting at this index.

Exceptions
std::exceptionif called pre-finalize, see MultibodyPlant::Finalize().

◆ get_default_mass()

double get_default_mass ( ) const

Returns the default mass (not Context dependent) for this body.

In general, the mass for a body can be a parameter of the model that can be retrieved with the method get_mass(). When the mass of a body is a parameter, the value returned by get_default_mass() is used to initialize the mass parameter in the context.

◆ get_mass()

virtual const T& get_mass ( const systems::Context< T > &  context) const
pure virtual

(Advanced) Returns the mass of this body stored in context.

Implemented in RigidBody< T >.

◆ get_num_flexible_positions()

virtual int get_num_flexible_positions ( ) const
pure virtual

Returns the number of generalized positions q describing flexible deformations for this body.

A rigid body will therefore return zero.

Implemented in RigidBody< T >.

◆ get_num_flexible_velocities()

virtual int get_num_flexible_velocities ( ) const
pure virtual

Returns the number of generalized velocities v describing flexible deformations for this body.

A rigid body will therefore return zero.

Implemented in RigidBody< T >.

◆ GetForceInWorld()

const SpatialForce<T>& GetForceInWorld ( const systems::Context< T > &  ,
const MultibodyForces< T > &  forces 
) const

Gets the sptatial force on this body B from forces as F_BBo_W: applied at body B's origin Bo and expressed in world frame W.

◆ has_quaternion_dofs()

bool has_quaternion_dofs ( ) const

(Advanced) If true, this body is a floating body modeled with a quaternion floating mobilizer.

By implication, is_floating() is also true.

See also
floating_positions_start(), floating_velocities_start().
Exceptions
std::exceptionif called pre-finalize, see MultibodyPlant::Finalize().

◆ is_floating()

bool is_floating ( ) const

(Advanced) Returns true if this body is granted 6-dofs by a Mobilizer.

Note
A floating body is not necessarily modeled with a quaternion mobilizer, see has_quaternion_dofs(). Alternative options include a space XYZ parametrization of rotations, see SpaceXYZMobilizer.
Exceptions
std::exceptionif called pre-finalize, see MultibodyPlant::Finalize().

◆ is_locked()

bool is_locked ( const systems::Context< T > &  context) const
Returns
true if the body is locked, false otherwise.

◆ Lock()

void Lock ( systems::Context< T > *  context) const

For a floating body, lock its implicit inboard joint.

Its generalized velocities will be 0 until it is unlocked. Locking is not yet supported for continuous-mode systems.

Exceptions
std::exceptionif this body is not a floating body, or if the parent model uses continuous state.

◆ name()

const std::string& name ( ) const

Gets the name associated with this body.

◆ node_index()

internal::BodyNodeIndex node_index ( ) const

(Advanced) Returns the index of the node in the underlying tree structure of the parent MultibodyTree to which this body belongs.

◆ operator=() [1/2]

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

◆ operator=() [2/2]

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

◆ Unlock()

void Unlock ( systems::Context< T > *  context) const

For a floating body, unlock its implicit inboard joint.

Unlocking is not yet supported for continuous-mode systems.

Exceptions
std::exceptionif this body is not a floating body, or if the parent model uses continuous state.

Friends And Related Function Documentation

◆ internal::BodyAttorney< T >

friend class internal::BodyAttorney< T >
friend

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