Drake
Drake C++ Documentation
AxiallySymmetricFreeBodyPlant< T > Class Template Referencefinal

Detailed Description

template<typename T>
class drake::multibody::test::AxiallySymmetricFreeBodyPlant< T >

This plant models the free motion of a torque free body in space.

This body is axially symmetric with rotational inertia about its axis of revolution J and with a rotational inertia I about any axis perpendicular to its axis of revolution. This particular case has a nice closed form analytical solution which we have implemented in drake::multibody::benchmarks::free_body::FreeBody.

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

#include <drake/multibody/test_utilities/floating_body_plant.h>

Public Member Functions

 AxiallySymmetricFreeBodyPlant (double mass, double I, double J, double g, double time_step=0.0)
 Constructor from known rotational inertia values. More...
 
template<typename U >
 AxiallySymmetricFreeBodyPlant (const AxiallySymmetricFreeBodyPlant< U > &)
 Scalar-converting copy constructor. More...
 
void SetDefaultState (const systems::Context< T > &context, systems::State< T > *state) const override
 Sets state to a default value corresponding to a configuration in which the free body frame B is coincident with the world frame W and the angular and translational velocities have a value as returned by get_default_initial_angular_velocity() and get_default_initial_translational_velocity(), respectively. More...
 
Vector3< T > get_angular_velocity (const systems::Context< T > &context) const
 Returns the angular velocity w_WB stored in context of the free body B in the world frame W. More...
 
Vector3< T > get_translational_velocity (const systems::Context< T > &context) const
 Returns the translational velocity v_WB stored in context of the free body B in the world frame W. More...
 
math::RigidTransform< T > CalcPoseInWorldFrame (const systems::Context< T > &context) const
 Computes the pose X_WB of the body in the world frame. More...
 
SpatialVelocity< T > CalcSpatialVelocityInWorldFrame (const systems::Context< T > &context) const
 Calculates V_WB, this body B's spatial velocity in the world frame W. More...
 
const RigidBody< T > & body () const
 Returns a constant reference to the free body model of this plant. More...
 
Does not allow copy, move, or assignment
 AxiallySymmetricFreeBodyPlant (const AxiallySymmetricFreeBodyPlant &)=delete
 
AxiallySymmetricFreeBodyPlantoperator= (const AxiallySymmetricFreeBodyPlant &)=delete
 
 AxiallySymmetricFreeBodyPlant (AxiallySymmetricFreeBodyPlant &&)=delete
 
AxiallySymmetricFreeBodyPlantoperator= (AxiallySymmetricFreeBodyPlant &&)=delete
 
- Public Member Functions inherited from MultibodyPlant< T >
 MultibodyPlant (const MultibodyPlant &)=delete
 
MultibodyPlantoperator= (const MultibodyPlant &)=delete
 
 MultibodyPlant (MultibodyPlant &&)=delete
 
MultibodyPlantoperator= (MultibodyPlant &&)=delete
 
const systems::OutputPort< T > & get_body_poses_output_port () const
 Returns the output port of all body poses in the world frame. More...
 
const systems::OutputPort< T > & get_body_spatial_velocities_output_port () const
 Returns the output port of all body spatial velocities in the world frame. More...
 
const systems::OutputPort< T > & get_body_spatial_accelerations_output_port () const
 Returns the output port of all body spatial accelerations in the world frame. More...
 
const systems::InputPort< T > & get_actuation_input_port () const
 Returns a constant reference to the input port for external actuation for all actuated dofs. More...
 
const systems::InputPort< T > & get_actuation_input_port (ModelInstanceIndex model_instance) const
 Returns a constant reference to the input port for external actuation for a specific model instance. More...
 
const systems::OutputPort< T > & get_net_actuation_output_port () const
 Returns a constant reference to the output port that reports actuation values applied through joint actuators. More...
 
const systems::OutputPort< T > & get_net_actuation_output_port (ModelInstanceIndex model_instance) const
 Returns a constant reference to the output port that reports actuation values applied through joint actuators, for a specific model instance. More...
 
const systems::InputPort< T > & get_desired_state_input_port (ModelInstanceIndex model_instance) const
 For models with PD controlled joint actuators, returns the port to provide the desired state for the full model_instance. More...
 
const systems::InputPort< T > & get_applied_generalized_force_input_port () const
 Returns a constant reference to the vector-valued input port for applied generalized forces, and the vector will be added directly into tau (see System dynamics). More...
 
const systems::InputPort< T > & get_applied_spatial_force_input_port () const
 Returns a constant reference to the input port for applying spatial forces to bodies in the plant. More...
 
const systems::InputPort< T > & get_geometry_query_input_port () const
 Returns a constant reference to the input port used to perform geometric queries on a SceneGraph. More...
 
const systems::OutputPort< T > & get_state_output_port () const
 Returns a constant reference to the output port for the multibody state x = [q, v] of the model. More...
 
const systems::OutputPort< T > & get_state_output_port (ModelInstanceIndex model_instance) const
 Returns a constant reference to the output port for the state xᵢ = [qᵢ vᵢ] of model instance i. More...
 
const systems::OutputPort< T > & get_generalized_acceleration_output_port () const
 Returns a constant reference to the output port for generalized accelerations v̇ of the model. More...
 
const systems::OutputPort< T > & get_generalized_acceleration_output_port (ModelInstanceIndex model_instance) const
 Returns a constant reference to the output port for the generalized accelerations v̇ᵢ ⊆ v̇ for model instance i. More...
 
const systems::OutputPort< T > & get_generalized_contact_forces_output_port (ModelInstanceIndex model_instance) const
 Returns a constant reference to the output port of generalized contact forces for a specific model instance. More...
 
const systems::OutputPort< T > & get_reaction_forces_output_port () const
 Returns the port for joint reaction forces. More...
 
const systems::OutputPort< T > & get_contact_results_output_port () const
 Returns a constant reference to the port that outputs ContactResults. More...
 
const systems::OutputPort< T > & get_geometry_poses_output_port () const
 Returns the output port of frames' poses to communicate with a SceneGraph. More...
 
 MultibodyPlant (double time_step)
 This constructor creates a plant with a single "world" body. More...
 
template<typename U >
 MultibodyPlant (const MultibodyPlant< U > &other)
 Scalar-converting copy constructor. See System Scalar Conversion. More...
 
const RigidBody< T > & AddRigidBody (const std::string &name, ModelInstanceIndex model_instance, const SpatialInertia< double > &M_BBo_B=SpatialInertia< double >::Zero())
 Creates a rigid body with the provided name and spatial inertia. More...
 
const RigidBody< T > & AddRigidBody (const std::string &name, const SpatialInertia< double > &M_BBo_B=SpatialInertia< double >::Zero())
 Creates a rigid body with the provided name and spatial inertia. More...
 
template<template< typename > class FrameType>
const FrameType< T > & AddFrame (std::unique_ptr< FrameType< T >> frame)
 This method adds a Frame of type FrameType<T>. More...
 
template<template< typename Scalar > class JointType>
const JointType< T > & AddJoint (std::unique_ptr< JointType< T >> joint)
 This method adds a Joint of type JointType between two bodies. More...
 
