Drake

MultibodyPlant is a Drake system framework representation (see systems::System) for the model of a physical system consisting of a collection of interconnected bodies.
See Multibody Kinematics and Dynamics for an overview of concepts/notation.
 MultibodyPlant 

The ports whose names begin with model_instance_name[i] represent groups of ports, one for each of the model instances, with i ∈ {0, ..., N1} for the N model instances. If a model instance does not contain any data of the indicated type the port will still be present but its value will be a zerolength vector. (Model instances world_model_instance()
and default_model_instance()
always exist.)
The ports shown in green are for communication with Drake's SceneGraph system for dealing with geometry.
MultibodyPlant provides a userfacing API for:
A MultiBodyPlant may contain multiple model instances. Each model instance corresponds to a set of bodies and their connections (joints). Model instances provide methods to get or set the state of the set of bodies (e.g., through GetPositionsAndVelocities() and SetPositionsAndVelocities()), connecting controllers (through get_state_output_port() and get_actuation_input_port()), and organizing duplicate models (read through a parser). In fact, many MultibodyPlant methods are overloaded to allow operating on the entire plant or just the subset corresponding to the model instance; for example, one GetPositions() method obtains the generalized positions for the entire plant while another GetPositions() method obtains the generalized positions for model instance.
Model instances are frequently defined through SDF files (using the model
tag) and are automatically created when SDF files are parsed (by Parser). There are two special multibody::ModelInstanceIndex values. The world body is always multibody::ModelInstanceIndex 0. multibody::ModelInstanceIndex 1 is reserved for all elements with no explicit model instance and is generally only relevant for elements created programmatically (and only when a model instance is not explicitly specified). Note that Parser creates model instances (resulting in a multibody::ModelInstanceIndex ≥ 2) as needed.
See num_model_instances(), num_positions(), num_velocities(), num_actuated_dofs(), AddModelInstance() GetPositionsAndVelocities(), GetPositions(), GetVelocities(), SetPositionsAndVelocities(), SetPositions(), SetVelocities(), GetPositionsFromArray(), GetVelocitiesFromArray(), SetPositionsInArray(), SetVelocitiesInArray(), SetActuationInArray(), HasModelInstanceNamed(), GetModelInstanceName(), get_state_output_port(), get_actuation_input_port().
The state of a multibody system x = [q; v]
is given by its generalized positions vector q, of size nq
(see num_positions()), and by its generalized velocities vector v, of size nv
(see num_velocities()). As a Drake System, MultibodyPlant implements the governing equations for a multibody dynamical system in the form ẋ = f(t, x, u)
with t being time and u the actuation forces. The governing equations for the dynamics of a multibody system modeled with MultibodyPlant are [Featherstone 2008, Jain 2010]:
q̇ = N(q)v (1) M(q)v̇ + C(q, v)v = τ
where M(q)
is the mass matrix of the multibody system, C(q, v)v
contains Coriolis, centripetal, and gyroscopic terms and N(q)
is the kinematic coupling matrix describing the relationship between q̇ (the time derivatives of the generalized positions) and the generalized velocities v, [Seth 2010]. N(q)
is an nq x nv
matrix. The vector τ ∈ ℝⁿᵛ
on the right hand side of Eq. (1) is the system's generalized forces. These incorporate gravity, springs, externally applied body forces, constraint forces, and contact forces.
Drake has the capability to load multibody models from SDF and URDF files. Consider the example below which loads an acrobot model:
As in the example above, for models including visual geometry, collision geometry or both, the user must specify a SceneGraph for geometry handling. You can find a full example of the LQR controlled acrobot in examples/multibody/acrobot/run_lqr.cc.
AddModelFromFile() can be invoked multiple times on the same plant in order to load multiple model instances. Other methods are available on Parser such as AddAllModelsFromFile() which allows creating model instances per each <model>
tag found in the file. Please refer to each of these methods' documentation for further details.
Probably the simplest way to add and wire up a MultibodyPlant with a SceneGraph in your Diagram is using AddMultibodyPlantSceneGraph().
Recommended usages:
Assign to a MultibodyPlant reference (ignoring the SceneGraph):
This flavor is the simplest, when the SceneGraph is not explicitly needed. (It can always be retrieved later via GetSubsystemByName("scene_graph").)
Assign to auto, and use the named public fields:
or taking advantage of C++17's structured binding
This is the easiest way to use both the plant and scene_graph.
Assign to alreadydeclared pointer variables:
This flavor is most useful when the pointers are class member fields (and so perhaps cannot be references).
MultibodyPlant users can register geometry with a SceneGraph for essentially two purposes; a) visualization and, b) contact modeling.
Before any geometry registration takes place, a user must first make a call to RegisterAsSourceForSceneGraph() in order to register the MultibodyPlant as a client of a SceneGraph instance, point at which the plant will have assigned a valid geometry::SourceId. At Finalize(), MultibodyPlant will declare input/output ports as appropriate to communicate with the SceneGraph instance on which registrations took place. All geometry registration must be performed prefinalize.
Multibodyplant declares an input port for geometric queries, see get_geometry_query_input_port(). If MultibodyPlant registers geometry with a SceneGraph via calls to RegisterCollisionGeometry(), users may use this port for geometric queries. Users must connect this input port to the output port for geometric queries of the SceneGraph used for registration, which can be obtained with SceneGraph::get_query_output_port(). In summary, if MultibodyPlant registers collision geometry, the setup process will include:
Refer to the documentation provided in each of the methods above for further details.
MultibodyPlant's point contact model looks for model parameters stored as geometry::ProximityProperties by geometry::SceneGraph. These properties can be obtained before or after context creation through geometry::SceneGraphInspector APIs as outlined below. MultibodyPlant expects the following properties for point contact modeling:
Group name  Property Name  Required  Property Type  Property Description 

