pydrake.multibody.plant

Bindings for MultibodyPlant and related classes.

pydrake.multibody.plant.AddMultibodyPlantSceneGraph(*args, **kwargs)

Overloaded function.

  1. AddMultibodyPlantSceneGraph(builder: pydrake.systems.framework.DiagramBuilder_[float], plant: pydrake.multibody.plant.MultibodyPlant_[float] = None, scene_graph: pydrake.geometry.SceneGraph_[float] = None) -> tuple

Adds a MultibodyPlant and a SceneGraph instance to a diagram builder, connecting the geometry ports.

Parameter builder:
Builder to add to.
Parameter plant:
(optional) Constructed plant (e.g. for using a discrete plant). By default, a continuous plant is used.
Parameter scene_graph:
(optional) Constructed scene graph. If none is provided, one will be created and used.
Returns:Pair of the registered plant and scene graph.
Precondition:
builder must be non-null.

Recommended usages:

Assign to a MultibodyPlant reference (ignoring the SceneGraph):

MultibodyPlant<double>& plant = AddMultibodyPlantSceneGraph(&builder);
  plant.DoFoo(...);

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:

auto items = AddMultibodyPlantSceneGraph(&builder);
  items.plant.DoFoo(...);
  items.scene_graph.DoBar(...);

or

auto items = AddMultibodyPlantSceneGraph(&builder);
  MultibodyPlant<double>& plant = items.plant;
  SceneGraph<double>& scene_graph = items.scene_graph;
  ...
  plant.DoFoo(...);
  scene_graph.DoBar(...);

This is the easiest way to use both the plant and scene_graph.

Assign to already-declared pointer variables:

MultibodyPlant<double>* plant{};
  SceneGraph<double>* scene_graph{};
  std::tie(plant, scene_graph) = AddMultibodyPlantSceneGraph(&builder);
  plant->DoFoo(...);
  scene_graph->DoBar(...);

This flavor is most useful when the pointers are class member fields (and so perhaps cannot be references).

  1. AddMultibodyPlantSceneGraph(builder: pydrake.systems.framework.DiagramBuilder_[AutoDiffXd], plant: pydrake.multibody.plant.MultibodyPlant_[AutoDiffXd] = None, scene_graph: pydrake.geometry.SceneGraph_[AutoDiffXd] = None) -> tuple

Adds a MultibodyPlant and a SceneGraph instance to a diagram builder, connecting the geometry ports.

Parameter builder:
Builder to add to.
Parameter plant:
(optional) Constructed plant (e.g. for using a discrete plant). By default, a continuous plant is used.
Parameter scene_graph:
(optional) Constructed scene graph. If none is provided, one will be created and used.
Returns:Pair of the registered plant and scene graph.
Precondition:
builder must be non-null.

Recommended usages:

Assign to a MultibodyPlant reference (ignoring the SceneGraph):

MultibodyPlant<double>& plant = AddMultibodyPlantSceneGraph(&builder);
  plant.DoFoo(...);

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:

auto items = AddMultibodyPlantSceneGraph(&builder);
  items.plant.DoFoo(...);
  items.scene_graph.DoBar(...);

or

auto items = AddMultibodyPlantSceneGraph(&builder);
  MultibodyPlant<double>& plant = items.plant;
  SceneGraph<double>& scene_graph = items.scene_graph;
  ...
  plant.DoFoo(...);
  scene_graph.DoBar(...);

This is the easiest way to use both the plant and scene_graph.

Assign to already-declared pointer variables:

MultibodyPlant<double>* plant{};
  SceneGraph<double>* scene_graph{};
  std::tie(plant, scene_graph) = AddMultibodyPlantSceneGraph(&builder);
  plant->DoFoo(...);
  scene_graph->DoBar(...);

This flavor is most useful when the pointers are class member fields (and so perhaps cannot be references).

pydrake.multibody.plant.ConnectContactResultsToDrakeVisualizer(builder: pydrake.systems.framework.DiagramBuilder_[float], plant: pydrake.multibody.plant.MultibodyPlant_[float], lcm: pydrake.lcm.DrakeLcmInterface = None) → pydrake.systems.lcm.LcmPublisherSystem

Extends a Diagram with the required components to publish contact results to drake_visualizer. This must be called during Diagram building and uses the given builder to add relevant subsystems and connections.

This is a convenience method to simplify some common boilerplate for adding contact results visualization capability to a Diagram. What it does is:

  • adds systems ContactResultsToLcmSystem and LcmPublisherSystem to

the Diagram and connects the draw message output to the publisher input, - connects the multibody_plant contact results output to the ContactResultsToLcmSystem system, and - sets the publishing rate to 1/60 of a second (simulated time).

Parameter builder:
The diagram builder being used to construct the Diagram.
Parameter multibody_plant:
The System in builder containing the plant whose contact results are to be visualized.
Parameter lcm:
An optional lcm interface through which lcm messages will be dispatched. Will be allocated internally if none is supplied.
Precondition:
The given multibody_plant must be contained within the supplied DiagramBuilder.
Returns:the LcmPublisherSystem (in case callers, e.g., need to change the default publishing rate).
pydrake.multibody.plant.ContactResults

alias of pydrake.multibody.plant.ContactResults_[float]

class pydrake.multibody.plant.ContactResultsToLcmSystem

Bases: pydrake.systems.framework.LeafSystem_[float]

A System that encodes ContactResults into a lcmt_contact_results_for_viz message. It has a single input port with type ContactResults<T> and a single output port with lcmt_contact_results_for_viz.

Template parameter T:
Must be one of drake’s default scalar types.
get_contact_result_input_port(self: pydrake.multibody.plant.ContactResultsToLcmSystem) → pydrake.systems.framework.InputPort_[float]
get_lcm_message_output_port(self: pydrake.multibody.plant.ContactResultsToLcmSystem) → pydrake.systems.framework.OutputPort_[float]
template pydrake.multibody.plant.ContactResults_

Instantiations: ContactResults_[float], ContactResults_[AutoDiffXd], ContactResults_[Expression]

class ContactResults_[float]

A container class storing the contact results information for each contact pair for a given state of the simulation.

Template parameter T:
Must be one of drake’s default scalar types.
AddContactInfo(self: pydrake.multibody.plant.ContactResults_[float], point_pair_info: pydrake.multibody.plant.PointPairContactInfo_[float]) → None

Add a new contact pair result to the set of contact pairs stored by this class.

contact_info(self: pydrake.multibody.plant.ContactResults_[float], i: int) → pydrake.multibody.plant.PointPairContactInfo_[float]

Deprecated. Use point_pair_contact_info() instead.

num_contacts(self: pydrake.multibody.plant.ContactResults_[float]) → int

Returns the number of unique collision element pairs in contact.

point_pair_contact_info(self: pydrake.multibody.plant.ContactResults_[float], i: int) → pydrake.multibody.plant.PointPairContactInfo_[float]

Retrieves the ith PointPairContactInfo instance. The input index i must be in the range [0, get_num_contacts() - 1] or this method aborts.

pydrake.multibody.plant.CoulombFriction

alias of pydrake.multibody.plant.CoulombFriction_[float]

template pydrake.multibody.plant.CoulombFriction_

Instantiations: CoulombFriction_[float], CoulombFriction_[AutoDiffXd], CoulombFriction_[Expression]

class CoulombFriction_[float]

Parameters for Coulomb’s Law of Friction, namely:

  • Static friction coefficient, for a pair of surfaces at rest relative to each other.
  • Dynamic (or kinematic) friction coefficient, for a pair of surfaces in relative motion.

These coefficients are an empirical property characterizing the interaction by friction between a pair of contacting surfaces. Friction coefficients depend upon the mechanical properties of the surfaces’ materials and on the roughness of the surfaces. They are determined experimentally.

Even though the Coulomb’s law coefficients of friction characterize a pair of surfaces interacting by friction, we associate the abstract idea of friction coefficients to a single surface by considering the coefficients for contact between two identical surfaces. For this case of two identical surfaces, the friction coefficients that describe the surface pair are taken to equal those of one of the identical surfaces. We extend this idea to the case of different surfaces by defining a combination law that allow us to obtain the Coulomb’s law coefficients of friction characterizing the pair of surfaces, given the individual friction coefficients of each surface. We would like this combination law to satisfy:

  • The friction coefficient of two identical surfaces is the friction coefficient of one of the surfaces.
  • The combination law is commutative. That is, surface A combined with surface B gives the same results as surface B combined with surface A.
  • For two surfaces M and N with very different friction coefficients, say μₘ μₙ, the combined friction coefficient should be in the order of magnitude of the smallest friction coefficient (in the example μₘ). To understand this requirement, consider rubber (high friction coefficient) sliding on ice (low friction coefficient). We’d like the surface pair to be defined by a friction coefficient close to that of ice, since rubber will easily slide on ice.

These requirements are met by the following ad-hoc combination law:

