This ForceElement models a torsional spring attached to a RevoluteJoint and applies a torque to that joint.
τ = -k⋅(θ - θ₀)
where θ₀ is the nominal joint position. Note that joint damping exists within the RevoluteJoint itself, and so is not included here.
The k (stiffness) and θ₀ (nominal angle) specified in the constructor are kept as default values. These parameters are stored within the context and can be accessed and set by context dependent getters/setters.
T | The scalar type, which must be one of the default scalars. |
#include <drake/multibody/tree/revolute_spring.h>
Public Member Functions | |
RevoluteSpring (const RevoluteJoint< T > &joint, double nominal_angle, double stiffness) | |
Constructor for a spring attached to the given joint. More... | |
~RevoluteSpring () override | |
const RevoluteJoint< T > & | joint () const |
Returns the joint associated with this spring. More... | |
double | default_nominal_angle () const |
Returns the default spring reference angle θ₀ in radians. More... | |
double | default_stiffness () const |
Returns the default stiffness constant k in N⋅m/rad. More... | |
const T & | GetNominalAngle (const systems::Context< T > &context) const |
Returns the Context dependent nominal angle θ₀ stored as a parameter in context . More... | |
void | SetNominalAngle (systems::Context< T > *context, const T &nominal_angle) const |
Sets the value of the nominal angle θ₀ for this force element, stored as a parameter in context . More... | |
const T & | GetStiffness (const systems::Context< T > &context) const |
Returns the Context dependent stiffness coefficient k stored as a parameter in context . More... | |
void | SetStiffness (systems::Context< T > *context, const T &stiffness) const |
Sets the value of the linear stiffness coefficient k for this force element, stored as a parameter in context . More... | |
T | CalcPotentialEnergy (const systems::Context< T > &context, const internal::PositionKinematicsCache< T > &pc) const override |
(Advanced) Calculates the potential energy currently stored given the configuration provided in context . More... | |
T | CalcConservativePower (const systems::Context< T > &context, const internal::PositionKinematicsCache< T > &pc, const internal::VelocityKinematicsCache< T > &vc) const override |
(Advanced) Calculates and returns the power generated by conservative force elements or zero if this force element is non-conservative. More... | |
T | CalcNonConservativePower (const systems::Context< T > &context, const internal::PositionKinematicsCache< T > &pc, const internal::VelocityKinematicsCache< T > &vc) const override |
(Advanced) Calculates the rate at which mechanical energy is being generated (positive) or dissipated (negative) other than by conversion between potential and kinetic energy. More... | |
Does not allow copy, move, or assignment | |
RevoluteSpring (const RevoluteSpring &)=delete | |
RevoluteSpring & | operator= (const RevoluteSpring &)=delete |
RevoluteSpring (RevoluteSpring &&)=delete | |
RevoluteSpring & | operator= (RevoluteSpring &&)=delete |
![]() | |
ForceElement (ModelInstanceIndex model_instance) | |
Default constructor for a generic force element. More... | |
~ForceElement () override | |
ForceElementIndex | index () const |
Returns this element's unique index. More... | |
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. More... | |
ForceElement (const ForceElement &)=delete | |
ForceElement & | operator= (const ForceElement &)=delete |
ForceElement (ForceElement &&)=delete | |
ForceElement & | operator= (ForceElement &&)=delete |
![]() | |
virtual | ~MultibodyElement () |
ModelInstanceIndex | model_instance () const |
Returns the ModelInstanceIndex of the model instance to which this element belongs. More... | |
template<typename = void> | |
const MultibodyPlant< T > & | 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... | |
void | DeclareDiscreteState (internal::MultibodyTreeSystem< T > *tree_system) |
Declares MultibodyTreeSystem discrete states. More... | |
void | DeclareCacheEntries (internal::MultibodyTreeSystem< T > *tree_system) |
(Advanced) Declares all cache entries needed by this element. More... | |
bool | is_ephemeral () const |
Returns true if this MultibodyElement was added during Finalize() rather than something a user added. More... | |
void | set_is_ephemeral (bool is_ephemeral) |
(Internal use only) Sets the is_ephemeral flag to the indicated value. More... | |
MultibodyElement (const MultibodyElement &)=delete | |
MultibodyElement & | operator= (const MultibodyElement &)=delete |
MultibodyElement (MultibodyElement &&)=delete | |
MultibodyElement & | operator= (MultibodyElement &&)=delete |
Protected Member Functions | |
void | DoCalcAndAddForceContribution (const systems::Context< T > &context, const internal::PositionKinematicsCache< T > &pc, const internal::VelocityKinematicsCache< T > &vc, MultibodyForces< T > *forces) const override |
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. More... | |
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 . More... | |
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. More... | |
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(). More... | |
Methods to make a clone templated on different scalar types. | |
Specific force element subclasses must implement these to support scalar conversion to other types. These methods are only called from MultibodyTree::CloneToScalar(); users must not call these explicitly since there is no external mechanism to ensure the argument template <typename T> public: // Class's constructor. SpringElement( const RigidBody<T>& body1, const RigidBody<T>& body2, double stiffness); // Get the first body to which this spring is connected. const RigidBody<T>& get_body1() const; // Get the second body to which this spring is connected. const RigidBody<T>& get_body2() const; // Get the spring stiffness constant. double get_stiffness() const; protected: // Implementation of the scalar conversion from T to double. std::unique_ptr<ForceElement<double>> DoCloneToScalar( const MultibodyTree<double>& tree_clone) const) { const RigidBody<ToScalar>& body1_clone = tree_clone.get_variant(get_body1()); const RigidBody<ToScalar>& body2_clone = tree_clone.get_variant(get_body2()); return std::make_unique<SpringElement<double>>( body1_clone, body2_clone, get_stiffness()); } MultibodyTree::get_variant() methods are available to retrieve cloned variants from For examples on how a MultibodyTree model can be converted to other scalar types, please refer to the documentation for MultibodyTree::CloneToScalar(). | |
![]() | |
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... | |
template<typename ElementOrdinalType = int64_t> | |
ElementOrdinalType | 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... | |
virtual void | DoDeclareDiscreteState (internal::MultibodyTreeSystem< T > *) |
Implementation of the NVI DeclareDiscreteState(). More... | |
virtual void | DoDeclareCacheEntries (internal::MultibodyTreeSystem< T > *) |
Derived classes must override this method to declare cache entries needed by this element. 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... | |
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(). More... | |
systems::CacheEntry & | DeclareCacheEntry (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(). More... | |
bool | has_parent_tree () const |
Returns true if this multibody element has a parent tree, otherwise false. More... | |
Friends | |
template<typename U > | |
class | RevoluteSpring |
|
delete |
|
delete |
RevoluteSpring | ( | const RevoluteJoint< T > & | joint, |
double | nominal_angle, | ||
double | stiffness | ||
) |
Constructor for a spring attached to the given joint.
[in] | nominal_angle | The nominal angle of the spring θ₀, in radians, at which the spring applies no moment. |
[in] | stiffness | The stiffness k of the spring in N⋅m/rad. |
std::exception | if stiffness is negative. |
|
override |
|
overridevirtual |
(Advanced) Calculates and returns the power generated by conservative force elements or zero if this
force element is non-conservative.
This quantity is defined to be positive when the potential energy is decreasing. In other words, if PE
is the potential energy as defined by CalcPotentialEnergy(), then the conservative power, Pc
, is Pc = -d(PE)/dt
.
Implements ForceElement< T >.
|
overridevirtual |
(Advanced) Calculates the rate at which mechanical energy is being generated (positive) or dissipated (negative) other than by conversion between potential and kinetic energy.
Integrating this quantity yields work W, and the total energy E = PE + KE - W
should be conserved by any physically-correct model, to within integration accuracy of W.
Implements ForceElement< T >.
|
overridevirtual |
(Advanced) Calculates the potential energy currently stored given the configuration provided in context
.
Non-conservative force elements will return zero.
[in] | context | The context containing the state of the MultibodyTree model. |
[in] | pc | A position kinematics cache object already updated to be in sync with context . |
pc
must have been previously updated with a call to CalcPositionKinematicsCache().this
force element. For non-conservative force models, zero.Implements ForceElement< T >.
double default_nominal_angle | ( | ) | const |
Returns the default spring reference angle θ₀ in radians.
double default_stiffness | ( | ) | const |
Returns the default stiffness constant k in N⋅m/rad.
|
overrideprotectedvirtual |
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.
pc
must have been previously updated with a call to CalcPositionKinematicsCache(). vc
must have been previously updated with a call to CalcVelocityKinematicsCache(). Implements ForceElement< T >.
|
overrideprotectedvirtual |
Clones this ForceElement (templated on T) to a mobilizer templated on double
.
Implements ForceElement< T >.
|
overrideprotectedvirtual |
Clones this ForceElement (templated on T) to a mobilizer templated on AutoDiffXd.
Implements ForceElement< T >.
|
overrideprotectedvirtual |
Implements ForceElement< T >.
|
overrideprotectedvirtual |
NVI for ShallowClone().
Reimplemented from ForceElement< T >.
const T& GetNominalAngle | ( | const systems::Context< T > & | context | ) | const |
Returns the Context dependent nominal angle θ₀ stored as a parameter in context
.
[in] | context | The context storing the state and parameters for the model to which this spring belongs. |
returns | the nominal angle θ₀ in radians. |
const T& GetStiffness | ( | const systems::Context< T > & | context | ) | const |
Returns the Context dependent stiffness coefficient k stored as a parameter in context
.
[in] | context | The context storing the state and parameters for the model to which this spring belongs. |
returns | the stiffness k in N⋅m/rad stored within the context. |
const RevoluteJoint<T>& joint | ( | ) | const |
Returns the joint associated with this spring.
std::exception | if this element is not associated with a MultibodyPlant. |
|
delete |
|
delete |
void SetNominalAngle | ( | systems::Context< T > * | context, |
const T & | nominal_angle | ||
) | const |
Sets the value of the nominal angle θ₀ for this force element, stored as a parameter in context
.
[out] | context | The context storing the state and parameters for the model to which this spring belongs. |
[in] | nominal_angle | The nominal angle θ₀ in radians stored within the context. |
void SetStiffness | ( | systems::Context< T > * | context, |
const T & | stiffness | ||
) | const |
Sets the value of the linear stiffness coefficient k for this force element, stored as a parameter in context
.
[out] | context | The context storing the state and parameters for the model to which this spring belongs. |
[in] | stiffness | The stiffness value k with units N⋅m/rad. |
std::exception | if stiffness is negative. |
|
friend |