template<template< typename > class JointType, typename... Args>
const JointType< T > & AddJoint (const std::string &name, const RigidBody< T > &parent, const std::optional< math::RigidTransform< double >> &X_PF, const RigidBody< T > &child, const std::optional< math::RigidTransform< double >> &X_BM, Args &&... args)
 This method adds a Joint of type JointType between two bodies. More...
 
const WeldJoint< T > & WeldFrames (const Frame< T > &frame_on_parent_F, const Frame< T > &frame_on_child_M, const math::RigidTransform< double > &X_FM=math::RigidTransform< double >::Identity())
 Welds frame_on_parent_F and frame_on_child_M with relative pose X_FM. More...
 
template<template< typename Scalar > class ForceElementType, typename... Args>
const ForceElementType< T > & AddForceElement (Args &&... args)
 Adds a new force element model of type ForceElementType to this MultibodyPlant. More...
 
const JointActuator< T > & AddJointActuator (const std::string &name, const Joint< T > &joint, double effort_limit=std::numeric_limits< double >::infinity())
 Creates and adds a JointActuator model for an actuator acting on a given joint. More...
 
void RemoveJointActuator (const JointActuator< T > &actuator)
 Removes and deletes actuator from this MultibodyPlant. More...
 
ModelInstanceIndex AddModelInstance (const std::string &name)
 Creates a new model instance. More...
 
void RenameModelInstance (ModelInstanceIndex model_instance, const std::string &name)
 Renames an existing model instance. More...
 
void Finalize ()
 This method must be called after all elements in the model (joints, bodies, force elements, constraints, etc.) are added and before any computations are performed. More...
 
int num_constraints () const
 Returns the total number of constraints specified by the user. More...
 
int num_coupler_constraints () const
 Returns the total number of coupler constraints specified by the user. More...
 
int num_distance_constraints () const
 Returns the total number of distance constraints specified by the user. More...
 
int num_ball_constraints () const
 Returns the total number of ball constraints specified by the user. More...
 
int num_weld_constraints () const
 Returns the total number of weld constraints specified by the user. More...
 
const internal::CouplerConstraintSpec & get_coupler_constraint_specs (MultibodyConstraintId id) const
 (Internal use only) Returns the coupler constraint specification corresponding to id More...
 
const internal::DistanceConstraintSpec & get_distance_constraint_specs (MultibodyConstraintId id) const
 (Internal use only) Returns the distance constraint specification corresponding to id More...
 
const internal::BallConstraintSpec & get_ball_constraint_specs (MultibodyConstraintId id) const
 (Internal use only) Returns the ball constraint specification corresponding to id More...
 
const internal::WeldConstraintSpec & get_weld_constraint_specs (MultibodyConstraintId id) const
 (Internal use only) Returns the weld constraint specification corresponding to id More...
 
const std::map< MultibodyConstraintId, internal::CouplerConstraintSpec > & get_coupler_constraint_specs () const
 (Internal use only) Returns a reference to the all of the coupler constraints in this plant as a map from MultibodyConstraintId to CouplerConstraintSpec. More...
 
const std::map< MultibodyConstraintId, internal::DistanceConstraintSpec > & get_distance_constraint_specs () const
 (Internal use only) Returns a reference to the all of the distance constraints in this plant as a map from MultibodyConstraintId to DistanceConstraintSpec. More...
 
const std::map< MultibodyConstraintId, internal::BallConstraintSpec > & get_ball_constraint_specs () const
 (Internal use only) Returns a reference to all of the ball constraints in this plant as a map from MultibodyConstraintId to BallConstraintSpec. More...
 
const std::map< MultibodyConstraintId, internal::WeldConstraintSpec > & get_weld_constraint_specs () const
 (Internal use only) Returns a reference to the all of the weld constraints in this plant as a map from MultibodyConstraintId to WeldConstraintSpec. More...
 
bool GetConstraintActiveStatus (const systems::Context< T > &context, MultibodyConstraintId id) const
 Returns the active status of the constraint given by id in context. More...
 
void SetConstraintActiveStatus (systems::Context< T > *context, MultibodyConstraintId id, bool status) const
 Sets the active status of the constraint given by id in context. More...
 
MultibodyConstraintId AddCouplerConstraint (const Joint< T > &joint0, const Joint< T > &joint1, double gear_ratio, double offset=0.0)
 Defines a holonomic constraint between two single-dof joints joint0 and joint1 with positions q₀ and q₁, respectively, such that q₀ = ρ⋅q₁ + Δq, where ρ is the gear ratio and Δq is a fixed offset. More...
 
MultibodyConstraintId AddDistanceConstraint (const RigidBody< T > &body_A, const Vector3< double > &p_AP, const RigidBody< T > &body_B, const Vector3< double > &p_BQ, double distance, double stiffness=std::numeric_limits< double >::infinity(), double damping=0.0)
 Defines a distance constraint between a point P on a body A and a point Q on a body B. More...
 
MultibodyConstraintId AddBallConstraint (const RigidBody< T > &body_A, const Vector3< double > &p_AP, const RigidBody< T > &body_B, const Vector3< double > &p_BQ)
 Defines a constraint such that point P affixed to body A is coincident at all times with point Q affixed to body B, effectively modeling a ball-and-socket joint. More...
 
MultibodyConstraintId AddWeldConstraint (const RigidBody< T > &body_A, const math::RigidTransform< double > &X_AP, const RigidBody< T > &body_B, const math::RigidTransform< double > &X_BQ)
 Defines a constraint such that frame P affixed to body A is coincident at all times with frame Q affixed to body B, effectively modeling a weld joint. More...
 
void RemoveConstraint (MultibodyConstraintId id)
 Removes the constraint id from the plant. More...
 
geometry::SourceId RegisterAsSourceForSceneGraph (geometry::SceneGraph< T > *scene_graph)
 Registers this plant to serve as a source for an instance of SceneGraph. More...
 
geometry::GeometryId RegisterVisualGeometry (const RigidBody< T > &body, const math::RigidTransform< double > &X_BG, const geometry::Shape &shape, const std::string &name, const geometry::IllustrationProperties &properties)
 Registers geometry in a SceneGraph with a given geometry::Shape to be used for visualization of a given body. More...
 
geometry::GeometryId RegisterVisualGeometry (const RigidBody< T > &body, const math::RigidTransform< double > &X_BG, const geometry::Shape &shape, const std::string &name, const Vector4< double > &diffuse_color)
 Overload for visual geometry registration; it converts the diffuse_color (RGBA with values in the range [0, 1]) into a geometry::DrakeVisualizer-compatible set of geometry::IllustrationProperties. More...
 
geometry::GeometryId RegisterVisualGeometry (const RigidBody< T > &body, const math::RigidTransform< double > &X_BG, const geometry::Shape &shape, const std::string &name)
 Overload for visual geometry registration; it relies on the downstream geometry::IllustrationProperties consumer to provide default parameter values (see Geometry Queries and Roles for details). More...
 
const std::vector< geometry::GeometryId > & GetVisualGeometriesForBody (const RigidBody< T > &body) const
 Returns an array of GeometryId's identifying the different visual geometries for body previously registered with a SceneGraph. More...
 