μ = 2μₘμₙ/(μₘ + μₙ)

See CalcContactFrictionFromSurfaceProperties(), which implements this law. More complex combination laws could also be a function of other parameters such as the mechanical properties of the interacting surfaces or even their roughnesses. For instance, if the the rubber surface above has metal studs (somehow making the surface “rougher”), it will definitely have a better grip on an ice surface. Therefore this new variable should be taken into account in the combination law. Notice that in this example, this new combination law model for tires, will have a different set of requirements from the ones stated above.

Template parameter T:
Must be one of drake’s default scalar types.
pydrake.multibody.plant.ExternallyAppliedSpatialForce

alias of pydrake.multibody.plant.ExternallyAppliedSpatialForce_[float]

template pydrake.multibody.plant.ExternallyAppliedSpatialForce_

Instantiations: ExternallyAppliedSpatialForce_[float], ExternallyAppliedSpatialForce_[AutoDiffXd], ExternallyAppliedSpatialForce_[Expression]

class ExternallyAppliedSpatialForce_[float]
F_Bq_W

A spatial force applied to Body B at point Bq, expressed in the world frame.

body_index

The index of the body that the force is to be applied to.

p_BoBq_B

A position vector from Body B’s origin (Bo) to a point Bq (a point of B), expressed in B’s frame.

pydrake.multibody.plant.MultibodyPlant

alias of pydrake.multibody.plant.MultibodyPlant_[float]

template pydrake.multibody.plant.MultibodyPlant_

Instantiations: MultibodyPlant_[float], MultibodyPlant_[AutoDiffXd], MultibodyPlant_[Expression]

class MultibodyPlant_[float]

Bases: pydrake.systems.framework.LeafSystem_[float]

%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 for an overview of concepts/notation.

MultibodyPlant provides a user-facing API to:

  • add bodies, joints, force elements, and constraints,
  • register geometries to a provided SceneGraph instance,
  • create and manipulate its Context,
  • perform Context-dependent computational queries.

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 the time and u the input vector of actuation forces. The governing equations for the dynamics of a multibody system modeled with MultibodyPlant are [Featherstone 2008, Jain 2010]:

 = N(q)v
  (1)    M(q) + C(q, v)v = tau

where M(q) is the mass matrix of the multibody system, C(q, v)v corresponds to the bias term containing Coriolis and gyroscopic effects and N(q) is the kinematic coupling matrix describing the relationship between the rate of change of the generalized coordinates and the generalized velocities, [Seth 2010]. N(q) is an nq x nv matrix. The vector tau ℝⁿᵛ on the right hand side of Eq. (1) corresponds to generalized forces applied on the system. These can include externally applied body forces, constraint forces, and contact forces.

Drake has the capability of loading multibody models from SDF and URDF files. Consider the example below which loads an acrobot model:

MultibodyPlant<T> acrobot;
  SceneGraph<T> scene_graph;
  Parser parser(&acrobot, &scene_graph);
  const std::string relative_name =
    "drake/multibody/benchmarks/acrobot/acrobot.sdf";
  const std::string full_name = FindResourceOrThrow(relative_name);
  parser.AddModelFromFile(full_name);

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.

Clients of a MultibodyPlant can add multibody elements with the following methods:

  • Bodies: AddRigidBody().
  • Joints: AddJoint().

All modeling elements must be added pre-finalize.

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 pre-finalize.

If MultibodyPlant registers geometry with a SceneGraph via calls to RegisterCollisionGeometry(), an input port for geometric queries will be declared at Finalize() time, see get_geometry_query_input_port(). 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:

  1. Call to RegisterAsSourceForSceneGraph().
  2. Calls to RegisterCollisionGeometry(), as many as needed.
  3. Call to Finalize(), user is done specifying the model.
  4. Connect SceneGraph::get_query_output_port() to get_geometry_query_input_port().

Refer to the documentation provided in each of the methods above for further details.

Please refer to drake_contacts “Contact Modeling in Drake” for details on the available approximations, setup, and considerations for a multibody simulation with frictional contact.

Once the user is done adding modeling elements and registering geometry, a call to Finalize() must be performed. This call will:

  • Build the underlying MultibodyTree topology, see MultibodyTree::Finalize() for details,
  • declare the plant’s state,
  • declare the plant’s input and output ports,
  • declare input and output ports for communication with a SceneGraph.

** References **

  • [Featherstone 2008] Featherstone, R., 2008.
    Rigid body dynamics algorithms. Springer.
  • [Jain 2010] Jain, A., 2010.
    Robot and multibody dynamics: analysis and algorithms. Springer Science & Business Media.
  • [Seth 2010] Seth, A., Sherman, M., Eastman, P. and Delp, S., 2010.
    Minimal formulation of joint motion for biomechanisms. Nonlinear dynamics, 62(1), pp.291-303.
Template parameter T:
Must be one of drake’s default scalar types.
AddForceElement(*args, **kwargs)

Overloaded function.

  1. AddForceElement(self: pydrake.multibody.plant.MultibodyPlant_[float], force_element: pydrake.multibody.tree.UniformGravityFieldElement_[float]) -> pydrake.multibody.tree.UniformGravityFieldElement_[float]

Adds a new force element model of type ForceElementType to this MultibodyPlant. The arguments to this method args are forwarded to ``ForceElementType`’s constructor.

Parameter 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.
Template parameter 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. As there was not always a default gravity field element, for compatibility purposes calling this function to add a UniformGravityFieldElement which has the same value as gravity_field() is not an error.
Returns:A constant reference to the new ForceElement just added, of type ForceElementType<T> specialized on the scalar type T of this MultibodyPlant. It will remain valid for the lifetime of this MultibodyPlant.

See also

The ForceElement class’s documentation for further details on how a force element is defined.

  1. AddForceElement(self: pydrake.multibody.plant.MultibodyPlant_[float], force_element: pydrake.multibody.tree.ForceElement_[float]) -> pydrake.multibody.tree.ForceElement_[float]

Adds a new force element model of type ForceElementType to this MultibodyPlant. The arguments to this method args are forwarded to ``ForceElementType`’s constructor.

Parameter 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.
Template parameter 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. As there was not always a default gravity field element, for compatibility purposes calling this function to add a UniformGravityFieldElement which has the same value as gravity_field() is not an error.
Returns:A constant reference to the new ForceElement just added, of type ForceElementType<T> specialized on the scalar type T of this MultibodyPlant. It will remain valid for the lifetime of this MultibodyPlant.

See also

The ForceElement class’s documentation for further details on how a force element is defined.

AddFrame(self: pydrake.multibody.plant.MultibodyPlant_[float], frame: pydrake.multibody.tree.Frame_[float]) → pydrake.multibody.tree.Frame_[float]

This method adds a Frame of type FrameType<T>. For more information, please see the corresponding constructor of FrameType.

Template parameter FrameType:
Template which will be instantiated on T.
Parameter frame:
Unique pointer frame instance.
Returns:A constant reference to the new Frame just added, which will remain valid for the lifetime of this MultibodyPlant.
AddJoint(self: pydrake.multibody.plant.MultibodyPlant_[float], joint: pydrake.multibody.tree.Joint_[float]) → pydrake.multibody.tree.Joint_[float]

This method adds a Joint of type JointType between two bodies. For more information, see the below overload of AddJoint<>, and the related MultibodyTree::AddJoint<> method.

AddModelInstance(self: pydrake.multibody.plant.MultibodyPlant_[float], name: str) → pydrake.multibody.tree.ModelInstanceIndex

Creates a new model instance. Returns the index for the model instance.

Parameter 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().
AddRigidBody(*args, **kwargs)

Overloaded function.

  1. AddRigidBody(self: pydrake.multibody.plant.MultibodyPlant_[float], name: str, M_BBo_B: pydrake.multibody.tree.SpatialInertia_[float]) -> pydrake.multibody.tree.RigidBody_[float]

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 (model_instance “more on model instances”).

Example of usage:

MultibodyPlant<T> plant;
  // ... Code to define spatial_inertia, a SpatialInertia<T> object ...
  const RigidBody<T>& body =
    plant.AddRigidBody("BodyName", spatial_inertia);
Parameter name:
A string that identifies the new body to be added to this model. A RuntimeError is thrown if a body named name already is part of the model in the default model instance. See HasBodyNamed(), Body::name().
Parameter 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.
Returns:

A constant reference to the new RigidBody just added, which will remain valid for the lifetime of this MultibodyPlant.

Raises:
  • RuntimeError if additional model instances have been created
  • beyond the world and default instances.
  1. AddRigidBody(self: pydrake.multibody.plant.MultibodyPlant_[float], name: str, model_instance: pydrake.multibody.tree.ModelInstanceIndex, M_BBo_B: pydrake.multibody.tree.SpatialInertia_[float]) -> pydrake.multibody.tree.RigidBody_[float]

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:

MultibodyPlant<T> plant;
  // ... Code to define spatial_inertia, a SpatialInertia<T> object ...
  ModelInstanceIndex model_instance = plant.AddModelInstance("instance");
  const RigidBody<T>& body =
    plant.AddRigidBody("BodyName", model_instance, spatial_inertia);
Parameter name:
A string that identifies the new body to be added to this model. A RuntimeError is thrown if a body named name already is part of model_instance. See HasBodyNamed(), Body::name().
Parameter model_instance:
A model instance index which this body is part of.
Parameter 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.
Returns:A constant reference to the new RigidBody just added, which will remain valid for the lifetime of this MultibodyPlant.
CalcBiasTerm(self: pydrake.multibody.plant.MultibodyPlant_[float], context: pydrake.systems.framework.Context_[float]) → numpy.ndarray[float64[m, 1]]

Computes the bias term C(q, v)v containing Coriolis and gyroscopic effects of the multibody equations of motion:

M(q)v̇ + C(q, v)v = tau_app + ∑ J_WBᵀ(q) Fapp_Bo_W

where M(q) is the multibody model’s mass matrix 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 geometric Jacobian J_WB(q) which maps generalized velocities into body B spatial velocity as V_WB = J_WB(q)v.

Parameter context:
The context containing the state of the model. It stores the generalized positions q and the generalized velocities v.
Parameter Cv:
On output, Cv will contain the product C(q, v)v. It must be a valid (non-null) 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.
CalcConservativePower(self: pydrake.multibody.plant.MultibodyPlant_[float], context: pydrake.systems.framework.Context_[float]) → float

Computes and returns the power generated by conservative forces in the multibody model. This quantity is defined to be positive when the potential energy is decreasing. In other words, if U(q) is the potential energy as defined by CalcPotentialEnergy(), then the conservative power, Pc, is Pc = -U̇(q).

See also

CalcPotentialEnergy()

CalcForceElementsContribution(self: pydrake.multibody.plant.MultibodyPlant_[float], context: pydrake.systems.framework.Context_[float], forces: pydrake.multibody.tree.MultibodyForces_[float]) → None

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.

Parameter context:
The context containing the state of this model.
Parameter 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.
Raises:
  • RuntimeError if forces is null or not compatible with this
  • model, per MultibodyForces::CheckInvariants().
CalcFrameGeometricJacobianExpressedInWorld(self: pydrake.multibody.plant.MultibodyPlant_[float], context: pydrake.systems.framework.Context_[float], frame_B: pydrake.multibody.tree.Frame_[float], p_BoFo_B: numpy.ndarray[float64[3, 1]] = array([ 0., 0., 0.])) → numpy.ndarray[float64[m, n]]

Given a frame Fp defined by shifting a frame F from its origin Fo to a new origin P, this method computes the geometric Jacobian Jv_WFp for frame Fp. The new origin P is specified by the position vector p_FP in frame F. The frame geometric Jacobian Jv_WFp is defined by:

V_WFp(q, v) = Jv_WFp(q)⋅v

where V_WFp(q, v) is the spatial velocity of frame Fp measured and expressed in the world frame W and q and v are the vectors of generalized position and velocity, respectively. The geometric Jacobian Jv_WFp(q) is a function of the generalized coordinates q only.

Parameter context:
The context containing the state of the model. It stores the generalized positions q.
Parameter frame_F:
The position p_FP of frame Fp is measured and expressed in this frame F.
Parameter p_FP:
The (fixed) position of the origin P of frame Fp as measured and expressed in frame F.
Parameter Jv_WFp:
The geometric Jacobian Jv_WFp(q), function of the generalized positions q only. This Jacobian relates to the spatial velocity V_WFp of frame Fp by:
V_WFp(q, v) = Jv_WFp(q)⋅v

Therefore Jv_WFp is a matrix of size 6 x nv, with nv the number of generalized velocities. On input, matrix Jv_WFp must have size 6 x nv or this method throws an exception. The top rows of this matrix (which can be accessed with Jv_WFp.topRows<3>()) is the Jacobian Hw_WFp related to the angular velocity of Fp in W by w_WFp = Hw_WFp⋅v. The bottom rows of this matrix (which can be accessed with Jv_WFp.bottomRows<3>()) is the Jacobian Hv_WFp related to the translational velocity of the origin P of frame Fp in W by v_WFpo = Hv_WFp⋅v. This ordering is consistent with the internal storage of the SpatialVelocity class. Therefore the following operations results in a valid spatial velocity:

SpatialVelocity<double> Jv_WFp_times_v(Jv_WFp * v);
Raises:
  • RuntimeError if J_WFp is nullptr or if it is not of size ``6 x
  • nv``.
CalcGravityGeneralizedForces(self: pydrake.multibody.plant.MultibodyPlant_[float], context: pydrake.systems.framework.Context_[float]) → numpy.ndarray[float64[m, 1]]

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.

Parameter context:
The context storing the state of the model.
Returns:tau_g A vector containing the generalized forces due to gravity. The generalized forces are consistent with the vector of generalized velocities 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.
CalcInverseDynamics(self: pydrake.multibody.plant.MultibodyPlant_[float], context: pydrake.systems.framework.Context_[float], known_vdot: numpy.ndarray[float64[m, 1]], external_forces: pydrake.multibody.tree.MultibodyForces_[float]) → numpy.ndarray[float64[m, 1]]

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 geometric Jacobian J_WB(q) which maps generalized velocities into body B spatial velocity as V_WB = J_WB(q)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) Newton-Euler 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 Newton-Euler algorithm is the most efficient currently known general method for solving inverse dynamics [Featherstone 2008].

Parameter context:
The context containing the state of the model.
Parameter 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.
Parameter 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.
Returns:the vector of generalized forces that would need to be applied to the mechanical system in order to achieve the desired acceleration given by known_vdot.
CalcJacobianSpatialVelocity(self: pydrake.multibody.plant.MultibodyPlant_[float], context: pydrake.systems.framework.Context_[float], with_respect_to: pydrake.multibody.tree.JacobianWrtVariable, frame_B: pydrake.multibody.tree.Frame_[float], p_BP: numpy.ndarray[float64[3, 1]], frame_A: pydrake.multibody.tree.Frame_[float], frame_E: pydrake.multibody.tree.Frame_[float]) → numpy.ndarray[float64[m, n]]

For each point Bi of (fixed to) a frame B with spatial velocity v_ABi in a frame A, calculates Bi’s spatial velocity Jacobian in A with respect to 𝑠, which is defined as

Js_V_ABi = [ ∂(V_ABi)/∂𝑠₁,  ...  ∂(V_ABi)/∂𝑠ₙ ]    (n is j or k)

where “speeds” 𝑠 is either q̇ ≜ [q̇₁ … q̇ⱼ]ᵀ (time-derivatives of j generalized positions) or v ≜ [v₁ … vₖ]ᵀ (k generalized velocities). Note: Spatial velocity V_ABi is linear in 𝑠₁, … 𝑠ₙ and can be written V_ABi = Js_V_ABi 𝑠 where 𝑠 is [𝑠₁ … 𝑠ₙ]ᵀ.

Parameter context:
The state of the multibody system.
Parameter with_respect_to:
Enum equal to JacobianWrtVariable::kQDot or JacobianWrtVariable::kV, indicating whether the Jacobian Js_V_ABi is partial derivatives with respect to 𝑠 = q̇ (time-derivatives of generalized positions) or with respect to 𝑠 = v (generalized velocities).
Parameter frame_B:
The frame on which point Bi is fixed (e.g., welded).
Parameter p_BoBi_B:
A position vector or list of p position vectors from Bo (frame_B’s origin) to points Bi (regarded as fixed to B), where each position vector is expressed in frame_B.
Parameter frame_A:
The frame that measures v_ABi (Bi’s velocity in A).
Parameter frame_E:
The frame in which v_ABi is expressed on input and the frame in which the Jacobian Js_V_ABi is expressed on output.
Parameter Js_V_ABi_E:
Point Bi’s spatial velocity Jacobian in frame A with respect to speeds 𝑠 (which is either q̇ or v), expressed in frame E. Js_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). Note: If p = 1 (one point), a 6 x n matrix is returned with the first three rows storing frame B’s angular velocity Jacobian in A and rows 4-6 storing point Bi’s translational velocity Jacobian in A, i.e.,
Js_wAB_E = Js_V_ABi_E.topRows<3>();
    Js_vAB1_E = Js_V_ABi_E.bottomRows<3>();

If p = 2 (two points), a 12 x n matrix is returned. Rows 1-3 and 7-9 store exactly identical information (B’s angular velocity Jacobian in A). Rows 4-6 store point B1’s translational velocity Jacobian which differs from rows 10-12 which store point B2’s translational velocity Jacobian. If p is large and storage efficiency is a concern, calculate frame B’s angular velocity Jacobian using CalcJacobianAngularVelocity() and then use CalcJacobianTranslationalVelocity().

