Drake
Drake C++ Documentation
Loading...
Searching...
No Matches
UniformGravityFieldElement< T > Class Template Reference

Detailed Description

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

This ForceElement allows modeling the effect of a uniform gravity field as felt by bodies on the surface of the Earth or elsewhere.

This gravity field acts on all bodies in the MultibodyPlant except those whose ModelInstance has been explicitly excluded.

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

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

Public Member Functions

 UniformGravityFieldElement ()
 Constructs a uniform gravity field element with a default strength (on the Earth's surface) and direction (-z).
 UniformGravityFieldElement (Vector3< double > g_W)
 Constructs a uniform gravity field element with a strength and direction given by the gravity acceleration vector g_W, expressed in the world frame W.
 UniformGravityFieldElement (Vector3< double > g_W, std::set< ModelInstanceIndex > disabled_model_instances)
 Constructs a uniform gravity field element with a strength and direction given by the gravity acceleration vector g_W, expressed in the world frame W.
 ~UniformGravityFieldElement () override
const Vector3< double > & gravity_vector () const
 Returns the acceleration of gravity vector in m/s², expressed in the world frame W.
void set_gravity_vector (const Vector3< double > &g_W)
 Sets the gravity acceleration vector (in m/s²), expressed in the world frame W.
bool is_enabled (ModelInstanceIndex model_instance) const
 Checks whether gravity is enabled for a given model instance.
void set_enabled (ModelInstanceIndex model_instance, bool is_enabled)
 Enables or disables gravity for a given model_instance.
VectorX< T > CalcGravityGeneralizedForces (const systems::Context< T > &context) const
 Computes the generalized forces tau_g(q) due to this gravity field element as a function of the generalized positions q stored in the input context, for the multibody model to which this element belongs.
CalcPotentialEnergy (const systems::Context< T > &context, const internal::PositionKinematicsCache< T > &pc) const final
 Computes the total gravitational potential energy of all bodies in this MultibodyPlant except those whose model instance has been explicitly excluded from gravity.
CalcConservativePower (const systems::Context< T > &context, const internal::PositionKinematicsCache< T > &pc, const internal::VelocityKinematicsCache< T > &vc) const final
 Returns gravitational power acting on the system.
CalcNonConservativePower (const systems::Context< T > &context, const internal::PositionKinematicsCache< T > &pc, const internal::VelocityKinematicsCache< T > &vc) const final
 Returns zero always since gravity is conservative.
Does not allow copy, move, or assignment
 UniformGravityFieldElement (const UniformGravityFieldElement &)=delete
UniformGravityFieldElementoperator= (const UniformGravityFieldElement &)=delete
 UniformGravityFieldElement (UniformGravityFieldElement &&)=delete
UniformGravityFieldElementoperator= (UniformGravityFieldElement &&)=delete
Public Member Functions inherited from ForceElement< T >
 ForceElement (ModelInstanceIndex model_instance)
 Default constructor for a generic force element.
 ~ForceElement () override
ForceElementIndex index () const
 Returns this element's unique index.
void CalcAndAddForceContribution (const systems::Context< T > &context, const internal::PositionKinematicsCache< T > &pc, const internal::VelocityKinematicsCache< T > &vc, MultibodyForces< T > *forces) const
 (Advanced) Computes the force contribution for this force element and adds it to the output arrays of forces.
 ForceElement (const ForceElement &)=delete
ForceElementoperator= (const ForceElement &)=delete
 ForceElement (ForceElement &&)=delete
ForceElementoperator= (ForceElement &&)=delete
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.
template<typename = void>
const MultibodyPlant< T > & GetParentPlant () const
 Returns the MultibodyPlant that owns this MultibodyElement.
void DeclareParameters (internal::MultibodyTreeSystem< T > *tree_system)
 Declares MultibodyTreeSystem Parameters at MultibodyTreeSystem::Finalize() time.
void SetDefaultParameters (systems::Parameters< T > *parameters) const
 Sets default values of parameters belonging to each MultibodyElement in parameters at a call to MultibodyTreeSystem::SetDefaultParameters().
void DeclareDiscreteState (internal::MultibodyTreeSystem< T > *tree_system)
 Declares MultibodyTreeSystem discrete states.
void DeclareCacheEntries (internal::MultibodyTreeSystem< T > *tree_system)
 (Advanced) Declares all cache entries needed by this element.
bool is_ephemeral () const
 Returns true if this MultibodyElement was added during Finalize() rather than something a user added.
void set_is_ephemeral (bool is_ephemeral)
 (Internal use only) Sets the is_ephemeral flag to the indicated value.
 MultibodyElement (const MultibodyElement &)=delete
MultibodyElementoperator= (const MultibodyElement &)=delete
 MultibodyElement (MultibodyElement &&)=delete
MultibodyElementoperator= (MultibodyElement &&)=delete

Static Public Attributes

static constexpr double kDefaultStrength = 9.81
 The strength used by our class's default constructor (i.e., on the Earth's surface).