geometry::GeometryId RegisterCollisionGeometry (const RigidBody< T > &body, const math::RigidTransform< double > &X_BG, const geometry::Shape &shape, const std::string &name, geometry::ProximityProperties properties)
 Registers geometry in a SceneGraph with a given geometry::Shape to be used for the contact modeling of a given body. More...
 
geometry::GeometryId RegisterCollisionGeometry (const RigidBody< T > &body, const math::RigidTransform< double > &X_BG, const geometry::Shape &shape, const std::string &name, const CoulombFriction< double > &coulomb_friction)
 Overload which specifies a single property: coulomb_friction. More...
 
const std::vector< geometry::GeometryId > & GetCollisionGeometriesForBody (const RigidBody< T > &body) const
 Returns an array of GeometryId's identifying the different contact geometries for body previously registered with a SceneGraph. More...
 
void ExcludeCollisionGeometriesWithCollisionFilterGroupPair (const std::pair< std::string, geometry::GeometrySet > &collision_filter_group_a, const std::pair< std::string, geometry::GeometrySet > &collision_filter_group_b)
 Excludes the rigid collision geometries between two given collision filter groups. More...
 
geometry::GeometrySet CollectRegisteredGeometries (const std::vector< const RigidBody< T > * > &bodies) const
 For each of the provided bodies, collects up all geometries that have been registered to that body. More...
 
const RigidBody< T > * GetBodyFromFrameId (geometry::FrameId frame_id) const
 Given a geometry frame identifier, returns a pointer to the body associated with that id (nullptr if there is no such body). More...
 
std::optional< geometry::FrameIdGetBodyFrameIdIfExists (BodyIndex body_index) const
 If the body with body_index belongs to the called plant, it returns the geometry::FrameId associated with it. More...
 
geometry::FrameId GetBodyFrameIdOrThrow (BodyIndex body_index) const
 If the body with body_index belongs to the called plant, it returns the geometry::FrameId associated with it. More...
 
Eigen::VectorBlock< const VectorX< T > > GetPositionsAndVelocities (const systems::Context< T > &context) const
 Returns a const vector reference [q; v] to the generalized positions q and generalized velocities v in a given Context. More...
 
VectorX< T > GetPositionsAndVelocities (const systems::Context< T > &context, ModelInstanceIndex model_instance) const
 Returns a vector [q; v] containing the generalized positions q and generalized velocities v of a specified model instance in a given Context. More...
 
void GetPositionsAndVelocities (const systems::Context< T > &context, ModelInstanceIndex model_instance, EigenPtr< VectorX< T >> qv_out) const
 (Advanced) Populates output vector qv_out representing the generalized positions q and generalized velocities v of a specified model instance in a given Context. More...
 
void SetPositionsAndVelocities (systems::Context< T > *context, const Eigen::Ref< const VectorX< T >> &q_v) const
 Sets generalized positions q and generalized velocities v in a given Context from a given vector [q; v]. More...
 
void SetPositionsAndVelocities (systems::Context< T > *context, ModelInstanceIndex model_instance, const Eigen::Ref< const VectorX< T >> &q_v) const
 Sets generalized positions q and generalized velocities v from a given vector [q; v] for a specified model instance in a given Context. More...
 
Eigen::VectorBlock< const VectorX< T > > GetPositions (const systems::Context< T > &context) const
 Returns a const vector reference to the vector of generalized positions q in a given Context. More...
 
VectorX< T > GetPositions (const systems::Context< T > &context, ModelInstanceIndex model_instance) const
 Returns a vector containing the generalized positions q of a specified model instance in a given Context. More...
 
void GetPositions (const systems::Context< T > &context, ModelInstanceIndex model_instance, EigenPtr< VectorX< T >> q_out) const
 (Advanced) Populates output vector q_out with the generalized positions q of a specified model instance in a given Context. More...
 
void SetPositions (systems::Context< T > *context, const Eigen::Ref< const VectorX< T >> &q) const
 Sets the generalized positions q in a given Context from a given vector. More...
 
void SetPositions (systems::Context< T > *context, ModelInstanceIndex model_instance, const Eigen::Ref< const VectorX< T >> &q_instance) const
 Sets the generalized positions q for a particular model instance in a given Context from a given vector. More...
 
void SetPositions (const systems::Context< T > &context, systems::State< T > *state, ModelInstanceIndex model_instance, const Eigen::Ref< const VectorX< T >> &q_instance) const
 (Advanced) Sets the generalized positions q for a particular model instance in a given State from a given vector. More...
 
VectorX< T > GetDefaultPositions () const
 Gets the default positions for the plant, which can be changed via SetDefaultPositions(). More...
 
VectorX< T > GetDefaultPositions (ModelInstanceIndex model_instance) const
 Gets the default positions for the plant for a given model instance, which can be changed via SetDefaultPositions(). More...
 
void SetDefaultPositions (const Eigen::Ref< const Eigen::VectorXd > &q)
 Sets the default positions for the plant. More...
 
void SetDefaultPositions (ModelInstanceIndex model_instance, const Eigen::Ref< const Eigen::VectorXd > &q_instance)
 Sets the default positions for the model instance. More...
 
Eigen::VectorBlock< const VectorX< T > > GetVelocities (const systems::Context< T > &context) const
 Returns a const vector reference to the generalized velocities v in a given Context. More...
 
VectorX< T > GetVelocities (const systems::Context< T > &context, ModelInstanceIndex model_instance) const
 Returns a vector containing the generalized velocities v of a specified model instance in a given Context. More...
 
void GetVelocities (const systems::Context< T > &context, ModelInstanceIndex model_instance, EigenPtr< VectorX< T >> v_out) const
 (Advanced) Populates output vector v_out with the generalized velocities v of a specified model instance in a given Context. More...
 
void SetVelocities (systems::Context< T > *context, const Eigen::Ref< const VectorX< T >> &v) const
 Sets the generalized velocities v in a given Context from a given vector. More...
 
void SetVelocities (systems::Context< T > *context, ModelInstanceIndex model_instance, const Eigen::Ref< const VectorX< T >> &v_instance) const
 Sets the generalized velocities v for a particular model instance in a given Context from a given vector. More...
 
void SetVelocities (const systems::Context< T > &context, systems::State< T > *state, ModelInstanceIndex model_instance, const Eigen::Ref< const VectorX< T >> &v_instance) const
 (Advanced) Sets the generalized velocities v for a particular model instance in a given State from a given vector. More...
 