Raises:
  • RuntimeError if Js_V_ABi_E is nullptr or not sized ``3*p x
  • n``.
CalcMassMatrixViaInverseDynamics(self: pydrake.multibody.plant.MultibodyPlant_[float], context: pydrake.systems.framework.Context_[float]) → numpy.ndarray[float64[m, n]]
CalcPointsPositions(self: pydrake.multibody.plant.MultibodyPlant_[float], context: pydrake.systems.framework.Context_[float], frame_B: pydrake.multibody.tree.Frame_[float], p_BQi: numpy.ndarray[float64[m, n], flags.f_contiguous], frame_A: pydrake.multibody.tree.Frame_[float]) → numpy.ndarray[float64[m, n]]

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.

Parameter context:
The context containing the state of the model. It stores the generalized positions q of the model.
Parameter frame_B:
The frame B in which the positions p_BQi of a set of points Qi are given.
Parameter 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.
Parameter frame_A:
The frame A in which it is desired to compute the positions p_AQi of each point Qi in the set.
Parameter 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 ℝ³ˣⁿᵖ.

Note

Both p_BQi and p_AQi must have three rows. Otherwise this method will throw a RuntimeError exception. This method also throws a RuntimeError exception if p_BQi and p_AQi differ in the number of columns.

CalcPotentialEnergy(self: pydrake.multibody.plant.MultibodyPlant_[float], context: pydrake.systems.framework.Context_[float]) → float

Computes and returns the total potential energy stored in this multibody model for the configuration given by context.

Parameter context:
The context containing the state of the model.
Returns:The total potential energy stored in this multibody model.
CalcRelativeTransform(self: pydrake.multibody.plant.MultibodyPlant_[float], context: pydrake.systems.framework.Context_[float], frame_A: pydrake.multibody.tree.Frame_[float], frame_B: pydrake.multibody.tree.Frame_[float]) → pydrake.math.RigidTransform_[float]

Calculates the rigid transform (pose) X_FG relating frame F and frame G.

Parameter context:
The state of the multibody system, which includes the system’s generalized positions q. Note: X_FG is a function of q.
Parameter frame_F:
The frame F designated in the rigid transform X_FG.
Parameter frame_G:
The frame G designated in the rigid transform X_FG.
Returns X_FG:
The RigidTransform relating frame F and frame G.
CalcSpatialAccelerationsFromVdot(self: pydrake.multibody.plant.MultibodyPlant_[float], context: pydrake.systems.framework.Context_[float], known_vdot: numpy.ndarray[float64[m, 1]]) → List[pydrake.multibody.math.SpatialAcceleration_[float]]

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.

Parameter context:
The context containing the state of this model.
Parameter known_vdot:
A vector with the generalized accelerations for the full model.
Parameter 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.
Raises:RuntimeError if A_WB_array is not of size num_bodies().
EvalBodyPoseInWorld(self: pydrake.multibody.plant.MultibodyPlant_[float], context: pydrake.systems.framework.Context_[float], body: pydrake.multibody.tree.Body_[float]) → pydrake.math.RigidTransform_[float]

Evaluate the pose X_WB of a body B in the world frame W.

Parameter context:
The context storing the state of the model.
Parameter body_B:
The body B for which the pose is requested.
Returns X_WB:
The pose of body frame B in the world frame W.
Raises:
  • RuntimeError if Finalize() was not called on this model or if
  • body_B does not belong to this model.
EvalBodySpatialVelocityInWorld(self: pydrake.multibody.plant.MultibodyPlant_[float], context: pydrake.systems.framework.Context_[float], body: pydrake.multibody.tree.Body_[float]) → pydrake.multibody.math.SpatialVelocity_[float]

Evaluate the spatial velocity V_WB of a body B in the world frame W.

Parameter context:
The context storing the state of the model.
Parameter body_B:
The body B for which the spatial velocity is requested.
Returns:

V_WB The spatial velocity of body frame B in the world frame W.

Raises:
  • RuntimeError if Finalize() was not called on this model or if
  • body_B does not belong to this model.
Finalize(*args, **kwargs)

Overloaded function.

  1. Finalize(self: pydrake.multibody.plant.MultibodyPlant_[float]) -> None

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 pre-processing 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 up-to-date 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.

See also

is_finalized().

Raises:RuntimeError if the MultibodyPlant has already been finalized.
  1. Finalize(self: pydrake.multibody.plant.MultibodyPlant_[float], scene_graph: pydrake.geometry.SceneGraph_[float]) -> None

(Deprecated.)

Deprecated:
Remove the scene_graph argument at the call site; instead of passing a scene_graph pointer, call RegisterAsSourceForSceneGraph first, instead. This will be removed from Drake on or after 2019-10-01.
GetAccelerationLowerLimits(self: pydrake.multibody.plant.MultibodyPlant_[float]) → numpy.ndarray[float64[m, 1]]

Returns a vector of size num_velocities() containing the lower acceleration limits for every generalized velocity coordinate. These include joint and floating base coordinates. Any unbounded or unspecified limits will be -infinity.

Raises:RuntimeError if called pre-finalize.
GetAccelerationUpperLimits(self: pydrake.multibody.plant.MultibodyPlant_[float]) → numpy.ndarray[float64[m, 1]]

Upper limit analog of GetAccelerationsLowerLimits(), where any unbounded or unspecified limits will be +infinity.

See also

GetAccelerationLowerLimits() for more information.

GetBodyByName(*args, **kwargs)

Overloaded function.

  1. GetBodyByName(self: pydrake.multibody.plant.MultibodyPlant_[float], name: str) -> pydrake.multibody.tree.Body_[float]

Returns a constant reference to a body that is identified by the string name in this MultibodyPlant.

Raises:RuntimeError if there is no body with the requested name.
Raises:RuntimeError if the body name occurs in multiple model instances.

See also

HasBodyNamed() to query if there exists a body in this MultibodyPlant with a given specified name.

  1. GetBodyByName(self: pydrake.multibody.plant.MultibodyPlant_[float], name: str, model_instance: pydrake.multibody.tree.ModelInstanceIndex) -> pydrake.multibody.tree.Body_[float]

Returns a constant reference to the body that is uniquely identified by the string name and model_instance in this MultibodyPlant.

Raises:RuntimeError if there is no body with the requested name.

See also

HasBodyNamed() to query if there exists a body in this MultibodyPlant with a given specified name.

GetBodyFrameIdIfExists(self: pydrake.multibody.plant.MultibodyPlant_[float], body_index: pydrake.multibody.tree.BodyIndex) → Optional[pydrake.geometry.FrameId]

If the body with body_index has geometry registered with it, it returns the geometry::FrameId associated with it. Otherwise, it returns nullopt.

GetBodyFromFrameId(self: pydrake.multibody.plant.MultibodyPlant_[float], arg0: pydrake.geometry.FrameId) → pydrake.multibody.tree.Body_[float]

Given a geometry frame identifier, returns a pointer to the body associated with that id (nullptr if there is no such body).

GetBodyIndices(self: pydrake.multibody.plant.MultibodyPlant_[float], model_instance: pydrake.multibody.tree.ModelInstanceIndex) → List[pydrake.multibody.tree.BodyIndex]

Returns a list of body indices associated with model_instance.

GetFrameByName(*args, **kwargs)

Overloaded function.

  1. GetFrameByName(self: pydrake.multibody.plant.MultibodyPlant_[float], name: str) -> pydrake.multibody.tree.Frame_[float]

Returns a constant reference to a frame that is identified by the string name in this model.

Raises:RuntimeError if there is no frame with the requested name.
Raises:RuntimeError if the frame name occurs in multiple model instances.

See also

HasFrameNamed() to query if there exists a frame in this model with a given specified name.

  1. GetFrameByName(self: pydrake.multibody.plant.MultibodyPlant_[float], name: str, model_instance: pydrake.multibody.tree.ModelInstanceIndex) -> pydrake.multibody.tree.Frame_[float]

Returns a constant reference to the frame that is uniquely identified by the string name in model_instance.

Raises:RuntimeError if there is no frame with the requested name.
Raises:RuntimeError if model_instance is not valid for this model.

See also

HasFrameNamed() to query if there exists a frame in this model with a given specified name.

GetJointActuatorByName(self: pydrake.multibody.plant.MultibodyPlant_[float], name: str) → pydrake.multibody.tree.JointActuator_[float]

Returns a constant reference to an actuator that is identified by the string name in this MultibodyPlant.

Raises:

RuntimeError if there is no actuator with the requested name.

Raises:
  • RuntimeError if the actuator name occurs in multiple model
  • instances.

See also

HasJointActuatorNamed() to query if there exists an actuator in this MultibodyPlant with a given specified name.

GetJointByName(self: pydrake.multibody.plant.MultibodyPlant_[float], name: str, model_instance: Optional[pydrake.multibody.tree.ModelInstanceIndex] = None) → pydrake.multibody.tree.Joint_[float]

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.

