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.
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 |
double | nominal_angle () const |
double | stiffness () const |
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 |
Public Member Functions inherited from ForceElement< T > | |
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 |
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 | |
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 |
Protected Member Functions inherited from ForceElement< T > | |
virtual void | DoDeclareForceElementParameters (internal::MultibodyTreeSystem< T > *) |
Called by DoDeclareParameters(). More... | |
virtual void | DoSetDefaultForceElementParameters (systems::Parameters< T > *) const |
Called by DoSetDefaultParameters(). More... | |
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... | |
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 >.
|
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 >.
const RevoluteJoint<T>& joint | ( | ) | const |
double nominal_angle | ( | ) | const |
|
delete |
|
delete |
double stiffness | ( | ) | const |
|
friend |