material  coulomb_friction  yes¹  CoulombFriction<T>  Static and Dynamic friction. 
material  point_contact_stiffness  no²  T  Penalty method stiffness. 
material  hunt_crossley_dissipation  no²  T  Penalty method dissipation. 
¹ Collision geometry is required to be registered with a geometry::ProximityProperties object that contains the ("material", "coulomb_friction") property. If the property is missing, MultibodyPlant will throw an exeception.
² If the property is missing, MultibodyPlant will use a heuristic value as the default. Refer to the section Penalty method point contact for further details.
Accessing and modifying contact properties requires interfacing with geometry::SceneGraph's model inspector. Interfacing with a model inspector obtained from geometry::SceneGraph will provide the default registered values for a given parameter. These are the values that will initially appear in a systems::Context created by CreateDefaultContext(). Subsequently, true system parameters can be accessed and changed through a systems::Context once available. For both of the above cases, proximity properties are accessed through geometry::SceneGraphInspector APIs.
Before context creation an inspector can be retrieved directly from SceneGraph as:
After context creation, an inspector can be retrieved from the state stored in the context by the plant's geometry query input port:
Once an inspector is available, proximity properties can be retrieved as:
Several MultibodyElements expose parameters, allowing the user flexible modification of some aspects of the plant's model, post systems::Context creation. For details, refer to the docmentation for the MultibodyElement whose parameters you are trying to modify/access (e.g. RigidBody, FixedOffsetFrame, etc.)
As an example, here is how to access and modify rigid body mass parameters:
Another example, working with automatic differentiation in order to take derivatives with respect to one of the bodies' masses:
Add multibody elements to a MultibodyPlant with methods like:
All modeling elements must be added before Finalize() is called. See Finalize stage for a discussion.
Please refer to Contact Modeling in Drake for details on the available approximations, setup, and considerations for a multibody simulation with frictional contact.
MultibodyPlant implements the System energy and power methods, with some limitations.
See Drake issue #12942 for more discussion.
Once the user is done adding modeling elements and registering geometry, a call to Finalize() must be performed. This call will:
T  The scalar type, which must be one of the default scalars. 
#include <drake/multibody/tree/multibody_element.h>
Public Member Functions  
Does not allow copy, move, or assignment  
MultibodyPlant (const MultibodyPlant &)=delete  
MultibodyPlant &  operator= (const MultibodyPlant &)=delete 
MultibodyPlant (MultibodyPlant &&)=delete  
MultibodyPlant &  operator= (MultibodyPlant &&)=delete 
Input and output ports  
These methods provide access to the Drake System input and output ports as depicted in the MultibodyPlant class documentation. Actuation values can be provided through a single input port which describes the entire plant (in the case where only a single model instance has actuated dofs), or through multiple input ports which each provide the actuation values for a specific model instance. See AddJointActuator() and num_actuators(). Output ports provide information about the entire MultibodyPlant or its individual model instances.  
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 (ModelInstanceIndex model_instance) const 
Returns a constant reference to the input port for external actuation for a specific model instance. More...  
const systems::InputPort< T > &  get_actuation_input_port () const 
Returns a constant reference to the input port for external actuation for the case where only one model instance has actuated dofs. More...  
const systems::InputPort< T > &  get_applied_generalized_force_input_port () const 
Returns a constant reference to the vectorvalued 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...  
Construction  
To add modeling elements like bodies, joints, force elements, constraints, etc. to a MultibodyPlant, use one of the following construction methods. Once all modeling elements have been added, the Finalize() method must be called. A call to any construction method after a call to Finalize() causes an exception to be thrown. After calling Finalize(), you may invoke MultibodyPlant methods that perform computations. See Finalize() for details.  
MultibodyPlant (double time_step)  
This constructor creates a plant with a single "world" body. More...  
template<typename U >  
MultibodyPlant (const MultibodyPlant< U > &other)  
Scalarconverting copy constructor. See System Scalar Conversion. More...  
const RigidBody< T > &  AddRigidBody (const std::string &name, ModelInstanceIndex model_instance, const SpatialInertia< double > &M_BBo_B) 
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) 
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 Body< T > &parent, const std::optional< math::RigidTransform< double >> &X_PF, const Body< 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_P, const Frame< T > &frame_on_child_C, const math::RigidTransform< double > &X_PC=math::RigidTransform< double >::Identity()) 
Welds frame_on_parent_P and frame_on_child_C with relative pose X_PC . 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...  
ModelInstanceIndex  AddModelInstance (const std::string &name) 
Creates a new 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...  
Geometry  
The following geometry methods provide a convenient means for associating geometries with bodies. Ultimately, the geometries are owned by SceneGraph. These methods do the work of registering the requested geometries with SceneGraph and maintaining a mapping between the body and the registered data. Particularly, SceneGraph knows nothing about the concepts inherent in the MultibodyPlant. These methods account for those differences as documented below. Geometry registration with rolesGeometries can be associated with bodies via the All geometry registration methods return a geometry::GeometryId GeometryId. This is how SceneGraph refers to the geometries. The properties of an individual geometry can be accessed with its id and geometry::SceneGraphInspector and geometry::QueryObject (for its statedependent pose in world). Body frames and SceneGraph framesThe first time a geometry registration method is called on a particular body, that body's frame B is registered with SceneGraph. As SceneGraph knows nothing about bodies, in the SceneGraph domain, the frame is simply notated as F; this is merely an alias for the body frame. Thus, the pose of the geometry G in the SceneGraph frame F is the same as the pose of the geometry in the body frame B; The model instance index of the body is passed to the SceneGraph frame as its "frame group". This can be retrieved from the geometry::SceneGraphInspector::GetFrameGroup(FrameId) method. Given a GeometryId, SceneGraph cannot report what body it is affixed to. It can only report the SceneGraph alias frame F. But the following idiom can report the body: const MultibodyPlant<T>& plant = ...; const SceneGraphInspector<T>& inspector = ...; const GeometryId g_id = id_from_some_query; const FrameId f_id = inspector.GetFrameId(g_id); const Body<T>* body = plant.GetBodyFromFrameId(f_id); See documentation of geometry::SceneGraphInspector on where to get an inspector. In MultibodyPlant, frame names only have to be unique in a single model instance. However, SceneGraph knows nothing of model instances. So, to generate unique names for the corresponding frames in SceneGraph, when MultibodyPlant registers the corresponding SceneGraph frame, it is named with a "scoped name". This is a concatenation of  
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 Body< 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 Body< 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::DrakeVisualizercompatible set of geometry::IllustrationProperties. More...  
geometry::GeometryId  RegisterVisualGeometry (const Body< 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 Body< 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 Body< 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 Body< 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 Body< 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 collision geometries between two given collision filter groups. More...  
geometry::GeometrySet  CollectRegisteredGeometries (const std::vector< const Body< T > * > &bodies) const 
For each of the provided bodies , collects up all geometries that have been registered to that body. More...  
const Body< 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::FrameId >  GetBodyFrameIdIfExists (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...  
Contact modeling  
Use methods in this section to choose the contact model and to provide parameters for that model. Currently Drake supports an advanced compliant contact model we call Hydroelastic contact that is still experimental, and a penaltybased point contact model as a reliable fallback. Hydroelastic contactTo understand how material properties enter into the modeling of contact traction in the hydroelastic model, the user is referred to [R. Elandt 2019] for details. For brevity, here we limit ourselves to state the relationship between the material properties and the computation of the normal traction or "pressure" E = Eᵃ⋅Eᵇ/(Eᵃ + Eᵇ), d = E/Eᵃ⋅dᵃ + E/Eᵇ⋅dᵇ = Eᵇ/(Eᵃ+Eᵇ)⋅dᵃ + Eᵃ/(Eᵃ+Eᵇ)⋅dᵇ The effective modulus of elasticity is computed in accordance with the Hertz theory of contact. Dissipation is weighted in accordance with the fact that the softer material will deform more and faster and thus the softer material dissipation is given more importance. Elastic modulus has units of pressure, i.e. We use a dissipation model inspired by the model in [Hunt and Crossley, 1975], parameterized by a dissipation constant with units of inverse of velocity, i.e. The elastic modulus and dissipation can be specified in one of two ways:
With the effective properties of the pair defined as above, the hydroelastic model pressure field is computed according to: p(x) = E⋅ε(x)⋅(1  d⋅vₙ(x))₊ where we defined the effective strain: ε(x) = εᵃ(x) + εᵇ(x) which relates to the quasistatic pressure field p₀(x) (i.e. when velocity is neglected) by: p₀(x) = E⋅ε(x) = Eᵃ⋅εᵃ(x) = Eᵇ⋅εᵇ(x) that is, the hydroelastic model computes the contact patch assuming quasistatic equilibrium. The separation speed [Elandt 2019] R. Elandt, E. Drumwright, M. Sherman, and A. Ruina. A pressure field model for fast, robust approximation of net contact force and moment between nominally rigid objects. Proc. IEEE/RSJ Intl. Conf. on Intelligent Robots and Systems (IROS), 2019. [Hunt and Crossley 1975] Hunt, KH and Crossley, FRE, 1975. Coefficient of restitution interpreted as damping in vibroimpact. Journal of Applied Mechanics, vol. 42, pp. 440–445. Penalty method point contactCurrently MultibodyPlant uses a rigid contact model that is, bodies in the model are infinitely stiff or ideal rigid bodies. Therefore, the mathematical description of the rigid contact model needs to include nonpenetration constraints among bodies in the formulation. There are several numerical methods to impose and solve these constraints. In a penalty method approach, we allow for a certain amount of interpenetration and we compute contact forces according to a simple law of the form: fₙ = k(1+dẋ)x where the normal contact force k = (k₁⋅k₂)/(k₁+k₂) d = (k₂/(k₁+k₂))⋅d₁ + (k₁/(k₁+k₂))⋅d₂ These parameters are optional for each geometry. For any geometry not assigned these parameters by a user PreFinalize, MultibodyPlant will assign default values such that the combined parameters of two geometries with default values match those estimated using the usersupplied "penetration allowance", as described below. These are adhoc parameters which need to be tuned as a tradeoff between:
There is no exact procedure for choosing these coefficients, and estimating them manually can be cumbersome since in general they will depend on the scale of the problem including masses, speeds and even body sizes. However, MultibodyPlant aids the estimation of these coefficients using a heuristic function based on a usersupplied "penetration allowance", see set_penetration_allowance(). The penetration allowance is a number in meters that specifies the order of magnitude of the average penetration between bodies in the system that the user is willing to accept as reasonable for the problem being solved. For instance, in the robotics manipulation of ordinary daily objects the user might set this number to 1 millimeter. However, the user might want to increase it for the simulation of heavy walking robots for which an allowance of 1 millimeter would result in a very stiff system. As for the dissipation coefficient in the simple law above, MultibodyPlant chooses the dissipation coefficient d to model inelastic collisions and therefore sets it so that the penetration distance x behaves as a critically damped oscillator. That is, at the limit of ideal rigid contact (very stiff penalty coefficient k or equivalently the penetration allowance goes to zero), this method behaves as a unilateral constraint on the penetration distance, which models a perfect inelastic collision. For most applications, such as manipulation and walking, this is the desired behavior. When set_penetration_allowance() is called, MultibodyPlant will estimate reasonable penalty method coefficients as a function of the input penetration allowance. Users will want to run their simulation a number of times and assess they are satisfied with the level of interpenetration actually observed in the simulation; if the observed penetration is too large, the user will want to set a smaller penetration allowance. If the system is too stiff and the time integration requires very small time steps while at the same time the user can afford larger interpenetrations, the user will want to increase the penetration allowance. Typically, the observed penetration will be proportional to the penetration allowance. Thus scaling the penetration allowance by say a factor of 0.5, would typically results in interpenetrations being reduced by the same factor of 0.5. In summary, users should choose the largest penetration allowance that results in interpenetration levels that are acceptable for the particular application (even when in theory this penetration should be zero for perfectly rigid bodies.) For a given penetration allowance, the contact interaction that takes two bodies with a nonzero approaching velocity to zero approaching velocity, takes place in a finite amount of time (for ideal rigid contact this time is zero.) A good estimate of this time period is given by a call to get_contact_penalty_method_time_scale(). Users might want to query this value to either set the maximum time step in errorcontrolled time integration or to set the time step for fixed time step integration. As a guidance, typical fixed time step integrators will become unstable for time steps larger than about a tenth of this time scale. For further details on contact modeling in Drake, please refer to the section Contact Modeling in Drake of our documentation.  
void  set_contact_model (ContactModel model) 
Sets the contact model to be used by this MultibodyPlant, see ContactModel for available options. More...  
void  set_penetration_allowance (double penetration_allowance=0.001) 
Sets the penetration allowance used to estimate the coefficients in the penalty method used to impose nonpenetration among bodies. More...  
double  get_contact_penalty_method_time_scale () const 
Returns a timescale estimate tc based on the requested penetration allowance δ set with set_penetration_allowance(). More...  
void  set_stiction_tolerance (double v_stiction=0.001) 
State accessors and mutators  
The following state methods allow getting and setting the kinematic state variables There are also utilities for accessing and mutating portions of state or actuation arrays corresponding to just a single model instance.  
Eigen::VectorBlock< const VectorX< T > >  GetPositionsAndVelocities (const systems::Context< T > &context) const 
Returns a const vector reference containing the vector [q; v] with q the vector of generalized positions and v the vector of generalized velocities. More...  
VectorX< T >  GetPositionsAndVelocities (const systems::Context< T > &context, ModelInstanceIndex model_instance) const 
Returns the vector [q; v] of the model with q the vector of generalized positions and v the vector of generalized velocities for model instance model_instance . More...  
Eigen::VectorBlock< VectorX< T > >  GetMutablePositionsAndVelocities (systems::Context< T > *context) const 
(Advanced) Returns a mutable vector containing the vector [q; v] of the model with q the vector of generalized positions and v the vector of generalized velocities (see warning). More...  
void  SetPositionsAndVelocities (systems::Context< T > *context, const VectorX< T > &q_v) const 
Sets all generalized positions and velocities from the given vector [q; v]. More...  
void  SetPositionsAndVelocities (systems::Context< T > *context, ModelInstanceIndex model_instance, const VectorX< T > &q_v) const 
Sets generalized positions and velocities from the given vector [q; v] for the specified model instance. More...  
Eigen::VectorBlock< const VectorX< T > >  GetPositions (const systems::Context< T > &context) const 
Returns a const vector reference containing the vector of generalized positions. More...  
VectorX< T >  GetPositions (const systems::Context< T > &context, ModelInstanceIndex model_instance) const 
Returns an vector containing the generalized positions (q ) for the given model instance. More...  
Eigen::VectorBlock< VectorX< T > >  GetMutablePositions (systems::Context< T > *context) const 
(Advanced) Returns a mutable vector reference containing the vector of generalized positions (see warning). More...  
Eigen::VectorBlock< VectorX< T > >  GetMutablePositions (const systems::Context< T > &context, systems::State< T > *state) const 
(Advanced) Returns a mutable vector reference containing the vector of generalized positions (see warning). More...  
void  SetPositions (systems::Context< T > *context, const VectorX< T > &q) const 
Sets all generalized positions from the given vector. More...  
void  SetPositions (systems::Context< T > *context, ModelInstanceIndex model_instance, const VectorX< T > &q_instance) const 
Sets the positions for a particular model instance from the given vector. More...  
void  SetPositions (const systems::Context< T > &context, systems::State< T > *state, ModelInstanceIndex model_instance, const VectorX< T > &q_instance) const 
Sets the positions for a particular model instance from the given vector. More...  
Eigen::VectorBlock< const VectorX< T > >  GetVelocities (const systems::Context< T > &context) const 
Returns a const vector reference containing the generalized velocities. More...  
VectorX< T >  GetVelocities (const systems::Context< T > &context, ModelInstanceIndex model_instance) const 
Returns a vector containing the generalized velocities (v ) for the given model instance. More...  
Eigen::VectorBlock< VectorX< T > >  GetMutableVelocities (const systems::Context< T > &context, systems::State< T > *state) const 
(Advanced) Returns a mutable vector reference containing the vector of generalized velocities (see warning). More...  
Eigen::VectorBlock< VectorX< T > >  GetMutableVelocities (systems::Context< T > *context) const 
See GetMutableVelocities() method above. More...  
void  SetVelocities (systems::Context< T > *context, const VectorX< T > &v) const 
Sets all generalized velocities from the given vector. More...  
void  SetVelocities (const systems::Context< T > &context, systems::State< T > *state, ModelInstanceIndex model_instance, const VectorX< T > &v_instance) const 
Sets the generalized velocities for a particular model instance from the given vector. More...  
void  SetVelocities (systems::Context< T > *context, ModelInstanceIndex model_instance, const VectorX< T > &v_instance) const 
Sets the generalized velocities for a particular model instance from the 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...  
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 model. More...  
void  SetActuationInArray (ModelInstanceIndex model_instance, const Eigen::Ref< const VectorX< T >> &u_instance, EigenPtr< VectorX< T >> u) const 
Given the actuation values u_instance for all actuators in model_instance , this method sets the actuation vector u for the entire 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  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  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...  
Working with free bodies  
A MultibodyPlant user adds sets of Body and Joint objects to  
std::unordered_set< BodyIndex >  GetFloatingBaseBodies () 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 Body< T > &body) const 
Gets the pose of a given body in the world frame W. More...  
void  SetFreeBodyPose (systems::Context< T > *context, const Body< 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 Body< 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 Body< T > &body, const math::RigidTransform< double > &X_WB) 
Sets the default pose of body . More...  
const math::RigidTransform< double > &  GetDefaultFreeBodyPose (const Body< T > &body) const 
Gets the default pose of body as set by SetDefaultFreeBodyPose(). More...  
void  SetFreeBodySpatialVelocity (systems::Context< T > *context, const Body< 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 Body< 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 Body< T > &body, const Vector3< symbolic::Expression > &position) 
Sets the distribution used by SetRandomState() to populate the free body's xyz position with respect to World. More...  
void  SetFreeBodyRandomRotationDistribution (const Body< 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 Body< 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 Body< 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 Body< 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 Body< 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...  
Kinematic and dynamic computations  
These methods return kinematic results for the state supplied in the given Context. Methods whose names being with  
const math::RigidTransform< T > &  EvalBodyPoseInWorld (const systems::Context< T > &context, const Body< 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 Body< 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 Body< 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...  
T  CalcTotalMass (const systems::Context< T > &context) const 
Calculates the total mass of all bodies in this MultibodyPlant. More...  
T  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 >  CalcCenterOfMassPosition (const systems::Context< T > &context) const 
(Deprecated.) 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 bodies contained in model_instances, expressed in the world frame W. More...  
Vector3< T >  CalcCenterOfMassPosition (const systems::Context< T > &context, const std::vector< ModelInstanceIndex > &model_instances) const 
(Deprecated.) 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  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  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...  
System matrix computations  
Methods in this section compute and return various matrices that appear in the system equations of motion. For better performance, prefer to use direct computations where available rather than work with explicit matrices. See Kinematic and dynamics computations for available computations. For example, you can obtain the mass matrix, Coriolis, centripetal, and gyroscopic "bias" terms, and a variety of Jacobian and actuation matrices.  
void  CalcMassMatrixViaInverseDynamics (const systems::Context< T > &context, EigenPtr< MatrixX< T >> M) const 
Performs the computation of the mass matrix M(q) of the model using inverse dynamics, where the generalized positions q are stored in context . More...  
void  CalcMassMatrix (const systems::Context< T > &context, EigenPtr< MatrixX< T >> M) const 
Performs the computation of the mass matrix M(q) of the model, as a function of the generalized positions q stored in context . 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̇ (timederivatives 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̇ (timederivatives 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  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 
This method computes 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 composite center of mass of the system of all bodies in the MultibodyPlant (except world_body()). 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< double >  MakeStateSelectorMatrix (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< double >  MakeActuatorSelectorMatrix (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_actuators() and nv equal to num_velocities(). More...  
MatrixX< double >  MakeActuatorSelectorMatrix (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 (ordered by JointActuatorIndex) and uₛ is a vector of actuation values for the actuators acting on the joints listed by user_to_joint_index_map . More...  
Introspection  
These methods allow a user to query whether a given multibody element is part of this plant's model. These queries can be performed at any time during the lifetime of a MultibodyPlant model, i.e. there is no restriction on whether they must be called before or after Finalize(). These queries can be performed while new multibody elements are being added to the model. These methods allow a user to retrieve a reference to a multibody element by its name. An exception is thrown if there is no element with the requested name. If the named element is present in more than one model instance and a model instance is not explicitly specified, std::logic_error is thrown.  
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 BodyFrame< 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 Body< T > &  get_body (BodyIndex body_index) const 
Returns a constant reference to the body with unique index body_index . More...  
bool  IsAnchored (const Body< T > &body) const 
Returns true if body is anchored (i.e. More...  
bool  HasBodyNamed (std::string_view name) const 
bool  HasBodyNamed (std::string_view name, ModelInstanceIndex model_instance) const 
const Body< 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 Body< 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< BodyIndex >  GetBodyIndices (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 Body< T > * >  GetBodiesWeldedTo (const Body< T > &body) const 
Returns all bodies that are transitively welded, or rigidly affixed, to body , per these two definitions: 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< JointIndex >  GetJointIndices (ModelInstanceIndex model_instance) const 
Returns a list of 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...  
int  num_actuators () const 
Returns the number of joint actuators in the model. 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...  
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) const 
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...  
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< double >  GetPositionLowerLimits () const 
Returns a vector of size num_positions() containing the lower position limits for every generalized position coordinate. More...  
VectorX< double >  GetPositionUpperLimits () const 
Upper limit analog of GetPositionsLowerLimits(), where any unbounded or unspecified limits will be +infinity. More...  
VectorX< double >  GetVelocityLowerLimits () const 
Returns a vector of size num_velocities() containing the lower velocity limits for every generalized velocity coordinate. More...  
VectorX< double >  GetVelocityUpperLimits () const 
Upper limit analog of GetVelocitysLowerLimits(), where any unbounded or unspecified limits will be +infinity. More...  
VectorX< double >  GetAccelerationLowerLimits () const 
Returns a vector of size num_velocities() containing the lower acceleration limits for every generalized velocity coordinate. More...  
VectorX< double >  GetAccelerationUpperLimits () const 
Upper limit analog of GetAccelerationsLowerLimits(), where any unbounded or unspecified limits will be +infinity. 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::SourceId >  get_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...  
Friends  
template<typename U >  
class  MultibodyPlant 
class  MultibodyPlantTester 
Related Functions  
(Note that these are not member functions.)  
template<typename T >  
AddMultibodyPlantSceneGraphResult< T >  AddMultibodyPlantSceneGraph (systems::DiagramBuilder< T > *builder, double time_step, std::unique_ptr< geometry::SceneGraph< T >> scene_graph=nullptr) 
Makes a new MultibodyPlant with discrete update period time_step and adds it to a diagram builder together with the provided SceneGraph instance, connecting the geometry ports. More...  
template<typename T >  
AddMultibodyPlantSceneGraphResult< T >  AddMultibodyPlantSceneGraph (systems::DiagramBuilder< T > *builder, std::unique_ptr< MultibodyPlant< T >> plant, std::unique_ptr< geometry::SceneGraph< T >> scene_graph=nullptr) 
Adds a MultibodyPlant and a SceneGraph instance to a diagram builder, connecting the geometry ports. More...  

delete 

delete 

explicit 
This constructor creates a plant with a single "world" body.
Therefore, right after creation, num_bodies() returns one.
MultibodyPlant offers two different modalities to model mechanical systems in time. These are:
time_step
is strictly greater than zero.time_step
equals exactly zero.Currently the discrete model is preferred for simulation given its robustness and speed in problems with frictional contact. However this might change as we work towards developing better strategies to model contact. See Choice of Time Advancement Strategy for further details.
[in]  time_step  Indicates whether this plant is modeled as a continuous system (time_step = 0 ) or as a discrete system with periodic updates of period time_step > 0 . See Choice of Time Advancement Strategy for further details. 
time_step = 0
does not support joint limits for simulation, these are ignored. MultibodyPlant prints a warning to console if joint limits are provided. If your simulation requires joint limits currently you must use a discrete MultibodyPlant model.std::exception  if time_step is negative. 

explicit 
Scalarconverting copy constructor. See System Scalar Conversion.
const ForceElementType<T>& AddForceElement  (  Args &&...  args  ) 
Adds a new force element model of type ForceElementType
to this
MultibodyPlant.
The arguments to this method args
are forwarded to ForceElementType
's constructor.
[in]  args  Zero or more parameters provided to the constructor of the new force element. It must be the case that ForceElementType<T>(args) is a valid constructor. 
ForceElementType  The type of the ForceElement to add. As there is always a UniformGravityFieldElement present (accessible through gravity_field()), an exception will be thrown if this function is called to add another UniformGravityFieldElement. 
ForceElementType<T>
specialized on the scalar type T of this
MultibodyPlant. It will remain valid for the lifetime of this
MultibodyPlant. const FrameType<T>& AddFrame  (  std::unique_ptr< FrameType< T >>  frame  ) 
This method adds a Frame of type FrameType<T>
.
For more information, please see the corresponding constructor of FrameType
.
FrameType  Template which will be instantiated on T . 
frame  Unique pointer frame instance. 
this
MultibodyPlant. const JointType<T>& AddJoint  (  std::unique_ptr< JointType< T >>  joint  ) 
This method adds a Joint of type JointType
between two bodies.
For more information, see the below overload of AddJoint<>
.
const JointType<T>& AddJoint  (  const std::string &  name, 
const Body< T > &  parent,  
const std::optional< math::RigidTransform< double >> &  X_PF,  
const Body< T > &  child,  
const std::optional< math::RigidTransform< double >> &  X_BM,  
Args &&...  args  
) 
This method adds a Joint of type JointType
between two bodies.
The two bodies connected by this Joint object are referred to as parent and child bodies. The parent/child ordering defines the sign conventions for the generalized coordinates and the coordinate ordering for multiDOF joints.
Note: The previous figure also shows Pcm which is body P's center of mass and point Bcm which is body B's center of mass.
As explained in the Joint class's documentation, in Drake we define a frame F attached to the parent body P with pose X_PF
and a frame M attached to the child body B with pose X_BM
. This method helps creating a joint between two bodies with fixed poses X_PF
and X_BM
. Refer to the Joint class's documentation for more details.
name  A string that uniquely identifies the new joint to be added to this model. A std::runtime_error is thrown if a joint named name already is part of the model. See HasJointNamed(), Joint::name().  
[in]  parent  The parent body connected by the new joint. 
[in]  X_PF  The fixed pose of frame F attached to the parent body, measured in the frame P of that body. X_PF is an optional parameter; empty curly braces {} imply that frame F is the same body frame P. If instead your intention is to make a frame F with pose X_PF equal to the identity pose, provide RigidTransform<double>::Identity() as your input. 
[in]  child  The child body connected by the new joint. 
[in]  X_BM  The fixed pose of frame M attached to the child body, measured in the frame B of that body. X_BM is an optional parameter; empty curly braces {} imply that frame M is the same body frame B. If instead your intention is to make a frame M with pose X_BM equal to the identity pose, provide RigidTransform<double>::Identity() as your input. 
[in]  args  Zero or more parameters provided to the constructor of the new joint. It must be the case that JointType<T>( const std::string&, const Frame<T>&, const Frame<T>&, args) is a valid constructor. 
JointType  The type of the Joint to add. 
JointType<T>
specialized on the scalar type T of this
MultibodyPlant. It will remain valid for the lifetime of this
MultibodyPlant.Example of usage:
std::exception  if this MultibodyPlant already contains a joint with the given name . See HasJointNamed(), Joint::name(). 
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
.
This method returns a constant reference to the actuator just added, which will remain valid for the lifetime of this
plant.
[in]  name  A string that uniquely identifies the new actuator to be added to this model. A std::runtime_error is thrown if an actuator with the same name already exists in the model. See HasJointActuatorNamed(). 
[in]  joint  The Joint to be actuated by the new JointActuator. 
[in]  effort_limit  The maximum effort for the actuator. It must be strictly positive, otherwise an std::exception is thrown. If +∞, the actuator has no limit, which is the default. The effort limit has physical units in accordance to the joint type it actuates. For instance, it will have units of N⋅m (torque) for revolute joints while it will have units of N (force) for prismatic joints. 
this
plant. std::exception  if joint.num_velocities() > 1 since for now we only support actuators for single dof joints. 
ModelInstanceIndex AddModelInstance  (  const std::string &  name  ) 
Creates a new model instance.
Returns the index for the model instance.
[in]  name  A string that uniquely identifies the new instance to be added to this model. An exception is thrown if an instance with the same name already exists in the model. See HasModelInstanceNamed(). 
const RigidBody<T>& AddRigidBody  (  const std::string &  name, 
ModelInstanceIndex  model_instance,  
const SpatialInertia< double > &  M_BBo_B  
) 
Creates a rigid body with the provided name and spatial inertia.
This method returns a constant reference to the body just added, which will remain valid for the lifetime of this
MultibodyPlant.
Example of usage:
[in]  name  A string that identifies the new body to be added to this model. A std::runtime_error is thrown if a body named name already is part of model_instance . See HasBodyNamed(), Body::name(). 
[in]  model_instance  A model instance index which this body is part of. 
[in]  M_BBo_B  The SpatialInertia of the new rigid body to be added to this MultibodyPlant, computed about the body frame origin Bo and expressed in the body frame B. 
this
MultibodyPlant. const RigidBody<T>& AddRigidBody  (  const std::string &  name, 
const SpatialInertia< double > &  M_BBo_B  
) 
Creates a rigid body with the provided name and spatial inertia.
This method returns a constant reference to the body just added, which will remain valid for the lifetime of this
MultibodyPlant. The body will use the default model instance (more on model instances).
Example of usage:
[in]  name  A string that identifies the new body to be added to this model. A std::runtime_error is thrown if a body named name already is part of the model in the default model instance. See HasBodyNamed(), Body::name(). 
[in]  M_BBo_B  The SpatialInertia of the new rigid body to be added to this MultibodyPlant, computed about the body frame origin Bo and expressed in the body frame B. 
this
MultibodyPlant. std::logic_error  if additional model instances have been created beyond the world and default instances. 
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.
abias_ACcm is the part of a_ACcm (Ccm's translational acceleration) that does not multiply ṡ, equal to abias_ACcm = J̇𝑠_v_ACcm ⋅ s. This allows a_ACcm to be written as a_ACcm = J𝑠_v_ACcm ⋅ ṡ + abias_ACcm.
[in]  context  The state of the multibody system. 
[in]  with_respect_to  Enum equal to JacobianWrtVariable::kQDot or JacobianWrtVariable::kV, indicating whether the Jacobian abias_ACcm is partial derivatives with respect to 𝑠 = q̇ (timederivatives of generalized positions) or with respect to 𝑠 = v (generalized velocities). 
[in]  frame_A  The frame in which abias_ACcm is measured. 
[in]  frame_E  The frame in which abias_ACcm is expressed on output. 
abias_ACcm_E  Point Ccm's translational "bias" acceleration term in frame A with respect to "speeds" 𝑠, expressed in frame E. 
std::runtime_error  if Ccm does not exist, which occurs if there are no massive bodies in MultibodyPlant (except world_body()). 
std::exception  if composite_mass <= 0, where composite_mass is the total mass of all bodies except world_body() in MultibodyPlant. 
std::exception  if frame_A is not the world frame. 
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̇ (timederivatives of generalized positions) or v (generalized velocities).
A𝑠Bias_ABp is the term in A_ABp (Bp's spatial acceleration in A) that does not include 𝑠̇, i.e., A𝑠Bias_ABp is Bi's translational acceleration in A when 𝑠̇ = 0.
A_ABp = J𝑠_V_ABp ⋅ 𝑠̇ + J̇𝑠_V_ABp ⋅ 𝑠 (𝑠 = q̇ or 𝑠 = v), hence A𝑠Bias_ABp = J̇𝑠_V_ABp ⋅ 𝑠
where J𝑠_V_ABp is Bp's spatial velocity Jacobian in frame A for speeds s (see CalcJacobianSpatialVelocity() for details on J𝑠_V_ABp).
[in]  context  The state of the multibody system. 
[in]  with_respect_to  Enum equal to JacobianWrtVariable::kQDot or JacobianWrtVariable::kV, indicating whether the spatial accceleration bias is with respect to 𝑠 = q̇ or 𝑠 = v. 
[in]  frame_B  The frame on which point Bp is affixed/welded. 
[in]  p_BoBp_B  Position vector from Bo (frame_B's origin) to point Bp (regarded as affixed/welded to B), expressed in frame_B. 
[in]  frame_A  The frame that measures A𝑠Bias_ABp. Currently, an exception is thrown if frame_A is not the World frame. 
[in]  frame_E  The frame in which A𝑠Bias_ABp is expressed on output. 
V_ABp = J𝑠_V_ABp ⋅ 𝑠 which upon vector differentiation in A gives A_ABp = J𝑠_V_ABp ⋅ 𝑠̇ + J̇𝑠_V_ABp ⋅ 𝑠 Since J̇𝑠_V_ABp is linear in 𝑠, A𝑠Bias_ABp = J̇𝑠_V_ABp ⋅ 𝑠 is quadratic in 𝑠.
std::exception  if with_respect_to is not JacobianWrtVariable::kV 
std::exception  if frame_A is not the world frame. 
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:
M(q) v̇ + C(q, v) v = tau_app + ∑ (Jv_V_WBᵀ(q) ⋅ Fapp_Bo_W)
where M(q)
is the multibody model's mass matrix and tau_app
is a vector of generalized forces. The last term is a summation over all bodies of the dotproduct of Fapp_Bo_W
(applied spatial force on body B at Bo) with Jv_V_WB(q)
(B's spatial Jacobian in world W with respect to generalized velocities v). Note: B's spatial velocity in W can be written V_WB = Jv_V_WB * v
.
[in]  context  The context containing the state of the model. It stores the generalized positions q and the generalized velocities v. 
[out]  Cv  On output, Cv will contain the product C(q, v)v . It must be a valid (nonnull) pointer to a column vector in ℛⁿ with n the number of generalized velocities (num_velocities()) of the model. This method aborts if Cv is nullptr or if it does not have the proper size. 
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̇ (timederivatives of generalized positions) or v (generalized velocities).
a𝑠Bias_ABi is the term in a_ABi (Bi's translational acceleration in A) that does not include 𝑠̇, i.e., a𝑠Bias_ABi is Bi's translational acceleration in A when 𝑠̇ = 0.
a_ABi = J𝑠_v_ABi ⋅ 𝑠̇ + J̇𝑠_v_ABi ⋅ 𝑠 (𝑠 = q̇ or 𝑠 = v), hence a𝑠Bias_ABi = J̇𝑠_v_ABi ⋅ 𝑠
where J𝑠_v_ABi is Bi's translational velocity Jacobian in frame A for s (see CalcJacobianTranslationalVelocity() for details on J𝑠_v_ABi).
[in]  context  The state of the multibody system. 
[in]  with_respect_to  Enum equal to JacobianWrtVariable::kQDot or JacobianWrtVariable::kV, indicating whether the translational accceleration bias is with respect to 𝑠 = q̇ or 𝑠 = v. 
[in]  frame_B  The frame on which points Bi are affixed/welded. 
[in]  p_BoBi_B  A position vector or list of p position vectors from Bo (frame_B's origin) to points Bi (regarded as affixed to B), where each position vector is expressed in frame_B. Each column in the 3 x p matrix p_BoBi_B corresponds to a position vector. 
[in]  frame_A  The frame that measures a𝑠Bias_ABi. Currently, an exception is thrown if frame_A is not the World frame. 
[in]  frame_E  The frame in which a𝑠Bias_ABi is expressed on output. 
3 x p
matrix, where p is the number of points Bi. v_ABi = J𝑠_v_ABi ⋅ 𝑠 which upon vector differentiation in A gives a_ABi = J𝑠_v_ABi ⋅ 𝑠̇ + J̇𝑠_v_ABi ⋅ 𝑠 Since J̇𝑠_v_ABi is linear in 𝑠, a𝑠Bias_ABi = J̇𝑠_v_ABi ⋅ 𝑠 is quadratic in 𝑠.
std::exception  if with_respect_to is not JacobianWrtVariable::kV 
std::exception  if frame_A is not the world frame. 
Vector3<T> CalcCenterOfMassPosition  (  const systems::Context< T > &  context  )  const 
(Deprecated.)
Vector3<T> CalcCenterOfMassPosition  (  const systems::Context< T > &  context, 
const std::vector< ModelInstanceIndex > &  model_instances  
)  const 
(Deprecated.)
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.
[in]  context  Contains the state of the model. 
p_WScm_W  position vector from Wo to Scm expressed in world frame W, where Scm is the center of mass of the system S stored by this plant. 
std::exception  if this has no body except world_body(). 
std::exception  if mₛ ≤ 0 (mₛ is the mass of this system S). 
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 bodies contained in model_instances, expressed in the world frame W.
[in]  context  Contains the state of the model. 
[in]  model_instances  Vector of selected model instances. This method does not distinguish between welded, joint connected, or floating bodies. 
p_WScm_W  position vector from world origin Wo to Scm expressed in the world frame W, where Scm is the center of mass of the system S of bodies contained in model_instances. 
std::exception  if model_instance only has world_model_instance(), i.e., model_instances has no body except world_body(). 
std::exception  if mₛ ≤ 0 (mₛ is the mass of the system S). 
Vector3<T> CalcCenterOfMassTranslationalVelocityInWorld  (  const systems::Context< T > &  context  )  const 
Calculates system center of mass translational velocity in world frame W.
[in]  context  The context contains the state of the model. 
v_WScm_W  Scm's translational velocity in frame W, expressed in W, where Scm is the center of mass of the system S stored by this plant. 
std::exception  if this has no body except world_body(). 
std::exception  if mₛ ≤ 0 (mₛ is the mass of this system S). 
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.
[in]  context  The context contains the state of the model. 
[in]  model_instances  The vector of selected model instances. 
v_WScm_W  Scm's translational velocity in frame W, expressed in W, where Scm is the center of mass of the system S in model_instances. 
std::exception  if this has no body except world_body(). 
std::exception  if mₛ ≤ 0 (mₛ is the mass of this system S). 
void CalcForceElementsContribution  (  const systems::Context< T > &  context, 
MultibodyForces< T > *  forces  
)  const 
Computes the combined force contribution of ForceElement objects in the model.
A ForceElement can apply forces as a spatial force per body or as generalized forces, depending on the ForceElement model. ForceElement contributions are a function of the state and time only. The output from this method can immediately be used as input to CalcInverseDynamics() to include the effect of applied forces by force elements.
[in]  context  The context containing the state of this model. 
[out]  forces  A pointer to a valid, non nullptr, multibody forces object. On output forces will store the forces exerted by all the ForceElement objects in the model. 
std::exception  if forces is null or not compatible with this model, per MultibodyForces::CheckInvariants(). 
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
.
The vector of generalized forces due to gravity 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:
Mv̇ + C(q, v)v = tau_g(q) + tau_app
where tau_app
includes any other generalized forces applied on the system.
[in]  context  The context storing the state of the model. 
v
for this
so that the inner product v⋅tau_g
corresponds to the power applied by the gravity forces on the mechanical system. That is, v⋅tau_g > 0
corresponds to potential energy going into the system, as either mechanical kinetic energy, some other potential energy, or heat, and therefore to a decrease of the gravitational potential energy. 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.
Mathematically, this method computes:
tau = M(q)v̇ + C(q, v)v  tau_app  ∑ J_WBᵀ(q) Fapp_Bo_W
where M(q)
is the model's mass matrix, C(q, v)v
is the bias term containing Coriolis and gyroscopic effects and tau_app
consists of a vector applied generalized forces. The last term is a summation over all bodies in the model where Fapp_Bo_W
is an applied spatial force on body B at Bo
which gets projected into the space of generalized forces with the transpose of Jv_V_WB(q)
(where Jv_V_WB
is B's spatial velocity Jacobian in W with respect to generalized velocities v). Note: B's spatial velocity in W can be written as V_WB = Jv_V_WB * v
. This method does not compute explicit expressions for the mass matrix nor for the bias term, which would be of at least O(n²)
complexity, but it implements an O(n)
NewtonEuler recursive algorithm, where n is the number of bodies in the model. The explicit formation of the mass matrix M(q)
would require the calculation of O(n²)
entries while explicitly forming the product C(q, v) * v
could require up to O(n³)
operations (see [Featherstone 1987, §4]), depending on the implementation. The recursive NewtonEuler algorithm is the most efficient currently known general method for solving inverse dynamics [Featherstone 2008].
[in]  context  The context containing the state of the model. 
[in]  known_vdot  A vector with the known generalized accelerations vdot for the full model. Use the provided Joint APIs in order to access entries into this array. 
[in]  external_forces  A set of forces to be applied to the system either as body spatial forces Fapp_Bo_W or generalized forces tau_app , see MultibodyForces for details. 
known_vdot
. 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" 𝑠.
J𝑠_w_AB ≜ [ ∂(w_AB)/∂𝑠₁, ... ∂(w_AB)/∂𝑠ₙ ] (n is j or k) w_AB = J𝑠_w_AB ⋅ 𝑠 w_AB is linear in 𝑠 ≜ [𝑠₁ ... 𝑠ₙ]ᵀ
w_AB
is B's angular velocity in frame A and "speeds" 𝑠 is either q̇ ≜ [q̇₁ ... q̇ⱼ]ᵀ (timederivatives of j generalized positions) or v ≜ [v₁ ... vₖ]ᵀ (k generalized velocities).
[in]  context  The state of the multibody system. 
[in]  with_respect_to  Enum equal to JacobianWrtVariable::kQDot or JacobianWrtVariable::kV, indicating whether the Jacobian J𝑠_w_AB is partial derivatives with respect to 𝑠 = q̇ (timederivatives of generalized positions) or with respect to 𝑠 = v (generalized velocities). 
[in]  frame_B  The frame B in w_AB (B's angular velocity in A). 
[in]  frame_A  The frame A in w_AB (B's angular velocity in A). 
[in]  frame_E  The frame in which w_AB is expressed on input and the frame in which the Jacobian J𝑠_w_AB is expressed on output. 
[out]  J𝑠_w_AB_E  Frame B's angular velocity Jacobian in frame A with respect to speeds 𝑠 (which is either q̇ or v), expressed in frame E. The Jacobian is a function of only generalized positions q (which are pulled from the context). The previous definition shows J𝑠_w_AB_E is a matrix of size 3 x n , where n is the number of elements in 𝑠. 
std::exception  if J𝑠_w_AB_E is nullptr or not of size 3 x n . 
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 
This method computes 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 composite center of mass of the system of all bodies in the MultibodyPlant (except world_body()).
[in]  context  The state of the multibody system. 
[in]  with_respect_to  Enum equal to JacobianWrtVariable::kQDot or JacobianWrtVariable::kV, indicating whether the Jacobian J𝑠_v_ACcm_E is partial derivatives with respect to 𝑠 = q̇ (timederivatives of generalized positions) or with respect to 𝑠 = v (generalized velocities). 
[in]  frame_A  The frame in which the translational velocity v_ACcm and its Jacobian J𝑠_v_ACcm are measured. 
[in]  frame_E  The frame in which the Jacobian J𝑠_v_ACcm is expressed on output. 
[out]  J𝑠_v_ACcm_E  Point Ccm's translational velocity Jacobian in frame A with respect to speeds 𝑠 (𝑠 = q̇ or 𝑠 = v), expressed in frame E. J𝑠_v_ACcm_E is a 3 x n matrix, where n is the number of elements in 𝑠. The Jacobian is a function of only generalized positions q (which are pulled from the context). 
std::runtime_error  if CCm does not exist, which occurs if there are no massive bodies in MultibodyPlant (except world_body()). 
std::exception  if composite_mass <= 0, where composite_mass is the total mass of all bodies except world_body() in MultibodyPlant. 
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" 𝑠.
J𝑠_V_ABp ≜ [ ∂(V_ABp)/∂𝑠₁, ... ∂(V_ABp)/∂𝑠ₙ ] (n is j or k) V_ABp = J𝑠_V_ABp ⋅ 𝑠 V_ABp is linear in 𝑠 ≜ [𝑠₁ ... 𝑠ₙ]ᵀ
V_ABp
is Bp's spatial velocity in frame A and "speeds" 𝑠 is either q̇ ≜ [q̇₁ ... q̇ⱼ]ᵀ (timederivatives of j generalized positions) or v ≜ [v₁ ... vₖ]ᵀ (k generalized velocities).
[in]  context  The state of the multibody system. 
[in]  with_respect_to  Enum equal to JacobianWrtVariable::kQDot or JacobianWrtVariable::kV, indicating whether the Jacobian J𝑠_V_ABp is partial derivatives with respect to 𝑠 = q̇ (timederivatives of generalized positions) or with respect to 𝑠 = v (generalized velocities). 
[in]  frame_B  The frame on which point Bp is fixed/welded. 
[in]  p_BoBp_B  A position vector from Bo (frame_B's origin) to point Bp (regarded as fixed/welded to B), expressed in frame_B. 
[in]  frame_A  The frame that measures v_ABp (Bp's velocity in A). Note: It is natural to wonder why there is no parameter p_AoAp_A (similar to the parameter p_BoBp_B for frame_B). There is no need for p_AoAp_A because Bp's velocity in A is defined as the derivative in frame A of Bp's position vector from any point fixed to A. 
[in]  frame_E  The frame in which v_ABp is expressed on input and the frame in which the Jacobian J𝑠_V_ABp is expressed on output. 
[out]  J𝑠_V_ABp_E  Point Bp's spatial velocity Jacobian in frame A with respect to speeds 𝑠 (which is either q̇ or v), expressed in frame E. J𝑠_V_ABp_E is a 6 x n matrix, where n is the number of elements in 𝑠. The Jacobian is a function of only generalized positions q (which are pulled from the context). Note: The returned 6 x n matrix stores frame B's angular velocity Jacobian in A in rows 13 and stores point Bp's translational velocity Jacobian in A in rows 46, i.e., J𝑠_w_AB_E = J𝑠_V_ABp_E.topRows<3>(); J𝑠_v_ABp_E = J𝑠_V_ABp_E.bottomRows<3>();Note: Consider CalcJacobianTranslationalVelocity() for multiple points fixed to frame B and consider CalcJacobianAngularVelocity() to calculate frame B's angular velocity Jacobian. 
std::exception  if J𝑠_V_ABp_E is nullptr or not sized 6 x n . 
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" 𝑠.
J𝑠_v_ABi ≜ [ ∂(v_ABi)/∂𝑠₁, ... ∂(v_ABi)/∂𝑠ₙ ] (n is j or k) v_ABi = J𝑠_v_ABi ⋅ 𝑠 v_ABi is linear in 𝑠 ≜ [𝑠₁ ... 𝑠ₙ]ᵀ
v_ABi
is Bi's translational velocity in frame A and "speeds" 𝑠 is either q̇ ≜ [q̇₁ ... q̇ⱼ]ᵀ (timederivatives of j generalized positions) or v ≜ [v₁ ... vₖ]ᵀ (k generalized velocities).
[in]  context  The state of the multibody system. 
[in]  with_respect_to  Enum equal to JacobianWrtVariable::kQDot or JacobianWrtVariable::kV, indicating whether the Jacobian J𝑠_v_ABi is partial derivatives with respect to 𝑠 = q̇ (timederivatives of generalized positions) or with respect to 𝑠 = v (generalized velocities). 
[in]  frame_B  The frame on which point Bi is affixed/welded. 
[in]  p_BoBi_B  A position vector or list of p position vectors from Bo (frame_B's origin) to points Bi (regarded as affixed to B), where each position vector is expressed in frame_B. 
[in]  frame_A  The frame that measures v_ABi (Bi's velocity in A). Note: It is natural to wonder why there is no parameter p_AoAi_A (similar to the parameter p_BoBi_B for frame_B). There is no need for p_AoAi_A because Bi's velocity in A is defined as the derivative in frame A of Bi's position vector from any point affixed to A. 
[in]  frame_E  The frame in which v_ABi is expressed on input and the frame in which the Jacobian J𝑠_v_ABi is expressed on output. 
[out]  J𝑠_v_ABi_E  Point Bi's velocity Jacobian in frame A with respect to speeds 𝑠 (which is either q̇ or v), expressed in frame E. J𝑠_v_ABi_E is a 3*p x n matrix, where p is the number of points Bi and n is the number of elements in 𝑠. The Jacobian is a function of only generalized positions q (which are pulled from the context). 
std::exception  if J𝑠_v_ABi_E is nullptr or not sized 3*p x n . 
Jq̇_v_ABi = Jq_p_AoBi
. In other words, point Bi's velocity Jacobian in frame A with respect to q̇ is equal to point Bi's position Jacobian from Ao (A's origin) in frame A with respect to q. [∂(v_ABi)/∂q̇₁, ... ∂(v_ABi)/∂q̇ⱼ] = [∂(p_AoBi)/∂q₁, ... ∂(p_AoBi)/∂qⱼ]Note: Each partial derivative of p_AoBi is taken in frame A.
void CalcMassMatrix  (  const systems::Context< T > &  context, 
EigenPtr< MatrixX< T >>  M  
)  const 
Performs the computation of the mass matrix M(q)
of the model, as a function of the generalized positions q stored in context
.
This method employs the Composite Body Algorithm, which is known to be the fastest O(n²) algorithm to compute the mass matrix of a multibody system.
[in]  context  The context containing the state of the model. 
[out]  M  A valid (nonnull) pointer to a squared matrix in ℛⁿˣⁿ with n the number of generalized velocities (num_velocities()) of the model. This method aborts if M is nullptr or if it does not have the proper size. 
void CalcMassMatrixViaInverseDynamics  (  const systems::Context< T > &  context, 
EigenPtr< MatrixX< T >>  M  
)  const 
Performs the computation of the mass matrix M(q)
of the model using inverse dynamics, where the generalized positions q are stored in context
.
Use CalcMassMatrix() for a faster implementation using the Composite Body Algorithm.
[in]  context  The context containing the state of the model. 
[out]  M  A valid (nonnull) pointer to a squared matrix in ℛⁿˣⁿ with n the number of generalized velocities (num_velocities()) of the model. This method aborts if H is nullptr or if it does not have the proper size. 
The algorithm used to build M(q)
consists in computing one column of M(q)
at a time using inverse dynamics. The result from inverse dynamics, with no applied forces, is the vector of generalized forces:
tau = M(q)v̇ + C(q, v)v
where q and v are the generalized positions and velocities, respectively. When v = 0
the Coriolis and gyroscopic forces term C(q, v)v
is zero. Therefore the ith
column of M(q)
can be obtained performing inverse dynamics with an acceleration vector v̇ = eᵢ
, with eᵢ
the standard (or natural) basis of ℛⁿ
with n the number of generalized velocities. We write this as:
M.ᵢ(q) = M(q) * e_i
where M.ᵢ(q)
(notice the dot for the rows index) denotes the ith
column in M(q).
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.
[in]  context  The context containing the state of the model. It stores the generalized positions q of the model. 
[in]  frame_B  The frame B in which the positions p_BQi of a set of points Qi are given. 
[in]  p_BQi  The input positions of each point Qi in frame B. p_BQi ∈ ℝ³ˣⁿᵖ with np the number of points in the set. Each column of p_BQi corresponds to a vector in ℝ³ holding the position of one of the points in the set as measured and expressed in frame B. 
[in]  frame_A  The frame A in which it is desired to compute the positions p_AQi of each point Qi in the set. 
[out]  p_AQi  The output positions of each point Qi now computed as measured and expressed in frame A. The output p_AQi must have the same size as the input p_BQi or otherwise this method aborts. That is p_AQi must be in ℝ³ˣⁿᵖ . 
p_BQi
and p_AQi
must have three rows. Otherwise this method will throw a std::runtime_error exception. This method also throws a std::runtime_error exception if p_BQi
and p_AQi
differ in the number of columns. 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.
[in]  context  The state of the multibody system, which includes the system's generalized positions q. Note: R_FG is a function of q. 
[in]  frame_F  The frame F designated in the rigid transform R_FG . 
[in]  frame_G  The frame G designated in the rigid transform R_FG . 
R_FG  The RigidTransform relating frame F and frame G. 
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.
[in]  context  The state of the multibody system, which includes the system's generalized positions q. Note: X_FG is a function of q. 
[in]  frame_F  The frame F designated in the rigid transform X_FG . 
[in]  frame_G  The frame G designated in the rigid transform X_FG . 
X_FG  The RigidTransform relating frame F and frame G. 
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.
[in]  context  The context containing the state of this model. 
[in]  known_vdot  A vector with the generalized accelerations for the full model. 
[out]  A_WB_array  A pointer to a valid, non nullptr, vector of spatial accelerations containing the spatial acceleration A_WB for each body. It must be of size equal to the number of bodies in the model. On output, entries will be ordered by BodyIndex. 
std::exception  if A_WB_array is not of size num_bodies() . 
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.
[in]  context  Contains the state of the model. 
[in]  p_WoP_W  Position from Wo (origin of the world frame W) to an arbitrary point P, expressed in the world frame W. 
L_WSP_W,spatial  momentum of the system S represented by this plant, measured in the world frame W, about point P, expressed in W. 
MultibodyPlant<T> plant; // ... code to load a model .... const Vector3<T> p_WoScm_W = plant.CalcCenterOfMassPositionInWorld(context); const SpatialMomentum<T> L_WScm_W = plant.CalcSpatialMomentumInWorldAboutPoint(context, p_WoScm_W);
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.
[in]  context  Contains the state of the model. 
[in]  model_instances  Set of selected model instances. 
[in]  p_WoP_W  Position from Wo (origin of the world frame W) to an arbitrary point P, expressed in the world frame W. 
L_WSP_W,spatial  momentum of the system S represented by the model_instances, measured in world frame W, about point P, expressed in W. 
MultibodyPlant<T> plant; // ... code to create a set of selected model instances, e.g., ... const ModelInstanceIndex gripper_model_instance = plant.GetModelInstanceByName("gripper"); const ModelInstanceIndex robot_model_instance = plant.GetBodyByName("end_effector").model_instance(); const std::vector<ModelInstanceIndex> model_instances{ gripper_model_instance, robot_model_instance}; const Vector3<T> p_WoScm_W = plant.CalcCenterOfMassPositionInWorld(context, model_instances); SpatialMomentum<T> L_WScm_W = plant.CalcSpatialMomentumInWorldAboutPoint(context, model_instances, p_WoScm_W);
std::exception  if model_instances contains an invalid ModelInstanceIndex. 
T CalcTotalMass  (  const systems::Context< T > &  context  )  const 
Calculates the total mass of all bodies in this MultibodyPlant.
[in]  context  Contains the state of the model. 
The  total mass of all bodies or 0 if there are none. 
T CalcTotalMass  (  const systems::Context< T > &  context, 
const std::vector< ModelInstanceIndex > &  model_instances  
)  const 
Calculates the total mass of all bodies contained in model_instances.
[in]  context  Contains the state of the model. 
[in]  model_instances  Vector of selected model instances. This method does not distinguish between welded, joint connected, or floating bodies. 
The  total mass of all bodies belonging to a model instance in model_instances or 0 if model_instances is empty. 
geometry::GeometrySet CollectRegisteredGeometries  (  const std::vector< const Body< T > * > &  bodies  )  const 
For each of the provided bodies
, collects up all geometries that have been registered to that body.
Intended to be used in conjunction with SceneGraph::ExcludeCollisionsWithin() and SceneGraph::ExcludeCollisionsBetween() to filter collisions between the geometries registered to the bodies.
For example:
Changing the order will cause exceptions to be thrown.
std::exception  if called prefinalize. 
const math::RigidTransform<T>& EvalBodyPoseInWorld  (  const systems::Context< T > &  context, 
const Body< T > &  body_B  
)  const 
Evaluate the pose X_WB
of a body B in the world frame W.
[in]  context  The context storing the state of the model. 
[in]  body_B  The body B for which the pose is requested. 
X_WB  The pose of body frame B in the world frame W. 
std::logic_error  if Finalize() was not called on this model or if body_B does not belong to this model. 
const SpatialAcceleration<T>& EvalBodySpatialAccelerationInWorld  (  const systems::Context< T > &  context, 
const Body< T > &  body_B  
)  const 
Evaluates A_WB, body B's spatial acceleration in the world frame W.
[in]  context  The context storing the state of the model. 
[in]  body_B  The body for which spatial acceleration is requested. 
A_WB_W  Body B's spatial acceleration in the world frame W, expressed in W (for point Bo, the body's origin). 
std::logic_error  if Finalize() was not called on this model or if body_B does not belong to this model. 
const SpatialVelocity<T>& EvalBodySpatialVelocityInWorld  (  const systems::Context< T > &  context, 
const Body< T > &  body_B  
)  const 
Evaluates V_WB, body B's spatial velocity in the world frame W.
[in]  context  The context storing the state of the model. 
[in]  body_B  The body B for which the spatial velocity is requested. 
V_WB_W  Body B's spatial velocity in the world frame W, expressed in W (for point Bo, the body's origin). 
std::logic_error  if Finalize() was not called on this model or if body_B does not belong to this model. 
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
.
Each entry in the returned vector corresponds to a single point pair corresponding to two interpenetrating bodies A and B. The size of the returned vector corresponds to the total number of contact penetration pairs. If no geometry was registered, the output vector is empty.
std::exception  if called prefinalize. See Finalize(). 
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 collision geometries between two given collision filter groups.
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.
It essentially compiles all the necessary "topological information", i.e. how bodies, joints and, any other elements connect with each other, and performs all the required preprocessing to enable computations at a later stage.
If the finalize stage is successful, the topology of this MultibodyPlant is valid, meaning that the topology is uptodate after this call. No more multibody elements can be added after a call to Finalize().
At Finalize(), state and input/output ports for this
plant are declared. If this
plant registered geometry with a SceneGraph, input and output ports to enable communication with that SceneGraph are declared as well.
If geometry has been registered on a SceneGraph instance, that instance must be provided to the Finalize() method so that any geometric implications of the finalization process can be appropriately handled.
std::logic_error  if the MultibodyPlant has already been finalized. 
bool geometry_source_is_registered  (  )  const 
Returns true
if this
MultibodyPlant was registered with a SceneGraph.
This method can be called at any time during the lifetime of this
plant to query if this
plant has been registered with a SceneGraph, either pre or postfinalize, see Finalize().
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.
This input port is a vector valued port, which can be set with JointActuator::set_actuation_vector().
this
plant. std::exception  if called before Finalize(). 
std::exception  if the model instance does not exist. 
const systems::InputPort<T>& get_actuation_input_port  (  )  const 
Returns a constant reference to the input port for external actuation for the case where only one model instance has actuated dofs.
This input port is a vector valued port, which can be set with JointActuator::set_actuation_vector().
this
plant. std::exception  if called before Finalize(), if the model does not contain any actuators, or if multiple model instances have actuated dofs. 
const systems::InputPort<T>& get_applied_generalized_force_input_port  (  )  const 
Returns a constant reference to the vectorvalued input port for applied generalized forces, and the vector will be added directly into tau
(see System dynamics).
This vector is ordered using the same convention as the plant velocities: you can set the generalized forces that will be applied to model instance i using, e.g., SetVelocitiesInArray(i, model_forces, &force_array)
.
std::exception  if called before Finalize(). 
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.
The data type for the port is an std::vector of ExternallyAppliedSpatialForce; any number of spatial forces can be applied to any number of bodies in the plant.
Returns a constant reference to the body with unique index body_index
.
std::exception  if body_index does not correspond to a body in this model. 
const systems::OutputPort<T>& get_body_poses_output_port  (  )  const 
Returns the output port of all body poses in the world frame.
You can obtain the pose X_WB
of a body B in the world frame W with:
As shown in the example above, the resulting std::vector
of body poses is indexed by BodyIndex, and it has size num_bodies(). BodyIndex "zero" (0) always corresponds to the world body, with pose equal to the identity at all times.
std::exception  if called prefinalize. 
const systems::OutputPort<T>& get_body_spatial_accelerations_output_port  (  )  const 
Returns the output port of all body spatial accelerations in the world frame.
You can obtain the spatial acceleration A_WB
of a body B in the world frame W with:
As shown in the example above, the resulting std::vector
of body spatial accelerations is indexed by BodyIndex, and it has size num_bodies(). BodyIndex "zero" (0) always corresponds to the world body, with zero spatial acceleration at all times.
std::exception  if called prefinalize. 
const systems::OutputPort<T>& get_body_spatial_velocities_output_port  (  )  const 
Returns the output port of all body spatial velocities in the world frame.
You can obtain the spatial velocity V_WB
of a body B in the world frame W with:
As shown in the example above, the resulting std::vector
of body spatial velocities is indexed by BodyIndex, and it has size num_bodies(). BodyIndex "zero" (0) always corresponds to the world body, with zero spatial velocity at all times.
std::exception  if called prefinalize. 
ContactModel get_contact_model  (  )  const 
Returns the model used for contact. See documentation for ContactModel.
double get_contact_penalty_method_time_scale  (  )  const 
Returns a timescale estimate tc
based on the requested penetration allowance δ set with set_penetration_allowance().
For the penalty method in use to enforce nonpenetration, this time scale relates to the time it takes the relative normal velocity between two bodies to go to zero. This time scale tc
is artificially introduced by the penalty method and goes to zero in the limit to ideal rigid contact. Since numerical integration methods for continuum systems must be able to resolve a system's dynamics, the time step used by an integrator must in general be much smaller than the time scale tc
. How much smaller will depend on the details of the problem and the convergence characteristics of the integrator and should be tuned appropriately. Another factor to take into account for setting up the simulation's time step is the speed of the objects in your simulation. If vn
represents a reference velocity scale for the normal relative velocity between bodies, the new time scale tn = δ / vn
represents the time it would take for the distance between two bodies approaching with relative normal velocity vn
to decrease by the penetration_allowance δ. In this case a user should choose a time step for simulation that can resolve the smallest of the two time scales tc
and tn
.
const systems::OutputPort<T>& get_contact_results_output_port  (  )  const 
Returns a constant reference to the port that outputs ContactResults.
std::exception  if called prefinalize, see Finalize(). 
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
.
std::runtime_error  when force_element_index does not correspond to a force element in this model. 
const Frame<T>& get_frame  (  FrameIndex  frame_index  )  const 
Returns a constant reference to the frame with unique index frame_index
.
std::exception  if frame_index does not correspond to a frame in this plant. 
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.
this
plant. std::exception  if called before Finalize(). 
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.
this
plant. std::exception  if called before Finalize(). 
std::exception  if the model instance does not exist. 
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.
this
plant. std::exception  if called before Finalize(). 
std::exception  if the model instance does not exist. 
const systems::OutputPort<T>& get_geometry_poses_output_port  (  )  const 
Returns the output port of frames' poses to communicate with a SceneGraph.
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.
See SceneGraph::get_query_output_port(). Refer to section Geometry of this class's documentation for further details on collision geometry registration and connection with a SceneGraph.
const Joint<T>& get_joint  (  JointIndex  joint_index  )  const 
Returns a constant reference to the joint with unique index joint_index
.
std::runtime_error  when joint_index does not correspond to a joint in this model. 
const JointActuator<T>& get_joint_actuator  (  JointActuatorIndex  actuator_index  )  const 
Returns a constant reference to the joint actuator with unique index actuator_index
.
std::exception  if actuator_index does not correspond to a joint actuator in this tree. 
Joint<T>& get_mutable_joint  (  JointIndex  joint_index  ) 
Returns a mutable reference to the joint with unique index joint_index
.
std::runtime_error  when joint_index does not correspond to a joint in this model. 
JointActuator<T>& get_mutable_joint_actuator  (  JointActuatorIndex  actuator_index  )  const 
Returns a mutable reference to the joint actuator with unique index actuator_index
.
std::exception  if actuator_index does not correspond to a joint actuator in this tree. 
const systems::OutputPort<T>& get_reaction_forces_output_port  (  )  const 
Returns the port for joint reaction forces.
A Joint models the kinematical relationship which characterizes the possible relative motion between two bodies. In Drake, a joint connects a frame Jp
on parent body P with a frame Jc
on a child body C. This usage of the terms parent and child is just a convention and implies nothing about the inboardoutboard relationship between the bodies. Since a Joint imposes a kinematical relationship which characterizes the possible relative motion between frames Jp and Jc, reaction forces on each body are established. That is, we could cut the model at the joint and replace it with equivalent forces equal to these reaction forces in order to attain the same motions of the mechanical system.
This output port allows to evaluate the reaction force F_CJc_Jc
on the child body C, at Jc
, and expressed in Jc for all joints in the model. This port evaluates to a vector of type std::vector<SpatialForce<T>> and size num_joints() indexed by JointIndex, see Joint::index(). Each entry corresponds to the spatial force F_CJc_Jc
applied on the joint's child body C (Joint::child_body()), at the joint's child frame Jc
(Joint::frame_on_child()) and expressed in frame Jc
.
std::exception  if called prefinalize. 
std::optional<geometry::SourceId> get_source_id  (  )  const 
Returns the unique id identifying this
plant as a source for a SceneGraph.
Returns nullopt
if this
plant did not register any geometry. This method can be called at any time during the lifetime of this
plant to query if this
plant has been registered with a SceneGraph, either pre or postfinalize, see Finalize(). However, a geometry::SourceId is only assigned once at the first call of any of this plant's geometry registration methods, and it does not change after that. Postfinalize calls will always return the same value.
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.
this
plant. std::exception  if called before Finalize(). 
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.
(Here qᵢ ⊆ q and vᵢ ⊆ v.)
this
plant. std::exception  if called before Finalize(). 
std::exception  if the model instance does not exist. 
VectorX<double> GetAccelerationLowerLimits  (  )  const 
Returns a vector of size num_velocities()
containing the lower acceleration limits for every generalized velocity coordinate.
These include joint and free body coordinates. Any unbounded or unspecified limits will be infinity.
std::logic_error  if called prefinalize. 
VectorX<double> GetAccelerationUpperLimits  (  )  const 
Upper limit analog of GetAccelerationsLowerLimits(), where any unbounded or unspecified limits will be +infinity.
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 model.
This method throws an exception if u
is not of size MultibodyPlant::num_actuated_dofs().
Returns all bodies that are transitively welded, or rigidly affixed, to body
, per these two definitions:
This method can be called at any time during the lifetime of this
plant, either pre or postfinalize, see Finalize().
Meant to be used with CollectRegisteredGeometries
.
The following example demonstrates filtering collisions between all bodies rigidly affixed to a door (which could be moving) and all bodies rigidly affixed to the world:
body
. This does not return the bodies in any prescribed order. std::exception  if body is not part of this plant. 
const Body<T>& GetBodyByName  (  std::string_view  name  )  const 
Returns a constant reference to a body that is identified by the string name
in this
MultibodyPlant.
std::logic_error  if there is no body with the requested name. 
std::logic_error  if the body name occurs in multiple model instances. 
this
MultibodyPlant with a given specified name. const Body<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.
std::logic_error  if there is no body with the requested name. 
this
MultibodyPlant with a given specified name. std::optional<geometry::FrameId> GetBodyFrameIdIfExists  (  BodyIndex  body_index  )  const 
If the body with body_index
belongs to the called plant, it returns the geometry::FrameId associated with it.
Otherwise, it returns nullopt.
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.
Otherwise this method throws an exception.
std::exception  if the called plant does not have the body indicated by body_index . 
const Body<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).
std::vector<BodyIndex> GetBodyIndices  (  ModelInstanceIndex  model_instance  )  const 
Returns a list of body indices associated with model_instance
.
const std::vector<geometry::GeometryId>& GetCollisionGeometriesForBody  (  const Body< T > &  body  )  const 
Returns an array of GeometryId's identifying the different contact geometries for body
previously registered with a SceneGraph.
this
plant, either pre or postfinalize, see Finalize(). Postfinalize calls will always return the same value. const math::RigidTransform<double>& GetDefaultFreeBodyPose  (  const Body< T > &  body  )  const 
Gets the default pose of body
as set by SetDefaultFreeBodyPose().
[in]  body  Body whose default pose will be retrieved. 
std::unordered_set<BodyIndex> GetFloatingBaseBodies  (  )  const 
Returns the set of body indexes corresponding to the free (floating) bodies in the model, in no particular order.
std::exception  if called prefinalize, see Finalize(). 
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.
If the optional template argument is supplied, then the returned value is downcast to the specified ForceElementType
.
ForceElementType  The specific type of the ForceElement to be retrieved. It must be a subclass of ForceElement. 
std::logic_error  if the force element is not of type ForceElementType or if there is no ForceElement with that index. 
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.
std::logic_error  if there is no frame with the requested name. 
std::logic_error  if the frame name occurs in multiple model instances. 
this
model with a given specified name. 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
.
std::logic_error  if there is no frame with the requested name. 
std::runtime_error  if model_instance is not valid for this model. 
this
model with a given specified name. math::RigidTransform<T> GetFreeBodyPose  (  const systems::Context< T > &  context, 
const Body< T > &  body  
)  const 
Gets the pose of a given body
in the world frame W.
std::exception  if body is not a free body in the model. 
std::exception  if called prefinalize. 
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.
std::logic_error  if there is no actuator with the requested name. 
std::logic_error  if the actuator name occurs in multiple model instances. 
this
MultibodyPlant with a given specified name. 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.
std::logic_error  if there is no actuator with the requested name. 
std::exception  if model_instance is not valid for this model. 
this
MultibodyPlant with a given specified name. 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.
If the optional template argument is supplied, then the returned value is downcast to the specified JointType
.
JointType  The specific type of the Joint to be retrieved. It must be a subclass of Joint. 
std::logic_error  if the named joint is not of type JointType or if there is no Joint with that name. 
std::exception  if model_instance is not valid for this model. 
this
MultibodyPlant with a given specified name. std::vector<JointIndex> GetJointIndices  (  ModelInstanceIndex  model_instance  )  const 
Returns a list of joint indices associated with model_instance
.
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.
std::logic_error  if there is no instance with the requested name. 
this
MultibodyPlant with a given specified name. const std::string& GetModelInstanceName  (  ModelInstanceIndex  model_instance  )  const 
Returns the name of a model_instance
.
std::logic_error  when model_instance does not correspond to a model in this model. 
JointType<T>& GetMutableJointByName  (  std::string_view  name, 
std::optional< ModelInstanceIndex >  model_instance = std::nullopt 

) 
A version of GetJointByName that returns a mutable reference.
Eigen::VectorBlock<VectorX<T> > GetMutablePositions  (  systems::Context< T > *  context  )  const 
(Advanced) Returns a mutable vector reference containing the vector of generalized positions (see warning).
std::exception  if the context is nullptr or if it does not correspond to the context for a multibody model. 
Eigen::VectorBlock<VectorX<T> > GetMutablePositions  (  const systems::Context< T > &  context, 
systems::State< T > *  state  
)  const 
(Advanced) Returns a mutable vector reference containing the vector of generalized positions (see warning).
std::exception  if the state is nullptr or if the context does not correspond to the context for a multibody model. 
state
comes from this MultibodyPlant. Eigen::VectorBlock<VectorX<T> > GetMutablePositionsAndVelocities  (  systems::Context< T > *  context  )  const 
(Advanced) Returns a mutable vector containing the vector [q; v]
of the model with q
the vector of generalized positions and v
the vector of generalized velocities (see warning).
std::exception  if the context is nullptr or if it does not correspond to the context for a multibody model. 
Eigen::VectorBlock<VectorX<T> > GetMutableVelocities  (  const systems::Context< T > &  context, 
systems::State< T > *  state  
)  const 
(Advanced) Returns a mutable vector reference containing the vector of generalized velocities (see warning).
std::exception  if the context is nullptr or the context does not correspond to the context for a multibody model. 
state
comes from this MultibodyPlant. Eigen::VectorBlock<VectorX<T> > GetMutableVelocities  (  systems::Context< T > *  context  )  const 
See GetMutableVelocities() method above.
VectorX<double> GetPositionLowerLimits  (  )  const 
Returns a vector of size num_positions()
containing the lower position limits for every generalized position coordinate.
These include joint and free body coordinates. Any unbounded or unspecified limits will be infinity.
std::logic_error  if called prefinalize. 
Eigen::VectorBlock<const VectorX<T> > GetPositions  (  const systems::Context< T > &  context  )  const 
Returns a const vector reference containing the vector of generalized positions.
std::exception  if the context does not correspond to the context for a multibody model. 
VectorX<T> GetPositions  (  const systems::Context< T > &  context, 
ModelInstanceIndex  model_instance  
)  const 
Returns an vector containing the generalized positions (q
) for the given model instance.
std::exception  if the context does not correspond to the context for a multibody model. 
q.size()
associated with model_instance
in O(q.size()
) time. Eigen::VectorBlock<const VectorX<T> > GetPositionsAndVelocities  (  const systems::Context< T > &  context  )  const 
Returns a const vector reference containing the vector [q; v]
with q
the vector of generalized positions and v
the vector of generalized velocities.
std::exception  if the context does not correspond to the context for a multibody model. 
VectorX<T> GetPositionsAndVelocities  (  const systems::Context< T > &  context, 
ModelInstanceIndex  model_instance  
)  const 
Returns the vector [q; v]
of the model with q
the vector of generalized positions and v
the vector of generalized velocities for model instance model_instance
.
std::exception  if the context does not correspond to the context for a multibody model or model_instance is invalid. 
q.size() + v.size()
associated with model_instance
in O(q.size()
) time. 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.
This method throws an exception if q
is not of size MultibodyPlant::num_positions().
VectorX<double> GetPositionUpperLimits  (  )  const 
Upper limit analog of GetPositionsLowerLimits(), where any unbounded or unspecified limits will be +infinity.
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.
std::logic_error  if there is no body with the requested name. 
std::logic_error  if the body name occurs in multiple model instances. 
std::logic_error  if the requested body is not a RigidBody. 
this
model with a given specified name. 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
.
std::logic_error  if there is no body with the requested name. 
std::logic_error  if the requested body is not a RigidBody. 
std::runtime_error  if model_instance is not valid for this model. 
this
model with a given specified name. std::string GetTopologyGraphvizString  (  )  const 
Returns a Graphviz string describing the topology of this plant.
To render the string, use the Graphviz tool, dot
. http://www.graphviz.org/
Note: this method can be called either before or after Finalize()
.
const Body<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.
Throw an exception otherwise.
std::exception  if called prefinalize. 
std::exception  if model_instance is not valid. 
std::exception  if HasUniqueFreeBaseBody(model_instance) == false. 
Eigen::VectorBlock<const VectorX<T> > GetVelocities  (  const systems::Context< T > &  context  )  const 
Returns a const vector reference containing the generalized velocities.
VectorX<T> GetVelocities  (  const systems::Context< T > &  context, 
ModelInstanceIndex  model_instance  
)  const 
Returns a vector containing the generalized velocities (v
) for the given model instance.
std::exception  if the context does not correspond to the context for a multibody model. 
v.size()
associated with model_instance
in O(v.size()
) time. 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.
This method throws an exception if the input array is not of size MultibodyPlant::num_velocities().
VectorX<double> GetVelocityLowerLimits  (  )  const 
Returns a vector of size num_velocities()
containing the lower velocity limits for every generalized velocity coordinate.
These include joint and free body coordinates. Any unbounded or unspecified limits will be infinity.
std::logic_error  if called prefinalize. 
VectorX<double> GetVelocityUpperLimits  (  )  const 
Upper limit analog of GetVelocitysLowerLimits(), where any unbounded or unspecified limits will be +infinity.
const std::vector<geometry::GeometryId>& GetVisualGeometriesForBody  (  const Body< T > &  body  )  const 
Returns an array of GeometryId's identifying the different visual geometries for body
previously registered with a SceneGraph.
this
plant, either pre or postfinalize, see Finalize(). Postfinalize calls will always return the same value. const UniformGravityFieldElement<T>& gravity_field  (  )  const 
An accessor to the current gravity field.
bool HasBodyNamed  (  std::string_view  name  )  const 
true
if a body named name
was added to the MultibodyPlant. std::logic_error  if the body name occurs in multiple model instances. 
bool HasBodyNamed  (  std::string_view  name, 
ModelInstanceIndex  model_instance  
)  const 
true
if a body named name
was added to the MultibodyPlant in model_instance
. std::exception  if model_instance is not valid for this model. 
bool HasFrameNamed  (  std::string_view  name  )  const 
true
if a frame named name
was added to the model. std::logic_error  if the frame name occurs in multiple model instances. 
bool HasFrameNamed  (  std::string_view  name, 
ModelInstanceIndex  model_instance  
)  const 
true
if a frame named name
was added to model_instance
. std::exception  if model_instance is not valid for this model. 
bool HasJointActuatorNamed  (  std::string_view  name  )  const 
true
if an actuator named name
was added to this model. std::logic_error  if the actuator name occurs in multiple model instances. 
bool HasJointActuatorNamed  (  std::string_view  name, 
ModelInstanceIndex  model_instance  
)  const 
true
if an actuator named name
was added to model_instance
. std::exception  if model_instance is not valid for this model. 
bool HasJointNamed  (  std::string_view  name  )  const 
true
if a joint named name
was added to this model. std::logic_error  if the joint name occurs in multiple model instances. 
bool HasJointNamed  (  std::string_view  name, 
ModelInstanceIndex  model_instance  
)  const 
true
if a joint named name
was added to model_instance
. std::exception  if model_instance is not valid for this model. 
bool HasModelInstanceNamed  (  std::string_view  name  )  const 
true
if a model instance named name
was added to this model. 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.
std::exception  if called prefinalize. 
std::exception  if model_instance is not valid. 
bool is_finalized  (  )  const 
Returns true
if this MultibodyPlant was finalized with a call to Finalize().
bool IsAnchored  (  const Body< T > &  body  )  const 
Returns true
if body
is anchored (i.e.
the kinematic path between body
and the world only contains weld joints.)
std::exception  if called prefinalize. 
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_actuators() and nv
equal to num_velocities().
The vector u of actuation values is of size num_actuators(). For a given JointActuator, u[JointActuator::index()]
stores the value for the external actuation corresponding to that actuator. tau_u
on the other hand is indexed by generalized velocity indexes according to Joint::velocity_start()
.
O(n)
complexity, making a full B matrix has O(n²)
complexity. For most applications this cost can be neglected but it could become significant for very large systems. MatrixX<double> MakeActuatorSelectorMatrix  (  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.
The mapping, or selection, is returned in the form of a selector matrix Su such that u = Su⋅uₛ
. The size nₛ of uₛ is always smaller or equal than the size of the full vector of actuation values u. That is, a user might be interested in only a given subset of actuators in the model.
This selection matrix is particularly useful when adding PID control on a portion of the state, see systems::controllers::PidController.
A user specifies the preferred order in uₛ via user_to_actuator_index_map
. The actuation values in uₛ are a concatenation of the values for each actuator in the order they appear in user_to_actuator_index_map
. The full vector of actuation values u is ordered by JointActuatorIndex.
MatrixX<double> MakeActuatorSelectorMatrix  (  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 (ordered by JointActuatorIndex) and uₛ is a vector of actuation values for the actuators acting on the joints listed by user_to_joint_index_map
.
It is assumed that all joints referenced by user_to_joint_index_map
are actuated. See MakeActuatorSelectorMatrix(const std::vector<JointActuatorIndex>&) for details.
std::logic_error  if any of the joints in user_to_joint_index_map does not have an actuator. 
MatrixX<double> MakeStateSelectorMatrix  (  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.
The mapping, or selection, is returned in the form of a selector matrix Sx such that xₛ = Sx⋅x
. The size nₛ of xₛ is always smaller or equal than the size of the full state x. That is, a user might be interested in only a given portion of the full state x.
This selection matrix is particularly useful when adding PID control on a portion of the state, see systems::controllers::PidController.
A user specifies the preferred order in xₛ via user_to_joint_index_map
. The selected state is built such that selected positions are followed by selected velocities, as in xₛ = [qₛ, vₛ]
. The positions in qₛ are a concatenation of the positions for each joint in the order they appear in user_to_joint_index_map
. That is, the positions for user_to_joint_index_map[0]
are first, followed by the positions for user_to_joint_index_map[1]
, etc. Similarly for the selected velocities vₛ.
std::logic_error  if there are repeated indexes in user_to_joint_index_map . 
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
.
v
and q̇
are related linearly by q̇ = N(q)⋅v
. Although N(q)
is not necessarily square, its left pseudoinverse N⁺(q)
can be used to invert that relationship without residual error, provided that qdot
is in the range space of N(q)
(that is, if it could have been produced as q̇ = N(q)⋅v
for some v
). Using the configuration q
stored in the given context
this method calculates v = N⁺(q)⋅q̇
.
[in]  context  The context containing the state of the model. 
[in]  qdot  A vector containing the time derivatives of the generalized positions. This method aborts if qdot is not of size num_positions(). 
[out]  v  A valid (nonnull) pointer to a vector in ℛⁿ with n the number of generalized velocities. This method aborts if v is nullptr or if it is not of size num_velocities(). 
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
).
v
and qdot
are related linearly by q̇ = N(q)⋅v
. Using the configuration q
stored in the given context
this method calculates q̇ = N(q)⋅v
.
[in]  context  The context containing the state of the model. 
[in]  v  A vector of of generalized velocities for this model. This method aborts if v is not of size num_velocities(). 
[out]  qdot  A valid (nonnull) pointer to a vector in ℝⁿ with n being the number of generalized positions in this model, given by num_positions() . This method aborts if qdot is nullptr or if it is not of size num_positions(). 
UniformGravityFieldElement<T>& mutable_gravity_field  (  ) 
A mutable accessor to the current gravity field.
int num_actuated_dofs  (  )  const 
Returns the total number of actuated degrees of freedom.
That is, the vector of actuation values u has this size. See AddJointActuator().
int num_actuated_dofs  (  ModelInstanceIndex  model_instance  )  const 
Returns the total number of actuated degrees of freedom for a specific model instance.
That is, the vector of actuation values u has this size. See AddJointActuator().
int num_actuators  (  )  const 
Returns the number of joint actuators in the model.
int num_bodies  (  )  const 
Returns the number of bodies in the model, including the "world" body, which is always part of the model.
int num_collision_geometries  (  )  const 
Returns the number of geometries registered for contact modeling.
This method can be called at any time during the lifetime of this
plant, either pre or postfinalize, see Finalize(). Postfinalize calls will always return the same value.
int num_force_elements  (  )  const 
Returns the number of ForceElement objects.
int num_frames  (  )  const 
Returns the number of Frame objects in this model.
Frames include body frames associated with each of the bodies, including the world body. This means the minimum number of frames is one.
int num_joints  (  )  const 
Returns the number of joints in the model.
int num_model_instances  (  )  const 
Returns the number of model instances in the model.
int num_multibody_states  (  )  const 
Returns the size of the multibody system state vector x = [q v].
This will be num_positions()
plus num_velocities()
.
int num_multibody_states  (  ModelInstanceIndex  model_instance  )  const 
Returns the size of the multibody system state vector xᵢ = [qᵢ vᵢ] for model instance i.
(Here qᵢ ⊆ q and vᵢ ⊆ v.) will be num_positions(model_instance)
plus num_velocities(model_instance)
.
int num_positions  (  )  const 
Returns the size of the generalized position vector q for this model.
int num_positions  (  ModelInstanceIndex  model_instance  )  const 
Returns the size of the generalized position vector qᵢ for model instance i.
int num_velocities  (  )  const 
Returns the size of the generalized velocity vector v for this model.
int num_velocities  (  ModelInstanceIndex  model_instance  )  const 
Returns the size of the generalized velocity vector vᵢ for model instance i.
int num_visual_geometries  (  )  const 
Returns the number of geometries registered for visualization.
This method can be called at any time during the lifetime of this
plant, either pre or postfinalize, see Finalize(). Postfinalize calls will always return the same value.

delete 

delete 
geometry::SourceId RegisterAsSourceForSceneGraph  (  geometry::SceneGraph< T > *  scene_graph  ) 
Registers this
plant to serve as a source for an instance of SceneGraph.
This registration allows MultibodyPlant to register geometry with scene_graph
for visualization and/or collision queries. The string returned by this>get_name()
is passed to SceneGraph's RegisterSource, so it is highly recommended that you give the plant a recognizable name before calling this. Successive registration calls with SceneGraph must be performed on the same instance to which the pointer argument scene_graph
points to. Failure to do so will result in runtime exceptions.
scene_graph  A valid non nullptr to the SceneGraph instance for which this plant will sever as a source, see SceneGraph documentation for further details. 
this
plant in scene_graph
. It can also later on be retrieved with get_source_id(). std::exception  if called postfinalize. 
std::exception  if scene_graph is the nullptr. 
std::exception  if called more than once. 
geometry::GeometryId RegisterCollisionGeometry  (  const Body< 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 than one geometry can be registered with a body, in which case the body's contact geometry is the union of all geometries registered to that body.
[in]  body  The body for which geometry is being registered. 
[in]  X_BG  The fixed pose of the geometry frame G in the body frame B. 
[in]  shape  The geometry::Shape used for visualization. E.g.: geometry::Sphere, geometry::Cylinder, etc. 
[in]  properties  The proximity properties associated with the collision geometry. They must include the (material , coulomb_friction ) property of type CoulombFriction<double>. 
std::exception  if called postfinalize or if the properties are missing the coulomb friction property (or if it is of the wrong type). 
geometry::GeometryId RegisterCollisionGeometry  (  const Body< 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.
geometry::GeometryId RegisterVisualGeometry  (  const Body< 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
.
[in]  body  The body for which geometry is being registered. 
[in]  X_BG  The fixed pose of the geometry frame G in the body frame B. 
[in]  shape  The geometry::Shape used for visualization. E.g.: geometry::Sphere, geometry::Cylinder, etc. 
[in]  name  The name for the geometry. It must satisfy the requirements defined in drake::geometry::GeometryInstance. 
[in]  properties  The illustration properties for this geometry. 
std::exception  if called postfinalize. 
std::exception  if scene_graph does not correspond to the same instance with which RegisterAsSourceForSceneGraph() was called. 
geometry::GeometryId RegisterVisualGeometry  (  const Body< 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::DrakeVisualizercompatible set of geometry::IllustrationProperties.
geometry::GeometryId RegisterVisualGeometry  (  const Body< 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).
void set_contact_model  (  ContactModel  model  ) 
Sets the contact model to be used by this
MultibodyPlant, see ContactModel for available options.
The default contact model is ContactModel::kPointContactOnly.
std::exception  iff called postfinalize. 
void set_penetration_allowance  (  double  penetration_allowance = 0.001  ) 
Sets the penetration allowance used to estimate the coefficients in the penalty method used to impose nonpenetration among bodies.
Refer to the section Contact by penalty method for further details.
std::logic_error  if penetration_allowance is not positive. 
void set_stiction_tolerance  (  double  v_stiction = 0.001  ) 
Currently MultibodyPlant uses the Stribeck approximation to model dry friction. The Stribeck model of friction is an approximation to Coulomb's law of friction that allows using continuous time integration without the need to specify complementarity constraints. While this results in a simpler model immediately tractable with standard numerical methods for integration of ODE's, it often leads to stiff dynamics that require an explicit integrator to take very small time steps. It is therefore recommended to use error controlled integrators when using this model or the discrete time stepping (see Choice of Time Advancement Strategy). See Continuous Approximation of Coulomb for a detailed discussion of the Stribeck model.
Sets the stiction tolerance v_stiction
for the Stribeck model, where v_stiction
must be specified in m/s (meters per second.) v_stiction
defaults to a value of 1 millimeter per second. In selecting a value for v_stiction
, you must ask yourself the question, "When two objects are ostensibly in stiction, how much slip am I willing
to allow?" There are two opposing design issues in picking a value for vₛ. On the one hand, small values of vₛ make the problem numerically stiff during stiction, potentially increasing the integration cost. On the other hand, it should be picked to be appropriate for the scale of the problem. For example, a car simulation could allow a "large" value for vₛ of 1 cm/s (1×10⁻² m/s), but reasonable stiction for grasping a 10 cm box might require limiting residual slip to 1×10⁻³ m/s or less. Ultimately, picking the largest viable value will allow your simulation to run faster and more robustly. Note that v_stiction
is the slip velocity that we'd have when we are at edge of the friction cone. For cases when the friction force is well within the friction cone the slip velocity will always be smaller than this value. See also Continuous Approximation of Coulomb.
std::exception  if v_stiction is nonpositive. 
void SetActuationInArray  (  ModelInstanceIndex  model_instance, 
const Eigen::Ref< const VectorX< T >> &  u_instance,  
EigenPtr< VectorX< T >>  u  
)  const 
Given the actuation values u_instance
for all actuators in model_instance
, this method sets the actuation vector u for the entire model to which this actuator belongs to.
This method throws an exception if the size of u_instance
is not equal to the number of degrees of freedom of all of the actuated joints in model_instance
.
[in]  u_instance  Actuation values for the actuators. It must be of size equal to the number of degrees of freedom of all of the actuated joints in model_instance . 
[out]  u  The vector containing the actuation values for the entire model. 
void SetDefaultFreeBodyPose  (  const Body< T > &  body, 
const math::RigidTransform< double > &  X_WB  
) 
Sets the default pose of body
.
If body.is_floating()
is true, this will affect subsequent calls to SetDefaultState(); otherwise, this value is effectively ignored.
[in]  body  Body whose default pose will be set. 
[in]  X_WB  Default pose of the body. 

override 
Sets state
according to defaults set by the user for joints (e.g.
RevoluteJoint::set_default_angle()) and free bodies (SetDefaultFreeBodyPose()). If the user does not specify defaults, the state corresponds to zero generalized positions and velocities.
std::exception  if called prefinalize. See Finalize(). 
void SetFreeBodyPose  (  systems::Context< T > *  context, 
const Body< 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.
std::exception  if body is not a free body in the model. 
std::exception  if called prefinalize. 
void SetFreeBodyPose  (  const systems::Context< T > &  context, 
systems::State< T > *  state,  
const Body< 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.
std::exception  if body is not a free body in the model. 
std::exception  if called prefinalize. 
state
comes from this MultibodyPlant. void SetFreeBodyPoseInAnchoredFrame  (  systems::Context< T > *  context, 
const Frame< T > &  frame_F,  
const Body< 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.
Frame F must be anchored, meaning that it is either directly welded to the world frame W or, more generally, that there is a kinematic path between frame F and the world frame W that only includes weld joints.
std::logic_error  if called prefinalize. 
std::logic_error  if frame F is not anchored to the world. 
void SetFreeBodyPoseInWorldFrame  (  systems::Context< T > *  context, 
const Body< 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.
[in]  context  The context to store the pose X_WB of body_B . 
[in]  body_B  The body B corresponding to the pose X_WB to be stored in context . 
X_WB  The pose of body frame B in the world frame W. 
std::exception  if body is not a free body in the model. 
std::logic_error  if called prefinalize. 
void SetFreeBodyRandomPositionDistribution  (  const Body< T > &  body, 
const Vector3< symbolic::Expression > &  position  
) 
Sets the distribution used by SetRandomState() to populate the free body's xyz position
with respect to World.
std::exception  if body is not a free body in the model. 
std::exception  if called prefinalize. 
void SetFreeBodyRandomRotationDistribution  (  const Body< 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.
std::exception  if body is not a free body in the model. 
std::exception  if called prefinalize. 
void SetFreeBodyRandomRotationDistributionToUniform  (  const Body< T > &  body  ) 
Sets the distribution used by SetRandomState() to populate the free body's rotation with respect to World using uniformly random rotations.
std::exception  if body is not a free body in the model. 
std::exception  if called prefinalize. 
void SetFreeBodySpatialVelocity  (  systems::Context< T > *  context, 
const Body< 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.
std::exception  if body is not a free body in the model. 
std::exception  if called prefinalize. 
void SetFreeBodySpatialVelocity  (  const systems::Context< T > &  context, 
systems::State< T > *  state,  
const Body< 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.
std::exception  if body is not a free body in the model. 
std::exception  if called prefinalize. 
state
comes from this MultibodyPlant. void SetPositions  (  systems::Context< T > *  context, 
const VectorX< T > &  q  
)  const 
Sets all generalized positions from the given vector.
std::exception  if the context is nullptr, if the context does not correspond to the context for a multibody model, or if the length of q is not equal to num_positions() . 
void SetPositions  (  systems::Context< T > *  context, 
ModelInstanceIndex  model_instance,  
const VectorX< T > &  q_instance  
)  const 
Sets the positions for a particular model instance from the given vector.
std::exception  if the context is nullptr, if the context does not correspond to the context for a multibody model, if the model instance index is invalid, or if the length of q_instance is not equal to num_positions(model_instance) . 
void SetPositions  (  const systems::Context< T > &  context, 
systems::State< T > *  state,  
ModelInstanceIndex  model_instance,  
const VectorX< T > &  q_instance  
)  const 
Sets the positions for a particular model instance from the given vector.
std::exception  if the state is nullptr, if the context does not correspond to the context for a multibody model, if the model instance index is invalid, or if the length of q_instance is not equal to num_positions(model_instance) . 
state
comes from this MultibodyPlant. void SetPositionsAndVelocities  (  systems::Context< T > *  context, 
const VectorX< T > &  q_v  
)  const 
Sets all generalized positions and velocities from the given vector [q; v].
std::exception  if the context is nullptr, if the context does not correspond to the context for a multibody model, or if the length of q_v is not equal to num_positions() + num_velocities() . 
void SetPositionsAndVelocities  (  systems::Context< T > *  context, 
ModelInstanceIndex  model_instance,  
const VectorX< T > &  q_v  
)  const 
Sets generalized positions and velocities from the given vector [q; v] for the specified model instance.
std::exception  if the context is nullptr, if the context does not correspond to the context for a multibody model, if the model instance index is invalid, or if the length of q_v is not equal to num_positions(model_instance) + num_velocities(model_instance) . 
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.
This method throws an exception if q
is not of size MultibodyPlant::num_positions() or q_instance
is not of size MultibodyPlant::num_positions(model_instance)
.

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).
void SetVelocities  (  systems::Context< T > *  context, 
const VectorX< T > &  v  
)  const 
Sets all generalized velocities from the given vector.
std::exception  if the context is nullptr, if the context does not correspond to the context for a multibody model, or if the length of v is not equal to num_velocities() . 
void SetVelocities  (  const systems::Context< T > &  context, 
systems::State< T > *  state,  
ModelInstanceIndex  model_instance,  
const VectorX< T > &  v_instance  
)  const 
Sets the generalized velocities for a particular model instance from the given vector.
std::exception  if the context is nullptr, if the context does not correspond to the context for a multibody model, if the model instance index is invalid, or if the length of v_instance is not equal to num_velocities(model_instance) . 
state
comes from this MultibodyPlant. void SetVelocities  (  systems::Context< T > *  context, 
ModelInstanceIndex  model_instance,  
const VectorX< T > &  v_instance  
)  const 
Sets the generalized velocities for a particular model instance from the given vector.
std::exception  if the context is nullptr, if the context does not correspond to the context for a multibody model, if the model instance index is invalid, or if the length of v_instance is not equal to num_velocities(model_instance) . 
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.
This method throws an exception if v
is not of size MultibodyPlant::num_velocities() or v_instance
is not of size MultibodyPlant::num_positions(model_instance)
.
double time_step  (  )  const 
The time step (or period) used to model this
plant as a discrete system with periodic updates.
Returns 0 (zero) if the plant is modeled as a continuous system. This property of the plant is specified at construction and therefore this query can be performed either pre or postfinalize, see Finalize().
const WeldJoint<T>& WeldFrames  (  const Frame< T > &  frame_on_parent_P, 
const Frame< T > &  frame_on_child_C,  
const math::RigidTransform< double > &  X_PC = math::RigidTransform< double >::Identity() 

) 
Welds frame_on_parent_P
and frame_on_child_C
with relative pose X_PC
.
That is, the pose of frame C in frame P is fixed, with value X_PC
. If X_PC
is omitted, the identity transform will be used. The call to this method creates and adds a new WeldJoint to the model. The new WeldJoint is named as: P.name() + "_welds_to_" + C.name().
std::exception  if the weld produces a duplicate joint name. 
const RigidBody<T>& world_body  (  )  const 
Returns a constant reference to the world body.
const BodyFrame<T>& world_frame  (  )  const 
Returns a constant reference to the world frame.

related 
Makes a new MultibodyPlant with discrete update period time_step
and adds it to a diagram builder together with the provided SceneGraph instance, connecting the geometry ports.
[in,out]  builder  Builder to add to. 
[in]  time_step  The discrete update period for the new MultibodyPlant to be added. Please refer to the documentation provided in MultibodyPlant::MultibodyPlant(double) for further details on the parameter time_step . 
[in]  scene_graph  (optional) Constructed scene graph. If none is provided, one will be created and used. 
builder
must be nonnull.

related 
Adds a MultibodyPlant and a SceneGraph instance to a diagram builder, connecting the geometry ports.
[in,out]  builder  Builder to add to. 
[in]  plant  Plant to be added to the builder. 
[in]  scene_graph  (optional) Constructed scene graph. If none is provided, one will be created and used. 
builder
and plant
must be nonnull.

friend 

friend 