Template parameter JointType:
The specific type of the Joint to be retrieved. It must be a subclass of Joint.
Raises:
  • RuntimeError if the named joint is not of type JointType or if
  • there is no Joint with that name.
Raises:

RuntimeError if model_instance is not valid for this model.

See also

HasJointNamed() to query if there exists a joint in this MultibodyPlant with a given specified name.

GetModelInstanceByName(self: pydrake.multibody.plant.MultibodyPlant_[float], name: str) → pydrake.multibody.tree.ModelInstanceIndex

Returns the index to the model instance that is uniquely identified by the string name in this MultibodyPlant.

Raises:RuntimeError if there is no instance with the requested name.

See also

HasModelInstanceNamed() to query if there exists an instance in this MultibodyPlant with a given specified name.

GetModelInstanceName(self: pydrake.multibody.plant.MultibodyPlant_[float], model_instance: pydrake.multibody.tree.ModelInstanceIndex) → str

Returns the name of a model_instance.

Raises:
  • RuntimeError when model_instance does not correspond to a
  • model in this model.
GetMutableJointByName(self: pydrake.multibody.plant.MultibodyPlant_[float], name: str, model_instance: Optional[pydrake.multibody.tree.ModelInstanceIndex] = None) → pydrake.multibody.tree.Joint_[float]

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.

Template parameter JointType:
The specific type of the Joint to be retrieved. It must be a subclass of Joint.
Raises:
  • RuntimeError if the named joint is not of type JointType or if
  • there is no Joint with that name.
Raises:

RuntimeError if model_instance is not valid for this model.

See also

HasJointNamed() to query if there exists a joint in this MultibodyPlant with a given specified name.

GetMutablePositions(self: pydrake.multibody.plant.MultibodyPlant_[float], context: pydrake.systems.framework.Context_[float]) → numpy.ndarray[float64[m, 1], flags.writeable]

(Advanced) Returns a mutable vector reference containing the vector of generalized positions (see warning).

Note

This method returns a reference to existing data, exhibits constant i.e., O(1) time complexity, and runs very quickly.

Warning

You should use SetPositions() instead of this method unless you are fully aware of the possible interactions with the caching mechanism (see dangerous_get_mutable).

Raises:
  • RuntimeError if the context is nullptr or if it does not
  • correspond to the context for a multibody model.
GetMutablePositionsAndVelocities(self: pydrake.multibody.plant.MultibodyPlant_[float], context: pydrake.systems.framework.Context_[float]) → numpy.ndarray[float64[m, 1], flags.writeable]

(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).

Warning

You should use SetPositionsAndVelocities() instead of this method unless you are fully aware of the interactions with the caching mechanism (see dangerous_get_mutable).

Raises:
  • RuntimeError if the context is nullptr or if it does not
  • correspond to the context for a multibody model.
GetMutableVelocities(self: pydrake.multibody.plant.MultibodyPlant_[float], context: pydrake.systems.framework.Context_[float]) → numpy.ndarray[float64[m, 1], flags.writeable]

See GetMutableVelocities() method above.

GetPositionLowerLimits(self: pydrake.multibody.plant.MultibodyPlant_[float]) → numpy.ndarray[float64[m, 1]]

Returns a vector of size num_positions() containing the lower position limits for every generalized position coordinate. These include joint and floating base coordinates. Any unbounded or unspecified limits will be -infinity.

Raises:RuntimeError if called pre-finalize.
GetPositionUpperLimits(self: pydrake.multibody.plant.MultibodyPlant_[float]) → numpy.ndarray[float64[m, 1]]

Upper limit analog of GetPositionsLowerLimits(), where any unbounded or unspecified limits will be +infinity.

See also

GetPositionLowerLimits() for more information.

GetPositions(*args, **kwargs)

Overloaded function.

  1. GetPositions(self: pydrake.multibody.plant.MultibodyPlant_[float], context: pydrake.systems.framework.Context_[float]) -> numpy.ndarray[float64[m, 1]]

Returns a const vector reference containing the vector of generalized positions.

Note

This method returns a reference to existing data, exhibits constant i.e., O(1) time complexity, and runs very quickly.

Raises:
  • RuntimeError if the context does not correspond to the context
  • for a multibody model.
  1. GetPositions(self: pydrake.multibody.plant.MultibodyPlant_[float], context: pydrake.systems.framework.Context_[float], model_instance: pydrake.multibody.tree.ModelInstanceIndex) -> numpy.ndarray[float64[m, 1]]

Returns an vector containing the generalized positions (q) for the given model instance.

Raises:
  • RuntimeError if the context does not correspond to the context
  • for a multibody model.

Note

returns a dense vector of dimension q.size() associated with model_instance in O(q.size()) time.

  1. GetPositions(self: pydrake.multibody.plant.MultibodyPlant_[float], context: pydrake.systems.framework.Context_[float]) -> numpy.ndarray[float64[m, 1]]

Returns a const vector reference containing the vector of generalized positions.

Note

This method returns a reference to existing data, exhibits constant i.e., O(1) time complexity, and runs very quickly.

Raises:
  • RuntimeError if the context does not correspond to the context
  • for a multibody model.
  1. GetPositions(self: pydrake.multibody.plant.MultibodyPlant_[float], context: pydrake.systems.framework.Context_[float], model_instance: pydrake.multibody.tree.ModelInstanceIndex) -> numpy.ndarray[float64[m, 1]]

Returns an vector containing the generalized positions (q) for the given model instance.

Raises:
  • RuntimeError if the context does not correspond to the context
  • for a multibody model.

Note

returns a dense vector of dimension q.size() associated with model_instance in O(q.size()) time.

GetPositionsAndVelocities(*args, **kwargs)

Overloaded function.

  1. GetPositionsAndVelocities(self: pydrake.multibody.plant.MultibodyPlant_[float], context: pydrake.systems.framework.Context_[float]) -> numpy.ndarray[float64[m, 1]]

Returns a const vector reference containing the vector [q; v] with q the vector of generalized positions and v the vector of generalized velocities.

Note

This method returns a reference to existing data, exhibits constant i.e., O(1) time complexity, and runs very quickly.

Raises:
  • RuntimeError if the context does not correspond to the context
  • for a multibody model.
  1. GetPositionsAndVelocities(self: pydrake.multibody.plant.MultibodyPlant_[float], context: pydrake.systems.framework.Context_[float], model_instance: pydrake.multibody.tree.ModelInstanceIndex) -> numpy.ndarray[float64[m, 1]]

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.

Raises:
  • RuntimeError if the context does not correspond to the context
  • for a multibody model or model_instance is invalid.

Note

returns a dense vector of dimension q.size() + v.size() associated with model_instance in O(q.size()) time.

GetPositionsFromArray(self: pydrake.multibody.plant.MultibodyPlant_[float], model_instance: pydrake.multibody.tree.ModelInstanceIndex, q: numpy.ndarray[float64[m, 1]]) → numpy.ndarray[float64[m, 1]]

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().

GetVelocities(*args, **kwargs)

Overloaded function.

  1. GetVelocities(self: pydrake.multibody.plant.MultibodyPlant_[float], context: pydrake.systems.framework.Context_[float]) -> numpy.ndarray[float64[m, 1]]

Returns a const vector reference containing the generalized velocities.

Note

This method returns a reference to existing data, exhibits constant i.e., O(1) time complexity, and runs very quickly.

  1. GetVelocities(self: pydrake.multibody.plant.MultibodyPlant_[float], context: pydrake.systems.framework.Context_[float], model_instance: pydrake.multibody.tree.ModelInstanceIndex) -> numpy.ndarray[float64[m, 1]]

Returns a vector containing the generalized velocities (v) for the given model instance.

Raises:
  • RuntimeError if the context does not correspond to the context
  • for a multibody model.

Note

returns a dense vector of dimension v.size() associated with model_instance in O(v.size()) time.

  1. GetVelocities(self: pydrake.multibody.plant.MultibodyPlant_[float], context: pydrake.systems.framework.Context_[float]) -> numpy.ndarray[float64[m, 1]]

Returns a const vector reference containing the generalized velocities.

Note

This method returns a reference to existing data, exhibits constant i.e., O(1) time complexity, and runs very quickly.

  1. GetVelocities(self: pydrake.multibody.plant.MultibodyPlant_[float], context: pydrake.systems.framework.Context_[float], model_instance: pydrake.multibody.tree.ModelInstanceIndex) -> numpy.ndarray[float64[m, 1]]

Returns a vector containing the generalized velocities (v) for the given model instance.

Raises:
  • RuntimeError if the context does not correspond to the context
  • for a multibody model.

Note

returns a dense vector of dimension v.size() associated with model_instance in O(v.size()) time.