Protected Member Functions

void DoCalcAndAddForceContribution (const systems::Context< T > &context, const internal::PositionKinematicsCache< T > &pc, const internal::VelocityKinematicsCache< T > &vc, MultibodyForces< T > *forces) const final
 This method is called only from the public non-virtual CalcAndAddForceContributions() which will already have error-checked the parameters so you don't have to.
std::unique_ptr< ForceElement< double > > DoCloneToScalar (const internal::MultibodyTree< double > &tree_clone) const override
 Clones this ForceElement (templated on T) to a mobilizer templated on double.
std::unique_ptr< ForceElement< AutoDiffXd > > DoCloneToScalar (const internal::MultibodyTree< AutoDiffXd > &tree_clone) const override
 Clones this ForceElement (templated on T) to a mobilizer templated on AutoDiffXd.
std::unique_ptr< ForceElement< symbolic::Expression > > DoCloneToScalar (const internal::MultibodyTree< symbolic::Expression > &) const override
std::unique_ptr< ForceElement< T > > DoShallowClone () const override
 NVI for ShallowClone().
Protected Member Functions inherited from ForceElement< T >
virtual void DoDeclareForceElementParameters (internal::MultibodyTreeSystem< T > *)
 Called by DoDeclareParameters().
virtual void DoSetDefaultForceElementParameters (systems::Parameters< T > *) const
 Called by DoSetDefaultParameters().
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.
 MultibodyElement (ModelInstanceIndex model_instance)
 Constructor which allows specifying a model instance.
 MultibodyElement (ModelInstanceIndex model_instance, int64_t index)
 Both the model instance and element index are specified.
template<typename ElementIndexType>
ElementIndexType index_impl () const
 Returns this element's unique index.
template<typename ElementOrdinalType = int64_t>
ElementOrdinalType ordinal_impl () const
 Returns this element's unique ordinal.
const internal::MultibodyTree< T > & get_parent_tree () const
 Returns a constant reference to the parent MultibodyTree that owns this element.
const internal::MultibodyTreeSystem< T > & GetParentTreeSystem () const
 Returns a constant reference to the parent MultibodyTreeSystem that owns the parent MultibodyTree that owns this element.
void SetTopology ()
 (Internal use only) Gives MultibodyElement-derived objects the opportunity to set data members that depend on topology and coordinate assignments having been finalized.
virtual void DoDeclareDiscreteState (internal::MultibodyTreeSystem< T > *)
 Implementation of the NVI DeclareDiscreteState().
virtual void DoDeclareCacheEntries (internal::MultibodyTreeSystem< T > *)
 Derived classes must override this method to declare cache entries needed by this element.
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().
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().
systems::DiscreteStateIndex DeclareDiscreteState (internal::MultibodyTreeSystem< T > *tree_system, const VectorX< T > &model_value)
 To be used by MultibodyElement-derived objects when declaring discrete states in their implementation of DoDeclareDiscreteStates().