void SetDefaultState (const systems::Context< T > &context, systems::State< T > *state) const override
 Sets state according to defaults set by the user for joints (e.g. More...
 
void SetRandomState (const systems::Context< T > &context, systems::State< T > *state, RandomGenerator *generator) const override
 Assigns random values to all elements of the state, by drawing samples independently for each joint/free body (coming soon: and then solving a mathematical program to "project" these samples onto the registered system constraints). More...
 
std::vector< std::string > GetPositionNames (bool add_model_instance_prefix=true, bool always_add_suffix=true) const
 Returns a list of string names corresponding to each element of the position vector. More...
 
std::vector< std::string > GetPositionNames (ModelInstanceIndex model_instance, bool add_model_instance_prefix=false, bool always_add_suffix=true) const
 Returns a list of string names corresponding to each element of the position vector. More...
 
std::vector< std::string > GetVelocityNames (bool add_model_instance_prefix=true, bool always_add_suffix=true) const
 Returns a list of string names corresponding to each element of the velocity vector. More...
 
std::vector< std::string > GetVelocityNames (ModelInstanceIndex model_instance, bool add_model_instance_prefix=false, bool always_add_suffix=true) const
 Returns a list of string names corresponding to each element of the velocity vector. More...
 
std::vector< std::string > GetStateNames (bool add_model_instance_prefix=true) const
 Returns a list of string names corresponding to each element of the multibody state vector. More...
 
std::vector< std::string > GetStateNames (ModelInstanceIndex model_instance, bool add_model_instance_prefix=false) const
 Returns a list of string names corresponding to each element of the multibody state vector. More...
 
std::vector< std::string > GetActuatorNames (bool add_model_instance_prefix=true) const
 Returns a list of string names corresponding to each element of the actuation vector. More...
 
std::vector< std::string > GetActuatorNames (ModelInstanceIndex model_instance, bool add_model_instance_prefix=false) const
 Returns a list of string names corresponding to each element of the actuation vector. More...
 
VectorX< T > GetActuationFromArray (ModelInstanceIndex model_instance, const Eigen::Ref< const VectorX< T >> &u) const
 Returns a vector of actuation values for model_instance from a vector u of actuation values for the entire plant model. More...
 
void SetActuationInArray (ModelInstanceIndex model_instance, const Eigen::Ref< const VectorX< T >> &u_instance, EigenPtr< VectorX< T >> u) const
 Given actuation values u_instance for the actuators in model_instance, this function updates the actuation vector u for the entire plant model to which this actuator belongs to. More...
 
VectorX< T > GetPositionsFromArray (ModelInstanceIndex model_instance, const Eigen::Ref< const VectorX< T >> &q) const
 Returns a vector of generalized positions for model_instance from a vector q_array of generalized positions for the entire model model. More...
 
void GetPositionsFromArray (ModelInstanceIndex model_instance, const Eigen::Ref< const VectorX< T >> &q, EigenPtr< VectorX< T >> q_out) const
 (Advanced) Populates output vector q_out and with the generalized positions for model_instance from a vector q of generalized positions for the entire model. More...
 
void SetPositionsInArray (ModelInstanceIndex model_instance, const Eigen::Ref< const VectorX< T >> &q_instance, EigenPtr< VectorX< T >> q) const
 Sets the vector of generalized positions for model_instance in q using q_instance, leaving all other elements in the array untouched. More...
 
VectorX< T > GetVelocitiesFromArray (ModelInstanceIndex model_instance, const Eigen::Ref< const VectorX< T >> &v) const
 Returns a vector of generalized velocities for model_instance from a vector v of generalized velocities for the entire MultibodyPlant model. More...
 
void GetVelocitiesFromArray (ModelInstanceIndex model_instance, const Eigen::Ref< const VectorX< T >> &v, EigenPtr< VectorX< T >> v_out) const
 (Advanced) Populates output vector v_out with the generalized velocities for model_instance from a vector v of generalized velocities for the entire model. More...
 
void SetVelocitiesInArray (ModelInstanceIndex model_instance, const Eigen::Ref< const VectorX< T >> &v_instance, EigenPtr< VectorX< T >> v) const
 Sets the vector of generalized velocities for model_instance in v using v_instance, leaving all other elements in the array untouched. More...
 
std::unordered_set< BodyIndexGetFloatingBaseBodies () const
 Returns the set of body indexes corresponding to the free (floating) bodies in the model, in no particular order. More...
 
math::RigidTransform< T > GetFreeBodyPose (const systems::Context< T > &context, const RigidBody< T > &body) const
 Gets the pose of a given body in the world frame W. More...
 
void SetFreeBodyPose (systems::Context< T > *context, const RigidBody< T > &body, const math::RigidTransform< T > &X_WB) const
 Sets context to store the pose X_WB of a given body B in the world frame W. More...
 
void SetFreeBodyPose (const systems::Context< T > &context, systems::State< T > *state, const RigidBody< T > &body, const math::RigidTransform< T > &X_WB) const
 Sets state to store the pose X_WB of a given body B in the world frame W, for a given context of this model. More...
 
void SetDefaultFreeBodyPose (const RigidBody< T > &body, const math::RigidTransform< double > &X_WB)
 Sets the default pose of body. More...
 
math::RigidTransform< doubleGetDefaultFreeBodyPose (const RigidBody< T > &body) const
 Gets the default pose of body as set by SetDefaultFreeBodyPose(). More...
 
void SetFreeBodySpatialVelocity (systems::Context< T > *context, const RigidBody< T > &body, const SpatialVelocity< T > &V_WB) const
 Sets context to store the spatial velocity V_WB of a given body B in the world frame W. More...
 
void SetFreeBodySpatialVelocity (const systems::Context< T > &context, systems::State< T > *state, const RigidBody< T > &body, const SpatialVelocity< T > &V_WB) const
 Sets state to store the spatial velocity V_WB of a given body B in the world frame W, for a given context of this model. More...
 
void SetFreeBodyRandomPositionDistribution (const RigidBody< T > &body, const Vector3< symbolic::Expression > &position)
 Sets the distribution used by SetRandomState() to populate the free body's x-y-z position with respect to World. More...
 
void SetFreeBodyRandomRotationDistribution (const RigidBody< T > &body, const Eigen::Quaternion< symbolic::Expression > &rotation)
 Sets the distribution used by SetRandomState() to populate the free body's rotation with respect to World. More...
 
void SetFreeBodyRandomRotationDistributionToUniform (const RigidBody< T > &body)
 Sets the distribution used by SetRandomState() to populate the free body's rotation with respect to World using uniformly random rotations. More...
 
void SetFreeBodyPoseInWorldFrame (systems::Context< T > *context, const RigidBody< T > &body, const math::RigidTransform< T > &X_WB) const
 Sets context to store the pose X_WB of a given body B in the world frame W. More...
 
void SetFreeBodyPoseInAnchoredFrame (systems::Context< T > *context, const Frame< T > &frame_F, const RigidBody< T > &body, const math::RigidTransform< T > &X_FB) const
 Updates context to store the pose X_FB of a given body B in a frame F. More...
 
const RigidBody< T > & GetUniqueFreeBaseBodyOrThrow (ModelInstanceIndex model_instance) const
 If there exists a unique base body that belongs to the model given by model_instance and that unique base body is free (see HasUniqueBaseBody()), return that free body. More...
 
bool HasUniqueFreeBaseBody (ModelInstanceIndex model_instance) const
 Return true if there exists a unique base body in the model given by model_instance and that unique base body is free. More...
 
const math::RigidTransform< T > & EvalBodyPoseInWorld (const systems::Context< T > &context, const RigidBody< T > &body_B) const
 Evaluate the pose X_WB of a body B in the world frame W. More...
 
const SpatialVelocity< T > & EvalBodySpatialVelocityInWorld (const systems::Context< T > &context, const RigidBody< T > &body_B) const
 Evaluates V_WB, body B's spatial velocity in the world frame W. More...
 
const SpatialAcceleration< T > & EvalBodySpatialAccelerationInWorld (const systems::Context< T > &context, const RigidBody< T > &body_B) const
 Evaluates A_WB, body B's spatial acceleration in the world frame W. More...
 
const std::vector< geometry::PenetrationAsPointPair< T > > & EvalPointPairPenetrations (const systems::Context< T > &context) const
 Evaluates all point pairs of contact for a given state of the model stored in context. More...
 
math::RigidTransform< T > CalcRelativeTransform (const systems::Context< T > &context, const Frame< T > &frame_F, const Frame< T > &frame_G) const
 Calculates the rigid transform (pose) X_FG relating frame F and frame G. More...
 
math::RotationMatrix< T > CalcRelativeRotationMatrix (const systems::Context< T > &context, const Frame< T > &frame_F, const Frame< T > &frame_G) const
 Calculates the rotation matrix R_FG relating frame F and frame G. More...
 
void CalcPointsPositions (const systems::Context< T > &context, const Frame< T > &frame_B, const Eigen::Ref< const MatrixX< T >> &p_BQi, const Frame< T > &frame_A, EigenPtr< MatrixX< T >> p_AQi) const
 Given the positions p_BQi for a set of points Qi measured and expressed in a frame B, this method computes the positions p_AQi(q) of each point Qi in the set as measured and expressed in another frame A, as a function of the generalized positions q of the model. More...
 
CalcTotalMass (const systems::Context< T > &context) const
 Calculates the total mass of all bodies in this MultibodyPlant. More...
 
CalcTotalMass (const systems::Context< T > &context, const std::vector< ModelInstanceIndex > &model_instances) const
 Calculates the total mass of all bodies contained in model_instances. More...
 
Vector3< T > CalcCenterOfMassPositionInWorld (const systems::Context< T > &context) const
 Calculates the position vector from the world origin Wo to the center of mass of all bodies in this MultibodyPlant, expressed in the world frame W. More...
 
Vector3< T > CalcCenterOfMassPositionInWorld (const systems::Context< T > &context, const std::vector< ModelInstanceIndex > &model_instances) const
 Calculates the position vector from the world origin Wo to the center of mass of all non-world bodies contained in model_instances, expressed in the world frame W. More...
 
SpatialInertia< T > CalcSpatialInertia (const systems::Context< T > &context, const Frame< T > &frame_F, const std::vector< BodyIndex > &body_indexes) const
 Returns M_SFo_F, the spatial inertia of a set S of bodies about point Fo (the origin of a frame F), expressed in frame F. More...
 
Vector3< T > CalcCenterOfMassTranslationalVelocityInWorld (const systems::Context< T > &context) const
 Calculates system center of mass translational velocity in world frame W. More...
 
Vector3< T > CalcCenterOfMassTranslationalVelocityInWorld (const systems::Context< T > &context, const std::vector< ModelInstanceIndex > &model_instances) const
 Calculates system center of mass translational velocity in world frame W. More...
 
SpatialMomentum< T > CalcSpatialMomentumInWorldAboutPoint (const systems::Context< T > &context, const Vector3< T > &p_WoP_W) const
 This method returns the spatial momentum of this MultibodyPlant in the world frame W, about a designated point P, expressed in the world frame W. More...
 
SpatialMomentum< T > CalcSpatialMomentumInWorldAboutPoint (const systems::Context< T > &context, const std::vector< ModelInstanceIndex > &model_instances, const Vector3< T > &p_WoP_W) const
 This method returns the spatial momentum of a set of model instances in the world frame W, about a designated point P, expressed in frame W. More...
 
void CalcSpatialAccelerationsFromVdot (const systems::Context< T > &context, const VectorX< T > &known_vdot, std::vector< SpatialAcceleration< T >> *A_WB_array) const
 Given the state of this model in context and a known vector of generalized accelerations known_vdot, this method computes the spatial acceleration A_WB for each body as measured and expressed in the world frame W. More...
 
VectorX< T > CalcInverseDynamics (const systems::Context< T > &context, const VectorX< T > &known_vdot, const MultibodyForces< T > &external_forces) const
 Given the state of this model in context and a known vector of generalized accelerations vdot, this method computes the set of generalized forces tau that would need to be applied in order to attain the specified generalized accelerations. More...
 
void CalcImplicitTimeDerivativesResidual (const systems::Context< T > &context, const systems::ContinuousState< T > &proposed_derivatives, EigenPtr< VectorX< T >> residual) const
 MultibodyPlant implements the systems::System::CalcImplicitTimeDerivativesResidual method when the plant is modeled as a continuous-time system, returning one residual for each multibody state. More...
 
void CalcForceElementsContribution (const systems::Context< T > &context, MultibodyForces< T > *forces) const
 Computes the combined force contribution of ForceElement objects in the model. More...
 
VectorX< T > CalcGravityGeneralizedForces (const systems::Context< T > &context) const
 Computes the generalized forces tau_g(q) due to gravity as a function of the generalized positions q stored in the input context. More...
 
void CalcGeneralizedForces (const systems::Context< T > &context, const MultibodyForces< T > &forces, VectorX< T > *generalized_forces) const
 Computes the generalized forces result of a set of MultibodyForces applied to this model. More...
 
bool IsVelocityEqualToQDot () const
 Returns true iff the generalized velocity v is exactly the time derivative q̇ of the generalized coordinates q. More...
 
void MapVelocityToQDot (const systems::Context< T > &context, const Eigen::Ref< const VectorX< T >> &v, EigenPtr< VectorX< T >> qdot) const
 Transforms generalized velocities v to time derivatives qdot of the generalized positions vector q (stored in context). More...
 
void MapQDotToVelocity (const systems::Context< T > &context, const Eigen::Ref< const VectorX< T >> &qdot, EigenPtr< VectorX< T >> v) const
 Transforms the time derivative qdot of the generalized positions vector q (stored in context) to generalized velocities v. More...
 
Eigen::SparseMatrix< T > MakeVelocityToQDotMap (const systems::Context< T > &context) const
 Returns the matrix N(q), which maps q̇ = N(q)⋅v, as described in MapVelocityToQDot(). More...
 
Eigen::SparseMatrix< T > MakeQDotToVelocityMap (const systems::Context< T > &context) const
 Returns the matrix N⁺(q), which maps v = N⁺(q)⋅q̇, as described in MapQDotToVelocity(). More...
 
void CalcMassMatrixViaInverseDynamics (const systems::Context< T > &context, EigenPtr< MatrixX< T >> M) const
 Computes the mass matrix M(q) of the model using a slow method (inverse dynamics). More...
 
void CalcMassMatrix (const systems::Context< T > &context, EigenPtr< MatrixX< T >> M) const
 Efficiently computes the mass matrix M(q) of the model. More...
 
void CalcBiasTerm (const systems::Context< T > &context, EigenPtr< VectorX< T >> Cv) const
 Computes the bias term C(q, v)v containing Coriolis, centripetal, and gyroscopic effects in the multibody equations of motion: More...
 
Matrix3X< T > CalcBiasTranslationalAcceleration (const systems::Context< T > &context, JacobianWrtVariable with_respect_to, const Frame< T > &frame_B, const Eigen::Ref< const Matrix3X< T >> &p_BoBi_B, const Frame< T > &frame_A, const Frame< T > &frame_E) const
 For each point Bi affixed/welded to a frame B, calculates a𝑠Bias_ABi, Bi's translational acceleration bias in frame A with respect to "speeds" 𝑠, where 𝑠 is either q̇ (time-derivatives of generalized positions) or v (generalized velocities). More...
 
SpatialAcceleration< T > CalcBiasSpatialAcceleration (const systems::Context< T > &context, JacobianWrtVariable with_respect_to, const Frame< T > &frame_B, const Eigen::Ref< const Vector3< T >> &p_BoBp_B, const Frame< T > &frame_A, const Frame< T > &frame_E) const
 For one point Bp affixed/welded to a frame B, calculates A𝑠Bias_ABp, Bp's spatial acceleration bias in frame A with respect to "speeds" 𝑠, where 𝑠 is either q̇ (time-derivatives of generalized positions) or v (generalized velocities). More...
 
void CalcJacobianSpatialVelocity (const systems::Context< T > &context, JacobianWrtVariable with_respect_to, const Frame< T > &frame_B, const Eigen::Ref< const Vector3< T >> &p_BoBp_B, const Frame< T > &frame_A, const Frame< T > &frame_E, EigenPtr< MatrixX< T >> Js_V_ABp_E) const
 For one point Bp fixed/welded to a frame B, calculates J𝑠_V_ABp, Bp's spatial velocity Jacobian in frame A with respect to "speeds" 𝑠. More...
 
void CalcJacobianAngularVelocity (const systems::Context< T > &context, const JacobianWrtVariable with_respect_to, const Frame< T > &frame_B, const Frame< T > &frame_A, const Frame< T > &frame_E, EigenPtr< Matrix3X< T >> Js_w_AB_E) const
 Calculates J𝑠_w_AB, a frame B's angular velocity Jacobian in a frame A with respect to "speeds" 𝑠. More...
 
void CalcJacobianTranslationalVelocity (const systems::Context< T > &context, JacobianWrtVariable with_respect_to, const Frame< T > &frame_B, const Eigen::Ref< const Matrix3X< T >> &p_BoBi_B, const Frame< T > &frame_A, const Frame< T > &frame_E, EigenPtr< MatrixX< T >> Js_v_ABi_E) const
 For each point Bi affixed/welded to a frame B, calculates J𝑠_v_ABi, Bi's translational velocity Jacobian in frame A with respect to "speeds" 𝑠. More...
 
void CalcJacobianPositionVector (const systems::Context< T > &context, const Frame< T > &frame_B, const Eigen::Ref< const Matrix3X< T >> &p_BoBi_B, const Frame< T > &frame_A, const Frame< T > &frame_E, EigenPtr< MatrixX< T >> Jq_p_AoBi_E) const
 For each point Bi affixed/welded to a frame B, calculates Jq_p_AoBi, Bi's position vector Jacobian in frame A with respect to the generalized positions q ≜ [q₁ ... More...
 
void CalcJacobianCenterOfMassTranslationalVelocity (const systems::Context< T > &context, JacobianWrtVariable with_respect_to, const Frame< T > &frame_A, const Frame< T > &frame_E, EigenPtr< Matrix3X< T >> Js_v_ACcm_E) const
 Calculates J𝑠_v_ACcm_E, point Ccm's translational velocity Jacobian in frame A with respect to "speeds" 𝑠, expressed in frame E, where point CCm is the center of mass of the system of all non-world bodies contained in this MultibodyPlant. More...
 
void CalcJacobianCenterOfMassTranslationalVelocity (const systems::Context< T > &context, const std::vector< ModelInstanceIndex > &model_instances, JacobianWrtVariable with_respect_to, const Frame< T > &frame_A, const Frame< T > &frame_E, EigenPtr< Matrix3X< T >> Js_v_ACcm_E) const
 Calculates J𝑠_v_ACcm_E, point Ccm's translational velocity Jacobian in frame A with respect to "speeds" 𝑠, expressed in frame E, where point CCm is the center of mass of the system of all non-world bodies contained in model_instances. More...
 
Vector3< T > CalcBiasCenterOfMassTranslationalAcceleration (const systems::Context< T > &context, JacobianWrtVariable with_respect_to, const Frame< T > &frame_A, const Frame< T > &frame_E) const
 Calculates abias_ACcm_E, point Ccm's translational "bias" acceleration term in frame A with respect to "speeds" 𝑠, expressed in frame E, where point Ccm is the composite center of mass of the system of all bodies (except world_body()) in the MultibodyPlant. More...
 
MatrixX< doubleMakeStateSelectorMatrix (const std::vector< JointIndex > &user_to_joint_index_map) const
 This method allows users to map the state of this model, x, into a vector of selected state xₛ with a given preferred ordering. More...
 
MatrixX< doubleMakeActuatorSelectorMatrix (const std::vector< JointActuatorIndex > &user_to_actuator_index_map) const
 This method allows user to map a vector uₛ containing the actuation for a set of selected actuators into the vector u containing the actuation values for this full model. More...
 
MatrixX< T > MakeActuationMatrix () const
 This method creates an actuation matrix B mapping a vector of actuation values u into generalized forces tau_u = B * u, where B is a matrix of size nv x nu with nu equal to num_actuated_dofs() and nv equal to num_velocities(). More...
 
Eigen::SparseMatrix< doubleMakeActuationMatrixPseudoinverse () const
 Creates the pseudoinverse of the actuation matrix B directly (without requiring an explicit inverse calculation). More...
 
MatrixX< doubleMakeActuatorSelectorMatrix (const std::vector< JointIndex > &user_to_joint_index_map) const
 Alternative signature to build an actuation selector matrix Su such that u = Su⋅uₛ, where u is the vector of actuation values for the full model (see get_actuation_input_port()) and uₛ is a vector of actuation values for the actuators acting on the joints listed by user_to_joint_index_map. More...
 
double time_step () const
 The time step (or period) used to model this plant as a discrete system with periodic updates. More...
 
bool is_finalized () const
 Returns true if this MultibodyPlant was finalized with a call to Finalize(). More...
 
const RigidBody< T > & world_body () const
 Returns a constant reference to the world body. More...
 
const RigidBodyFrame< T > & world_frame () const
 Returns a constant reference to the world frame. More...
 
int num_bodies () const
 Returns the number of bodies in the model, including the "world" body, which is always part of the model. More...
 
const RigidBody< T > & get_body (BodyIndex body_index) const
 Returns a constant reference to the body with unique index body_index. More...
 
bool IsAnchored (const RigidBody< T > &body) const
 Returns true if body is anchored (i.e. More...
 
bool HasBodyNamed (std::string_view name) const
 
int NumBodiesWithName (std::string_view name) const
 
bool HasBodyNamed (std::string_view name, ModelInstanceIndex model_instance) const
 
const RigidBody< T > & GetBodyByName (std::string_view name) const
 Returns a constant reference to a body that is identified by the string name in this MultibodyPlant. More...
 
const RigidBody< T > & GetBodyByName (std::string_view name, ModelInstanceIndex model_instance) const
 Returns a constant reference to the body that is uniquely identified by the string name and model_instance in this MultibodyPlant. More...
 
std::vector< BodyIndexGetBodyIndices (ModelInstanceIndex model_instance) const
 Returns a list of body indices associated with model_instance. More...
 
const RigidBody< T > & GetRigidBodyByName (std::string_view name) const
 Returns a constant reference to a rigid body that is identified by the string name in this model. More...
 
const RigidBody< T > & GetRigidBodyByName (std::string_view name, ModelInstanceIndex model_instance) const
 Returns a constant reference to the rigid body that is uniquely identified by the string name in model_instance. More...
 
std::vector< const RigidBody< T > * > GetBodiesWeldedTo (const RigidBody< T > &body) const
 Returns all bodies that are transitively welded, or rigidly affixed, to body, per these two definitions: More...
 
std::vector< BodyIndexGetBodiesKinematicallyAffectedBy (const std::vector< JointIndex > &joint_indexes) const
 Returns all bodies whose kinematics are transitively affected by the given vector of joints. More...
 
int num_joints () const
 Returns the number of joints in the model. More...
 
const Joint< T > & get_joint (JointIndex joint_index) const
 Returns a constant reference to the joint with unique index joint_index. More...
 
bool HasJointNamed (std::string_view name) const
 
bool HasJointNamed (std::string_view name, ModelInstanceIndex model_instance) const
 
Joint< T > & get_mutable_joint (JointIndex joint_index)
 Returns a mutable reference to the joint with unique index joint_index. More...
 
std::vector< JointIndexGetJointIndices (ModelInstanceIndex model_instance) const
 Returns a list of joint indices associated with model_instance. More...
 
const std::vector< JointActuatorIndex > & GetJointActuatorIndices () const
 Returns a list of all joint actuator indices. More...
 
std::vector< JointActuatorIndexGetJointActuatorIndices (ModelInstanceIndex model_instance) const
 Returns a list of joint actuator indices associated with model_instance. More...
 
std::vector< JointIndexGetActuatedJointIndices (ModelInstanceIndex model_instance) const
 Returns a list of actuated joint indices associated with model_instance. More...
 
template<template< typename > class JointType = Joint>
const JointType< T > & GetJointByName (std::string_view name, std::optional< ModelInstanceIndex > model_instance=std::nullopt) const
 Returns a constant reference to a joint that is identified by the string name in this MultibodyPlant. More...
 
template<template< typename > class JointType = Joint>
JointType< T > & GetMutableJointByName (std::string_view name, std::optional< ModelInstanceIndex > model_instance=std::nullopt)
 A version of GetJointByName that returns a mutable reference. More...
 
int num_frames () const
 Returns the number of Frame objects in this model. More...
 
const Frame< T > & get_frame (FrameIndex frame_index) const
 Returns a constant reference to the frame with unique index frame_index. More...
 
bool HasFrameNamed (std::string_view name) const
 
bool HasFrameNamed (std::string_view name, ModelInstanceIndex model_instance) const
 
const Frame< T > & GetFrameByName (std::string_view name) const
 Returns a constant reference to a frame that is identified by the string name in this model. More...
 
const Frame< T > & GetFrameByName (std::string_view name, ModelInstanceIndex model_instance) const
 Returns a constant reference to the frame that is uniquely identified by the string name in model_instance. More...
 
std::vector< FrameIndexGetFrameIndices (ModelInstanceIndex model_instance) const
 Returns a list of frame indices associated with model_instance. More...
 
int num_actuators () const
 Returns the number of joint actuators in the model. More...
 
int num_actuators (ModelInstanceIndex model_instance) const
 Returns the number of actuators for a specific model instance. More...
 
int num_actuated_dofs () const
 Returns the total number of actuated degrees of freedom. More...
 
int num_actuated_dofs (ModelInstanceIndex model_instance) const
 Returns the total number of actuated degrees of freedom for a specific model instance. More...
 
bool has_joint_actuator (JointActuatorIndex actuator_index) const
 Returns true if plant has a joint actuator with unique index actuator_index. More...
 
const JointActuator< T > & get_joint_actuator (JointActuatorIndex actuator_index) const
 Returns a constant reference to the joint actuator with unique index actuator_index. More...
 
JointActuator< T > & get_mutable_joint_actuator (JointActuatorIndex actuator_index)
 Returns a mutable reference to the joint actuator with unique index actuator_index. More...
 
bool HasJointActuatorNamed (std::string_view name) const
 
bool HasJointActuatorNamed (std::string_view name, ModelInstanceIndex model_instance) const
 
const JointActuator< T > & GetJointActuatorByName (std::string_view name) const
 Returns a constant reference to an actuator that is identified by the string name in this MultibodyPlant. More...
 
const JointActuator< T > & GetJointActuatorByName (std::string_view name, ModelInstanceIndex model_instance) const
 Returns a constant reference to the actuator that is uniquely identified by the string name and model_instance in this MultibodyPlant. More...
 
int num_force_elements () const
 Returns the number of ForceElement objects. More...
 
const ForceElement< T > & get_force_element (ForceElementIndex force_element_index) const
 Returns a constant reference to the force element with unique index force_element_index. More...
 
template<template< typename > class ForceElementType = ForceElement>
const ForceElementType< T > & GetForceElement (ForceElementIndex force_element_index) const
 Returns a constant reference to a force element identified by its unique index in this MultibodyPlant. More...
 
bool is_gravity_enabled (ModelInstanceIndex model_instance) const
 
void set_gravity_enabled (ModelInstanceIndex model_instance, bool is_enabled)
 Sets is_gravity_enabled() for model_instance to is_enabled. More...
 
const UniformGravityFieldElement< T > & gravity_field () const
 An accessor to the current gravity field. More...
 
UniformGravityFieldElement< T > & mutable_gravity_field ()
 A mutable accessor to the current gravity field. More...
 
int num_model_instances () const
 Returns the number of model instances in the model. More...
 
const std::string & GetModelInstanceName (ModelInstanceIndex model_instance) const
 Returns the name of a model_instance. More...
 
bool HasModelInstanceNamed (std::string_view name) const
 
ModelInstanceIndex GetModelInstanceByName (std::string_view name) const
 Returns the index to the model instance that is uniquely identified by the string name in this MultibodyPlant. More...
 
std::string GetTopologyGraphvizString () const
 Returns a Graphviz string describing the topology of this plant. More...
 
int num_positions () const
 Returns the size of the generalized position vector q for this model. More...
 
int num_positions (ModelInstanceIndex model_instance) const
 Returns the size of the generalized position vector qᵢ for model instance i. More...
 
int num_velocities () const
 Returns the size of the generalized velocity vector v for this model. More...
 
int num_velocities (ModelInstanceIndex model_instance) const
 Returns the size of the generalized velocity vector vᵢ for model instance i. More...
 
int num_multibody_states () const
 Returns the size of the multibody system state vector x = [q v]. More...
 
int num_multibody_states (ModelInstanceIndex model_instance) const
 Returns the size of the multibody system state vector xᵢ = [qᵢ vᵢ] for model instance i. More...
 
VectorX< doubleGetPositionLowerLimits () const
 Returns a vector of size num_positions() containing the lower position limits for every generalized position coordinate. More...
 
VectorX< doubleGetPositionUpperLimits () const
 Upper limit analog of GetPositionLowerLimits(), where any unbounded or unspecified limits will be +infinity. More...
 
VectorX< doubleGetVelocityLowerLimits () const
 Returns a vector of size num_velocities() containing the lower velocity limits for every generalized velocity coordinate. More...
 
VectorX< doubleGetVelocityUpperLimits () const
 Upper limit analog of GetVelocitysLowerLimits(), where any unbounded or unspecified limits will be +infinity. More...
 
VectorX< doubleGetAccelerationLowerLimits () const
 Returns a vector of size num_velocities() containing the lower acceleration limits for every generalized velocity coordinate. More...
 
VectorX< doubleGetAccelerationUpperLimits () const
 Upper limit analog of GetAccelerationsLowerLimits(), where any unbounded or unspecified limits will be +infinity. More...
 
VectorX< doubleGetEffortLowerLimits () const
 Returns a vector of size num_actuated_dofs() containing the lower effort limits for every actuator. More...
 
VectorX< doubleGetEffortUpperLimits () const
 Returns a vector of size num_actuated_dofs() containing the upper effort limits for every actuator. More...
 
ContactModel get_contact_model () const
 Returns the model used for contact. See documentation for ContactModel. More...
 
int num_visual_geometries () const
 Returns the number of geometries registered for visualization. More...
 
int num_collision_geometries () const
 Returns the number of geometries registered for contact modeling. More...
 
std::optional< geometry::SourceIdget_source_id () const
 Returns the unique id identifying this plant as a source for a SceneGraph. More...
 
bool geometry_source_is_registered () const
 Returns true if this MultibodyPlant was registered with a SceneGraph. More...
 
geometry::SceneGraph< T > * GetMutableSceneGraphPreFinalize ()
 (Internal use only) Returns a mutable pointer to the SceneGraph that this plant is registered as a source for. More...
 
void set_contact_model (ContactModel model)
 Sets the contact model to be used by this MultibodyPlant, see ContactModel for available options. More...
 
void set_discrete_contact_solver (DiscreteContactSolver contact_solver)
 Sets the contact solver type used for discrete MultibodyPlant models. More...
 
DiscreteContactSolver get_discrete_contact_solver () const
 Returns the contact solver type used for discrete MultibodyPlant models. More...
 
void set_discrete_contact_approximation (DiscreteContactApproximation approximation)
 Sets the discrete contact model approximation. More...
 
DiscreteContactApproximation get_discrete_contact_approximation () const
 
void set_sap_near_rigid_threshold (double near_rigid_threshold=MultibodyPlantConfig{}.sap_near_rigid_threshold)
 Non-negative dimensionless number typically in the range [0.0, 1.0], though larger values are allowed even if uncommon. More...
 
double get_sap_near_rigid_threshold () const
 
void set_contact_surface_representation (geometry::HydroelasticContactRepresentation representation)
 Sets the representation of contact surfaces to be used by this MultibodyPlant. More...
 
geometry::HydroelasticContactRepresentation get_contact_surface_representation () const
 Gets the current representation of contact surfaces used by this MultibodyPlant. More...
 
void set_adjacent_bodies_collision_filters (bool value)
 Sets whether to apply collision filters to topologically adjacent bodies at Finalize() time. More...
 
bool get_adjacent_bodies_collision_filters () const
 Returns whether to apply collision filters to topologically adjacent bodies at Finalize() time. More...
 
void SetDiscreteUpdateManager (std::unique_ptr< internal::DiscreteUpdateManager< T >> manager)
 For use only by advanced developers wanting to try out their custom time stepping strategies, including contact resolution. More...
 
void AddPhysicalModel (std::unique_ptr< PhysicalModel< T >> model)
 For use only by advanced developers wanting to try out their new physical models. More...
 
std::vector< const PhysicalModel< T > * > physical_models () const
 Returns a vector of pointers to all physical models registered with this MultibodyPlant. More...
 
void set_penetration_allowance (double penetration_allowance=MultibodyPlantConfig{}.penetration_allowance)
 Sets the penetration allowance used to estimate the coefficients in the penalty method used to impose non-penetration among bodies. More...
 
double get_contact_penalty_method_time_scale () const
 Returns a time-scale estimate tc based on the requested penetration allowance δ set with set_penetration_allowance(). More...
 
void set_stiction_tolerance (double v_stiction=MultibodyPlantConfig{}.stiction_tolerance)
 
double stiction_tolerance () const
 

Static Public Member Functions

static Vector3< doubleget_default_initial_angular_velocity ()
 Returns the default value of the angular velocity set by default by SetDefaultState(). More...
 
static Vector3< doubleget_default_initial_translational_velocity ()
 Returns the default value of the translational velocity set by default by SetDefaultState(). More...
 
- Static Public Member Functions inherited from MultibodyPlant< T >
static geometry::HydroelasticContactRepresentation GetDefaultContactSurfaceRepresentation (double time_step)
 Return the default value for contact representation, given the desired time step. More...
 

Additional Inherited Members

Constructor & Destructor Documentation

◆ AxiallySymmetricFreeBodyPlant() [1/4]

◆ AxiallySymmetricFreeBodyPlant() [2/4]

◆ AxiallySymmetricFreeBodyPlant() [3/4]

AxiallySymmetricFreeBodyPlant ( double  mass,
double  I,
double  J,
double  g,
double  time_step = 0.0 
)

Constructor from known rotational inertia values.

Rotational inertia values have units of kg⋅m².

Parameters
massThe mass of the body.
Irotational inertia about any axis perpendicular to the axis of revolution of the body.
Jrotational inertia about the axis of revolution of the body.
gAcceleration of gravity. In this model if g > 0 the gravity vector points downward in the z-axis direction.
time_step[optional] See MultibodyPlant::MultibodyPlant.

◆ AxiallySymmetricFreeBodyPlant() [4/4]

Scalar-converting copy constructor.

Member Function Documentation

◆ body()

const RigidBody<T>& body ( ) const

Returns a constant reference to the free body model of this plant.

◆ CalcPoseInWorldFrame()

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

Computes the pose X_WB of the body in the world frame.

◆ CalcSpatialVelocityInWorldFrame()

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

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

◆ get_angular_velocity()

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

Returns the angular velocity w_WB stored in context of the free body B in the world frame W.

◆ get_default_initial_angular_velocity()

static Vector3<double> get_default_initial_angular_velocity ( )
static

Returns the default value of the angular velocity set by default by SetDefaultState().

Currently a non-zero value.

◆ get_default_initial_translational_velocity()

static Vector3<double> get_default_initial_translational_velocity ( )
static

Returns the default value of the translational velocity set by default by SetDefaultState().

Currently a non-zero value.

◆ get_translational_velocity()

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

Returns the translational velocity v_WB stored in context of the free body B in the world frame W.

◆ operator=() [1/2]

◆ operator=() [2/2]

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

◆ SetDefaultState()

void SetDefaultState ( const systems::Context< T > &  context,
systems::State< T > *  state 
) const
override

Sets state to a default value corresponding to a configuration in which the free body frame B is coincident with the world frame W and the angular and translational velocities have a value as returned by get_default_initial_angular_velocity() and get_default_initial_translational_velocity(), respectively.


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