GetVelocitiesFromArray(self: pydrake.multibody.plant.MultibodyPlant_[float], model_instance: pydrake.multibody.tree.ModelInstanceIndex, v: numpy.ndarray[float64[m, 1]]) → numpy.ndarray[float64[m, 1]]

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().

GetVelocityLowerLimits(self: pydrake.multibody.plant.MultibodyPlant_[float]) → numpy.ndarray[float64[m, 1]]

Returns a vector of size num_velocities() containing the lower velocity limits for every generalized velocity coordinate. These include joint and floating base coordinates. Any unbounded or unspecified limits will be -infinity.

Raises:RuntimeError if called pre-finalize.
GetVelocityUpperLimits(self: pydrake.multibody.plant.MultibodyPlant_[float]) → numpy.ndarray[float64[m, 1]]

Upper limit analog of GetVelocitysLowerLimits(), where any unbounded or unspecified limits will be +infinity.

See also

GetVelocityLowerLimits() for more information.

HasBodyNamed(*args, **kwargs)

Overloaded function.

  1. HasBodyNamed(self: pydrake.multibody.plant.MultibodyPlant_[float], name: str) -> bool
Returns:True if a body named name was added to the MultibodyPlant.

See also

AddRigidBody().

Raises:RuntimeError if the body name occurs in multiple model instances.
  1. HasBodyNamed(self: pydrake.multibody.plant.MultibodyPlant_[float], name: str, model_instance: pydrake.multibody.tree.ModelInstanceIndex) -> bool
Returns:True if a body named name was added to the MultibodyPlant in model_instance.

See also

AddRigidBody().

Raises:RuntimeError if model_instance is not valid for this model.
HasJointNamed(*args, **kwargs)

Overloaded function.

  1. HasJointNamed(self: pydrake.multibody.plant.MultibodyPlant_[float], name: str) -> bool
Returns:True if a joint named name was added to this model.

See also

AddJoint().

Raises:RuntimeError if the joint name occurs in multiple model instances.
  1. HasJointNamed(self: pydrake.multibody.plant.MultibodyPlant_[float], name: str, model_instance: pydrake.multibody.tree.ModelInstanceIndex) -> bool
Returns:True if a joint named name was added to model_instance.

See also

AddJoint().

Raises:RuntimeError if model_instance is not valid for this model.
MakeActuationMatrix(self: pydrake.multibody.plant.MultibodyPlant_[float]) → numpy.ndarray[float64[m, n]]

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().

Warning

B is a permutation matrix. While making a permutation has 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.

MapQDotToVelocity(self: pydrake.multibody.plant.MultibodyPlant_[float], context: pydrake.systems.framework.Context_[float], qdot: numpy.ndarray[float64[m, 1]]) → numpy.ndarray[float64[m, 1]]

Transforms the time derivative qdot of the generalized positions vector q (stored in context) to generalized velocities v. v and are related linearly by = N(q)⋅v. Although N(q) is not necessarily square, its left pseudo-inverse 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 = N(q)⋅v for some v). Using the configuration q stored in the given context this method calculates v = N⁺(q)⋅q̇.

Parameter context:
The context containing the state of the model.
Parameter qdot:
A vector containing the time derivatives of the generalized positions. This method aborts if qdot is not of size num_positions().
Parameter v:
A valid (non-null) 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().

See also

MapVelocityToQDot()

See also

Mobilizer::MapQDotToVelocity()

MapVelocityToQDot(self: pydrake.multibody.plant.MultibodyPlant_[float], context: pydrake.systems.framework.Context_[float], v: numpy.ndarray[float64[m, 1]]) → numpy.ndarray[float64[m, 1]]

Transforms generalized velocities v to time derivatives qdot of the generalized positions vector q (stored in context). v and qdot are related linearly by = N(q)⋅v. Using the configuration q stored in the given context this method calculates = N(q)⋅v.

Parameter context:
The context containing the state of the model.
Parameter v:
A vector of of generalized velocities for this model. This method aborts if v is not of size num_velocities().
Parameter qdot:
A valid (non-null) 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().

See also

MapQDotToVelocity()

See also

Mobilizer::MapVelocityToQDot()

RegisterAsSourceForSceneGraph(self: pydrake.multibody.plant.MultibodyPlant_[float], scene_graph: pydrake.geometry.SceneGraph_[float]) → pydrake.geometry.SourceId

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. 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.

Parameter 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.
Returns:the SourceId of this plant in scene_graph. It can also later on be retrieved with get_source_id().
Raises:RuntimeError if called post-finalize.
Raises:RuntimeError if scene_graph is the nullptr.
Raises:RuntimeError if called more than once.
RegisterCollisionGeometry(*args, **kwargs)

Overloaded function.

  1. RegisterCollisionGeometry(self: pydrake.multibody.plant.MultibodyPlant_[float], body: pydrake.multibody.tree.Body_[float], X_BG: pydrake.math.RigidTransform_[float], shape: pydrake.geometry.Shape, name: str, coulomb_friction: pydrake.multibody.plant.CoulombFriction_[float]) -> pydrake.geometry.GeometryId

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.

Parameter body:
The body for which geometry is being registered.
Parameter X_BG:
The fixed pose of the geometry frame G in the body frame B.
Parameter shape:
The geometry::Shape used for visualization. E.g.: geometry::Sphere, geometry::Cylinder, etc.
Parameter coulomb_friction:
Coulomb’s law of friction coefficients to model friction on the surface of shape for the given body.
Raises:RuntimeError if called post-finalize.
  1. RegisterCollisionGeometry(self: pydrake.multibody.plant.MultibodyPlant_[float], body: pydrake.multibody.tree.Body_[float], X_BG: pydrake.math.RigidTransform_[float], shape: pydrake.geometry.Shape, name: str, coulomb_friction: pydrake.multibody.plant.CoulombFriction_[float], scene_graph: pydrake.geometry.SceneGraph_[float]) -> None

(Deprecated.)

Deprecated:
Remove the scene_graph argument at the call site; instead of passing a scene_graph pointer, call RegisterAsSourceForSceneGraph first, instead. This will be removed from Drake on or after 2019-10-01.
RegisterVisualGeometry(*args, **kwargs)

Overloaded function.

  1. RegisterVisualGeometry(self: pydrake.multibody.plant.MultibodyPlant_[float], body: pydrake.multibody.tree.Body_[float], X_BG: pydrake.math.RigidTransform_[float], shape: pydrake.geometry.Shape, name: str, diffuse_color: numpy.ndarray[float64[4, 1]]) -> pydrake.geometry.GeometryId

Overload for visual geometry registration; it converts the diffuse_color (RGBA with values in the range [0, 1]) into a geometry::ConnectDrakeVisualizer()-compatible set of geometry::IllustrationProperties.

  1. RegisterVisualGeometry(self: pydrake.multibody.plant.MultibodyPlant_[float], body: pydrake.multibody.tree.Body_[float], X_BG: pydrake.math.RigidTransform_[float], shape: pydrake.geometry.Shape, name: str, diffuse_color: numpy.ndarray[float64[4, 1]], scene_graph: pydrake.geometry.SceneGraph_[float]) -> None

(Deprecated.) Overload for visual geometry registration; it converts the diffuse_color (RGBA with values in the range [0, 1]) into a geometry::ConnectDrakeVisualizer()-compatible set of geometry::IllustrationProperties.

  1. RegisterVisualGeometry(self: pydrake.multibody.plant.MultibodyPlant_[float], body: pydrake.multibody.tree.Body_[float], X_BG: pydrake.common.eigen_geometry.Isometry3_[float], shape: pydrake.geometry.Shape, name: str, diffuse_color: numpy.ndarray[float64[4, 1]], scene_graph: pydrake.geometry.SceneGraph_[float] = None) -> pydrake.geometry.GeometryId

This API using Isometry3 is / will be deprecated soon with the resolution of #9865. We only offer it for backwards compatibility. DO NOT USE!.

SetActuationInArray(self: pydrake.multibody.plant.MultibodyPlant_[float], model_instance: pydrake.multibody.tree.ModelInstanceIndex, u_instance: numpy.ndarray[float64[m, 1]], u: numpy.ndarray[float64[m, 1], flags.writeable]) → None

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.

Parameter 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.
Parameter u:
The vector containing the actuation values for the entire model.
SetDefaultState(self: pydrake.multibody.plant.MultibodyPlant_[float], context: pydrake.systems.framework.Context_[float], state: pydrake.systems.framework.State_[float]) → None

Sets the state so that generalized positions and velocities are zero.

Raises:RuntimeError if called pre-finalize. See Finalize().
SetFreeBodyPose(*args, **kwargs)

Overloaded function.

  1. SetFreeBodyPose(self: pydrake.multibody.plant.MultibodyPlant_[float], context: pydrake.systems.framework.Context_[float], body: pydrake.multibody.tree.Body_[float], X_WB: pydrake.math.RigidTransform_[float]) -> None

Sets context to store the pose X_WB of a given body B in the world frame W.