systems::CacheEntryDeclareCacheEntry (internal::MultibodyTreeSystem< T > *tree_system, std::string description, systems::ValueProducer value_producer, std::set< systems::DependencyTicket > prerequisites_of_calc)
 To be used by MultibodyElement-derived objects when declaring cache entries in their implementation of DoDeclareCacheEntries().
bool has_parent_tree () const
 Returns true if this multibody element has a parent tree, otherwise false.

Constructor & Destructor Documentation

◆ UniformGravityFieldElement() [1/5]

template<typename T>
UniformGravityFieldElement ( const UniformGravityFieldElement< T > & )
delete

◆ UniformGravityFieldElement() [2/5]

template<typename T>
UniformGravityFieldElement ( UniformGravityFieldElement< T > && )
delete

◆ UniformGravityFieldElement() [3/5]

template<typename T>
UniformGravityFieldElement ( )

Constructs a uniform gravity field element with a default strength (on the Earth's surface) and direction (-z).

◆ UniformGravityFieldElement() [4/5]

template<typename T>
UniformGravityFieldElement ( Vector3< double > g_W)
explicit

Constructs a uniform gravity field element with a strength and direction given by the gravity acceleration vector g_W, expressed in the world frame W.

◆ UniformGravityFieldElement() [5/5]

template<typename T>
UniformGravityFieldElement ( Vector3< double > g_W,
std::set< ModelInstanceIndex > disabled_model_instances )

Constructs a uniform gravity field element with a strength and direction given by the gravity acceleration vector g_W, expressed in the world frame W.

Gravity is disabled for the set of model instances disabled_model_instances.

See also
set_enabled(), is_enabled()

◆ ~UniformGravityFieldElement()

template<typename T>
~UniformGravityFieldElement ( )
override

Member Function Documentation

◆ CalcConservativePower()

template<typename T>
T CalcConservativePower ( const systems::Context< T > & context,
const internal::PositionKinematicsCache< T > & pc,
const internal::VelocityKinematicsCache< T > & vc ) const
finalvirtual

Returns gravitational power acting on the system.

This is positive when gravitational potential energy is decreasing. In terms of the generalized gravity forces tau_g and the generalized velocities v, the gravitational power is v ⋅ tau_g.

See also
CalcGravityGeneralizedForces()

Implements ForceElement< T >.

◆ CalcGravityGeneralizedForces()

template<typename T>
VectorX< T > CalcGravityGeneralizedForces ( const systems::Context< T > & context) const

Computes the generalized forces tau_g(q) due to this gravity field element as a function of the generalized positions q stored in the input context, for the multibody model to which this element belongs.

tau_g(q) is defined such that it appears on the right hand side of the equations of motion together with any other generalized forces, like so:

  M(q)v̇ + C(q, v)v = tau_g(q) + tau_app

where tau_app are generalized forces generated by any other applied spatial forces (i.e., other than gravity forces).

Note
Given the gravitational potential energy PE(q), the generalized force at the iᵗʰ joint degree of freedom is tau_gᵢ = −∂(PE)/∂qᵢ.
Parameters
[in]contextThe context storing the state of the multibody model to which this element belongs.
Returns
tau_g A vector containing the generalized forces due to this gravity field force element. The ordering of the generalized forces is consistent with the vector of generalized velocities v for the parent MultibodyPlant so that the inner product v ⋅ tau_g is the gravitational power acting on the mechanical system. That is, v ⋅ tau_g > 0 corresponds to a decreasing gravitational potential energy and hence an increasing kinetic or potential energy, work done, or heat.

◆ CalcNonConservativePower()

template<typename T>
T CalcNonConservativePower ( const systems::Context< T > & context,
const internal::PositionKinematicsCache< T > & pc,
const internal::VelocityKinematicsCache< T > & vc ) const
finalvirtual

Returns zero always since gravity is conservative.

Implements ForceElement< T >.

◆ CalcPotentialEnergy()

template<typename T>
T CalcPotentialEnergy ( const systems::Context< T > & context,
const internal::PositionKinematicsCache< T > & pc ) const
finalvirtual

Computes the total gravitational potential energy of all bodies in this MultibodyPlant except those whose model instance has been explicitly excluded from gravity.

The definition of potential energy allows for an arbitrary choice of zero energy height. This element takes the zero energy height to be the same as the World origin height. That is, a body will have zero gravitational potential energy when the height of the body's center of mass in the World is zero.

Implements ForceElement< T >.

◆ DoCalcAndAddForceContribution()

template<typename T>
void DoCalcAndAddForceContribution ( const systems::Context< T > & context,
const internal::PositionKinematicsCache< T > & pc,
const internal::VelocityKinematicsCache< T > & vc,
MultibodyForces< T > * forces ) const
finalprotectedvirtual

This method is called only from the public non-virtual CalcAndAddForceContributions() which will already have error-checked the parameters so you don't have to.

Refer to the documentation for CalcAndAddForceContribution() for details describing the purpose and parameters of this method. It assumes forces to be a valid pointer to a MultibodyForces object compatible with the MultibodyTree model owning this force element.

Precondition
The position kinematics pc must have been previously updated with a call to CalcPositionKinematicsCache().
The velocity kinematics vc must have been previously updated with a call to CalcVelocityKinematicsCache().

Implements ForceElement< T >.

◆ DoCloneToScalar() [1/3]

template<typename T>
std::unique_ptr< ForceElement< AutoDiffXd > > DoCloneToScalar ( const internal::MultibodyTree< AutoDiffXd > & tree_clone) const
overrideprotectedvirtual

Clones this ForceElement (templated on T) to a mobilizer templated on AutoDiffXd.

Implements ForceElement< T >.

◆ DoCloneToScalar() [2/3]

template<typename T>
std::unique_ptr< ForceElement< double > > DoCloneToScalar ( const internal::MultibodyTree< double > & tree_clone) const
overrideprotectedvirtual

Clones this ForceElement (templated on T) to a mobilizer templated on double.

Implements ForceElement< T >.

◆ DoCloneToScalar() [3/3]

template<typename T>
std::unique_ptr< ForceElement< symbolic::Expression > > DoCloneToScalar ( const internal::MultibodyTree< symbolic::Expression > & ) const
overrideprotectedvirtual

Implements ForceElement< T >.

◆ DoShallowClone()

template<typename T>
std::unique_ptr< ForceElement< T > > DoShallowClone ( ) const
overrideprotectedvirtual

NVI for ShallowClone().

Reimplemented from ForceElement< T >.

◆ gravity_vector()

template<typename T>
const Vector3< double > & gravity_vector ( ) const

Returns the acceleration of gravity vector in m/s², expressed in the world frame W.

◆ is_enabled()

template<typename T>
bool is_enabled ( ModelInstanceIndex model_instance) const

Checks whether gravity is enabled for a given model instance.

Returns
true iff gravity is enabled for model_instance.
Exceptions
std::exceptionif the model instance is invalid.
std::exceptionif this element is not associated with a MultibodyPlant.

◆ operator=() [1/2]

template<typename T>
UniformGravityFieldElement & operator= ( const UniformGravityFieldElement< T > & )
delete

◆ operator=() [2/2]

template<typename T>
UniformGravityFieldElement & operator= ( UniformGravityFieldElement< T > && )
delete

◆ set_enabled()

template<typename T>
void set_enabled ( ModelInstanceIndex model_instance,
bool is_enabled )

Enables or disables gravity for a given model_instance.

Calls to this function are only permitted pre-Finalize().

Exceptions
ifthe parent model is finalized.
std::exceptionif the model instance is invalid.
std::exceptionif this element is not associated with a MultibodyPlant.

◆ set_gravity_vector()

template<typename T>
void set_gravity_vector ( const Vector3< double > & g_W)

Sets the gravity acceleration vector (in m/s²), expressed in the world frame W.

Member Data Documentation

◆ kDefaultStrength

template<typename T>
double kDefaultStrength = 9.81
staticconstexpr

The strength used by our class's default constructor (i.e., on the Earth's surface).

This is an unsigned (positive) value in m/s².


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