Note

In general setting the pose and/or velocity of a body in the model would involve a complex inverse kinematics problem. This method allows us to simplify this process when we know the body is free in space.

Raises:RuntimeError if body is not a free body in the model.
Raises:RuntimeError if called pre-finalize.
  1. SetFreeBodyPose(self: pydrake.multibody.plant.MultibodyPlant_[float], context: pydrake.systems.framework.Context_[float], body: pydrake.multibody.tree.Body_[float], X_WB: pydrake.common.eigen_geometry.Isometry3_[float]) -> None

This API using Isometry3 is / will be deprecated soon with the resolution of #9865. We only offer it for backwards compatibility. DO NOT USE!.

SetFreeBodySpatialVelocity(self: pydrake.multibody.plant.MultibodyPlant_[float], body: pydrake.multibody.tree.Body_[float], V_WB: pydrake.multibody.math.SpatialVelocity_[float], context: pydrake.systems.framework.Context_[float]) → None

Sets context to store the spatial velocity V_WB of a given body B in the world frame W.

Note

In general setting the pose and/or velocity of a body in the model would involve a complex inverse kinematics problem. This method allows us to simplify this process when we know the body is free in space.

Raises:RuntimeError if body is not a free body in the model.
Raises:RuntimeError if called pre-finalize.
SetPositions(*args, **kwargs)

Overloaded function.

  1. SetPositions(self: pydrake.multibody.plant.MultibodyPlant_[float], context: pydrake.systems.framework.Context_[float], q: numpy.ndarray[float64[m, 1]]) -> None

Sets all generalized positions from the given vector.

Raises:
  • RuntimeError 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().
  1. SetPositions(self: pydrake.multibody.plant.MultibodyPlant_[float], context: pydrake.systems.framework.Context_[float], model_instance: pydrake.multibody.tree.ModelInstanceIndex, q: numpy.ndarray[float64[m, 1]]) -> None

Sets all generalized positions from the given vector.

Raises:
  • RuntimeError 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().
SetPositionsAndVelocities(*args, **kwargs)

Overloaded function.

  1. SetPositionsAndVelocities(self: pydrake.multibody.plant.MultibodyPlant_[float], context: pydrake.systems.framework.Context_[float], q_v: numpy.ndarray[float64[m, 1]]) -> None

Sets all generalized positions and velocities from the given vector [q; v].

Raises:
  • RuntimeError 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()``.
  1. SetPositionsAndVelocities(self: pydrake.multibody.plant.MultibodyPlant_[float], context: pydrake.systems.framework.Context_[float], model_instance: pydrake.multibody.tree.ModelInstanceIndex, q_v: numpy.ndarray[float64[m, 1]]) -> None

Sets generalized positions and velocities from the given vector [q; v] for the specified model instance.

Raises:
  • RuntimeError 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)``.
SetPositionsInArray(self: pydrake.multibody.plant.MultibodyPlant_[float], model_instance: pydrake.multibody.tree.ModelInstanceIndex, q_instance: numpy.ndarray[float64[m, 1]], q: numpy.ndarray[float64[m, 1], flags.writeable]) → None

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).

SetVelocities(*args, **kwargs)

Overloaded function.

  1. SetVelocities(self: pydrake.multibody.plant.MultibodyPlant_[float], context: pydrake.systems.framework.Context_[float], v: numpy.ndarray[float64[m, 1]]) -> None

Sets all generalized velocities from the given vector.

Raises:
  • RuntimeError 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().
  1. SetVelocities(self: pydrake.multibody.plant.MultibodyPlant_[float], context: pydrake.systems.framework.Context_[float], model_instance: pydrake.multibody.tree.ModelInstanceIndex, v: numpy.ndarray[float64[m, 1]]) -> None

Sets the generalized velocities for a particular model instance from the given vector.

Raises:
  • RuntimeError 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).
SetVelocitiesInArray(self: pydrake.multibody.plant.MultibodyPlant_[float], model_instance: pydrake.multibody.tree.ModelInstanceIndex, v_instance: numpy.ndarray[float64[m, 1]], v: numpy.ndarray[float64[m, 1], flags.writeable]) → None

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).

WeldFrames(*args, **kwargs)

Overloaded function.

  1. WeldFrames(self: pydrake.multibody.plant.MultibodyPlant_[float], A: pydrake.multibody.tree.Frame_[float], B: pydrake.multibody.tree.Frame_[float], X_AB: pydrake.math.RigidTransform_[float] = <pydrake.math.RigidTransform_[float] object at 0x7f5d1ee00570>) -> pydrake.multibody.tree.WeldJoint_[float]

Welds frames A and B with relative pose X_AB. That is, the pose of frame B in frame A is fixed, with value X_AB. The call to this method creates and adds a new WeldJoint to the model. The new WeldJoint is named as: A.name() + “_welds_to_” + B.name().

Returns:a constant reference to the WeldJoint welding frames A and B.
  1. WeldFrames(self: pydrake.multibody.plant.MultibodyPlant_[float], A: pydrake.multibody.tree.Frame_[float], B: pydrake.multibody.tree.Frame_[float], X_AB: pydrake.common.eigen_geometry.Isometry3_[float]) -> pydrake.multibody.tree.WeldJoint_[float]

This API using Isometry3 is / will be deprecated soon with the resolution of #9865. We only offer it for backwards compatibility. DO NOT USE!.

geometry_source_is_registered(self: pydrake.multibody.plant.MultibodyPlant_[float]) → bool

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 post-finalize, see Finalize().

get_actuation_input_port(*args, **kwargs)

Overloaded function.

  1. get_actuation_input_port(self: pydrake.multibody.plant.MultibodyPlant_[float]) -> pydrake.systems.framework.InputPort_[float]

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().

Precondition:
Finalize() was already called on this plant.
Raises:
  • RuntimeError if called before Finalize(), if the model does not
  • contain any actuators, or if multiple model instances have
  • actuated dofs.
  1. get_actuation_input_port(self: pydrake.multibody.plant.MultibodyPlant_[float], arg0: pydrake.multibody.tree.ModelInstanceIndex) -> pydrake.systems.framework.InputPort_[float]

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().

Precondition:
Finalize() was already called on this plant.
Raises:RuntimeError if called before Finalize().
Raises:RuntimeError if the model instance does not exist.
get_applied_generalized_force_input_port(self: pydrake.multibody.plant.MultibodyPlant_[float]) → pydrake.systems.framework.InputPort_[float]

Returns a constant reference to the vector-valued input port for applied generalized forces, and the vector will be added directly into tau (see equations_of_motion). 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).

Raises:RuntimeError if called before Finalize().
get_applied_spatial_force_input_port(self: pydrake.multibody.plant.MultibodyPlant_[float]) → pydrake.systems.framework.InputPort_[float]

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.

get_body(self: pydrake.multibody.plant.MultibodyPlant_[float], body_index: pydrake.multibody.tree.BodyIndex) → pydrake.multibody.tree.Body_[float]

Returns a constant reference to the body with unique index body_index.

Raises:
  • RuntimeError if body_index does not correspond to a body in
  • this model.
get_contact_results_output_port(self: pydrake.multibody.plant.MultibodyPlant_[float]) → pydrake.systems.framework.OutputPort_[float]

Returns a constant reference to the port that outputs ContactResults.

Raises:
  • RuntimeError if this plant is not modeled as a discrete system
  • with periodic updates.
Raises:

RuntimeError if called pre-finalize, see Finalize().

get_frame(self: pydrake.multibody.plant.MultibodyPlant_[float], frame_index: pydrake.multibody.tree.FrameIndex) → pydrake.multibody.tree.Frame_[float]

Returns a constant reference to the frame with unique index frame_index.

Raises:
  • RuntimeError if frame_index does not correspond to a frame in
  • this plant.
get_generalized_contact_forces_output_port(self: pydrake.multibody.plant.MultibodyPlant_[float], model_instance: pydrake.multibody.tree.ModelInstanceIndex) → pydrake.systems.framework.OutputPort_[float]

Returns a constant reference to the output port of generalized contact forces for a specific model instance. This output port is only available when modeling the plant as a discrete system with periodic updates, see is_discrete().

Precondition:
Finalize() was already called on this plant.
Raises:
  • RuntimeError if this plant is not modeled as a discrete system
  • with periodic updates.
Raises:
  • RuntimeError if called before Finalize() or if the model instance
  • does not have any generalized velocities.
Raises:

RuntimeError if the model instance does not exist.

get_geometry_poses_output_port(self: pydrake.multibody.plant.MultibodyPlant_[float]) → pydrake.systems.framework.OutputPort_[float]

Returns the output port of frames’ poses to communicate with a SceneGraph.

Raises:RuntimeError if this system was not registered with a SceneGraph.
get_geometry_query_input_port(self: pydrake.multibody.plant.MultibodyPlant_[float]) → pydrake.systems.framework.InputPort_[float]

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_registration of this class’s documentation for further details on collision geometry registration and connection with a SceneGraph.

Raises:RuntimeError if this system was not registered with a SceneGraph.
get_joint(self: pydrake.multibody.plant.MultibodyPlant_[float], joint_index: pydrake.multibody.tree.JointIndex) → pydrake.multibody.tree.Joint_[float]

Returns a constant reference to the joint with unique index joint_index.

Raises:
  • RuntimeError when joint_index does not correspond to a joint
  • in this model.
get_joint_actuator(self: pydrake.multibody.plant.MultibodyPlant_[float], actuator_index: pydrake.multibody.tree.JointActuatorIndex) → pydrake.multibody.tree.JointActuator_[float]

Returns a constant reference to the joint actuator with unique index actuator_index.

Raises:
  • RuntimeError if actuator_index does not correspond to a joint
  • actuator in this tree.
get_source_id(self: pydrake.multibody.plant.MultibodyPlant_[float]) → Optional[pydrake.geometry.SourceId]

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 post-finalize, 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. Post-finalize calls will always return the same value.

get_state_output_port(*args, **kwargs)

Overloaded function.

  1. get_state_output_port(self: pydrake.multibody.plant.MultibodyPlant_[float]) -> pydrake.systems.framework.OutputPort_[float]

Returns a constant reference to the output port for the full state x = [q v] of the model.

Precondition:
Finalize() was already called on this plant.
  1. get_state_output_port(self: pydrake.multibody.plant.MultibodyPlant_[float], arg0: pydrake.multibody.tree.ModelInstanceIndex) -> pydrake.systems.framework.OutputPort_[float]

Returns a constant reference to the output port for the state of a specific model instance.

Precondition:
Finalize() was already called on this plant.
Raises:
  • RuntimeError if called before Finalize() or if the model instance
  • does not have any state.
Raises:

RuntimeError if the model instance does not exist.

gravity_field(self: pydrake.multibody.plant.MultibodyPlant_[float]) → pydrake.multibody.tree.UniformGravityFieldElement_[float]

An accessor to the current gravity field.

is_finalized(self: pydrake.multibody.plant.MultibodyPlant_[float]) → bool

Returns True if this MultibodyPlant was finalized with a call to Finalize().

See also

Finalize().

mutable_gravity_field(self: pydrake.multibody.plant.MultibodyPlant_[float]) → pydrake.multibody.tree.UniformGravityFieldElement_[float]

A mutable accessor to the current gravity field.

num_actuated_dofs(self: pydrake.multibody.plant.MultibodyPlant_[float]) → int

Returns the total number of actuated degrees of freedom. That is, the vector of actuation values u has this size. See AddJointActuator().

num_actuators(self: pydrake.multibody.plant.MultibodyPlant_[float]) → int

Returns the number of joint actuators in the model.

See also

AddJointActuator().

num_bodies(self: pydrake.multibody.plant.MultibodyPlant_[float]) → int

Returns the number of bodies in the model, including the “world” body, which is always part of the model.

See also

AddRigidBody().

num_frames(self: pydrake.multibody.plant.MultibodyPlant_[float]) → int

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.

num_joints(self: pydrake.multibody.plant.MultibodyPlant_[float]) → int

Returns the number of joints in the model.

See also

AddJoint().

num_model_instances(self: pydrake.multibody.plant.MultibodyPlant_[float]) → int

Returns the number of model instances in the model.

See also

AddModelInstance().

num_multibody_states(self: pydrake.multibody.plant.MultibodyPlant_[float]) → int

Returns the size of the multibody system state vector x = [q; v]. This will be num_positions() plus num_velocities().

num_positions(*args, **kwargs)

Overloaded function.

  1. num_positions(self: pydrake.multibody.plant.MultibodyPlant_[float]) -> int

Returns the size of the generalized position vector q for this model.

  1. num_positions(self: pydrake.multibody.plant.MultibodyPlant_[float], model_instance: pydrake.multibody.tree.ModelInstanceIndex) -> int

Returns the size of the generalized position vector q for a specific model instance.

num_velocities(*args, **kwargs)

Overloaded function.

  1. num_velocities(self: pydrake.multibody.plant.MultibodyPlant_[float]) -> int

Returns the size of the generalized velocity vector v for this model.

  1. num_velocities(self: pydrake.multibody.plant.MultibodyPlant_[float], arg0: pydrake.multibody.tree.ModelInstanceIndex) -> int

Returns the size of the generalized velocity vector v for a specific model instance.

world_body(self: pydrake.multibody.plant.MultibodyPlant_[float]) → pydrake.multibody.tree.RigidBody_[float]

Returns a constant reference to the world body.

world_frame(self: pydrake.multibody.plant.MultibodyPlant_[float]) → pydrake.multibody.tree.BodyFrame_[float]

Returns a constant reference to the world frame.

pydrake.multibody.plant.PointPairContactInfo

alias of pydrake.multibody.plant.PointPairContactInfo_[float]

template pydrake.multibody.plant.PointPairContactInfo_

Instantiations: PointPairContactInfo_[float], PointPairContactInfo_[AutoDiffXd], PointPairContactInfo_[Expression]

class PointPairContactInfo_[float]

A class containing information regarding contact response between two bodies including:

  • The pair of bodies that are contacting, referenced by their BodyIndex.
  • A resultant contact force.
  • A contact point.
  • Separation speed.
  • Slip speed.
Template parameter T:
Must be one of drake’s default scalar types.
bodyA_index(self: pydrake.multibody.plant.PointPairContactInfo_[float]) → pydrake.multibody.tree.BodyIndex

Returns the index of body A in the contact pair.

bodyB_index(self: pydrake.multibody.plant.PointPairContactInfo_[float]) → pydrake.multibody.tree.BodyIndex

Returns the index of body B in the contact pair.

contact_force(self: pydrake.multibody.plant.PointPairContactInfo_[float]) → numpy.ndarray[float64[3, 1]]

Returns the contact force f_Bc_W on B at contact point C expressed in the world frame W.

contact_point(self: pydrake.multibody.plant.PointPairContactInfo_[float]) → numpy.ndarray[float64[3, 1]]

Returns the position p_WC of the contact point C in the world frame W.

point_pair(self: pydrake.multibody.plant.PointPairContactInfo_[float]) → pydrake.geometry.PenetrationAsPointPair_[float]

Returns additional information for the geometric contact query for this pair as a PenetrationAsPointPair.

separation_speed(self: pydrake.multibody.plant.PointPairContactInfo_[float]) → float

Returns the separation speed between body A and B along the normal direction (see PenetrationAsPointPair::nhat_BA_W) at the contact point. It is defined positive for bodies moving apart in the normal direction.

slip_speed(self: pydrake.multibody.plant.PointPairContactInfo_[float]) → float

Returns the slip speed between body A and B at contact point C.

pydrake.multibody.plant.VectorExternallyAppliedSpatialForced

alias of pydrake.multibody.plant.VectorExternallyAppliedSpatialForced_[float]

template pydrake.multibody.plant.VectorExternallyAppliedSpatialForced_

Instantiations: VectorExternallyAppliedSpatialForced_[float], VectorExternallyAppliedSpatialForced_[AutoDiffXd], VectorExternallyAppliedSpatialForced_[Expression]

class VectorExternallyAppliedSpatialForced_[float]
append(self: pydrake.multibody.plant._TemporaryName_St6vectorIN5drake9multibody29ExternallyAppliedSpatialForceIdEESaIS3_EE, x: pydrake.multibody.plant.ExternallyAppliedSpatialForce_[float]) → None

Add an item to the end of the list

extend(self: pydrake.multibody.plant._TemporaryName_St6vectorIN5drake9multibody29ExternallyAppliedSpatialForceIdEESaIS3_EE, L: pydrake.multibody.plant._TemporaryName_St6vectorIN5drake9multibody29ExternallyAppliedSpatialForceIdEESaIS3_EE) → None

Extend the list by appending all the items in the given list

insert(self: pydrake.multibody.plant._TemporaryName_St6vectorIN5drake9multibody29ExternallyAppliedSpatialForceIdEESaIS3_EE, i: int, x: pydrake.multibody.plant.ExternallyAppliedSpatialForce_[float]) → None

Insert an item at a given position.

pop(*args, **kwargs)

Overloaded function.

  1. pop(self: pydrake.multibody.plant._TemporaryName_St6vectorIN5drake9multibody29ExternallyAppliedSpatialForceIdEESaIS3_EE) -> pydrake.multibody.plant.ExternallyAppliedSpatialForce_[float]

Remove and return the last item

  1. pop(self: pydrake.multibody.plant._TemporaryName_St6vectorIN5drake9multibody29ExternallyAppliedSpatialForceIdEESaIS3_EE, i: int) -> pydrake.multibody.plant.ExternallyAppliedSpatialForce_[float]

Remove and return the item at index i