- Generated by
|
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. More...
#include <drake/multibody/plant/multibody_plant.h>
Public Member Functions | |
| MultibodyPlant (double time_step=0) | |
| Default constructor creates a plant with a single "world" body. More... | |
| template<typename U > | |
| MultibodyPlant (const MultibodyPlant< U > &other) | |
| Scalar-converting copy constructor. See System Scalar Conversion. More... | |
| int | num_frames () const |
| Returns the number of Frame objects in this model. 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... | |
| int | num_joints () const |
| Returns the number of joints in the model. More... | |
| int | num_actuators () const |
| Returns the number of joint actuators in the model. More... | |
| int | num_force_elements () const |
| Returns the number of ForceElement objects. More... | |
| int | num_model_instances () const |
| Returns the number of model instances in the model. 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 a specific model instance. 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 a specific model instance. More... | |
| int | num_multibody_states () const |
Returns the size of the multibody system state vector x = [q; v]. 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... | |
| 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... | |
| 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 Isometry3< double > &X_BG, const geometry::Shape &shape, const std::string &name, const geometry::IllustrationProperties &properties, geometry::SceneGraph< T > *scene_graph=nullptr) |
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 Isometry3< double > &X_BG, const geometry::Shape &shape, const std::string &name, const Vector4< double > &diffuse_color, geometry::SceneGraph< T > *scene_graph=nullptr) |
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. More... | |
| geometry::GeometryId | RegisterVisualGeometry (const Body< T > &body, const Isometry3< double > &X_BG, const geometry::Shape &shape, const std::string &name, geometry::SceneGraph< T > *scene_graph=nullptr) |
| Overload for visual geometry registration; it relies on the downstream geometry::IllustrationProperties consumer to provide default parameter values (see Geometry 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... | |
| int | num_visual_geometries () const |
| Returns the number of geometries registered for visualization. More... | |
| geometry::GeometryId | RegisterCollisionGeometry (const Body< T > &body, const Isometry3< double > &X_BG, const geometry::Shape &shape, const std::string &name, const CoulombFriction< double > &coulomb_friction, geometry::SceneGraph< T > *scene_graph=nullptr) |
Registers geometry in a SceneGraph with a given geometry::Shape to be used for the contact modeling of a given body. 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... | |
| int | num_collision_geometries () const |
| Returns the number of geometries registered for contact modeling. 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... | |
| 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... | |
| const CoulombFriction< double > & | default_coulomb_friction (geometry::GeometryId id) const |
Returns the friction coefficients provided during geometry registration for the given geometry id. More... | |
| bool | geometry_source_is_registered () const |
Returns true if this MultibodyPlant was registered with a SceneGraph. 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... | |
| optional< geometry::FrameId > | GetBodyFrameIdIfExists (BodyIndex body_index) const |
If the body with body_index has geometry registered with it, it returns the geometry::FrameId associated with it. More... | |
| geometry::FrameId | GetBodyFrameIdOrThrow (BodyIndex body_index) const |
If the body with body_index has geometry registered with it, it returns the geometry::FrameId associated with it. 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_contact_results_output_port () const |
| Returns a constant reference to the port that outputs ContactResults. 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... | |
| const Body< T > & | get_body (BodyIndex body_index) const |
Returns a constant reference to the body with unique index body_index. More... | |
| const Joint< T > & | get_joint (JointIndex joint_index) const |
Returns a constant reference to the joint with unique index joint_index. More... | |
| Joint< T > & | get_mutable_joint (JointIndex joint_index) |
Returns a mutable reference to the joint with unique index joint_index. More... | |
| const JointActuator< T > & | get_joint_actuator (JointActuatorIndex actuator_index) const |
Returns a constant reference to the joint actuator with unique index actuator_index. More... | |
| const Frame< T > & | get_frame (FrameIndex frame_index) const |
Returns a constant reference to the frame with unique index frame_index. More... | |
| const std::string & | GetModelInstanceName (ModelInstanceIndex model_instance) const |
Returns the name of a model_instance. More... | |
| bool | is_finalized () const |
Returns true if this MultibodyPlant was finalized with a call to Finalize(). More... | |
| bool | IsAnchored (const Body< T > &body) const |
Returns true if body is anchored (i.e. More... | |
| void | Finalize (geometry::SceneGraph< T > *scene_graph=nullptr) |
| 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... | |
| double | time_step () const |
The time step (or period) used to model this plant as a discrete system with periodic updates. More... | |
| void | SetDefaultState (const systems::Context< T > &context, systems::State< T > *state) const override |
Sets the state so that generalized positions and velocities are zero. 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/floating-base (coming soon: and then solving a mathematical program to "project" these samples onto the registered system constraints). More... | |
| const internal::MultibodyTree< T > & | tree () const |
| Deprecated. More... | |
Does not allow copy, move, or assignment | |
| MultibodyPlant (const MultibodyPlant &)=delete | |
| MultibodyPlant & | operator= (const MultibodyPlant &)=delete |
| MultibodyPlant (MultibodyPlant &&)=delete | |
| MultibodyPlant & | operator= (MultibodyPlant &&)=delete |
Position and velocity state component accessors and mutators. | |
Various methods for accessing and mutating | |
| 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... | |
| 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 Isometry3< 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 Isometry3< 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 | 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 x-y-z position component of the floating-base state. More... | |
| void | SetFreeBodyRandomRotationDistributionToUniform (const Body< T > &body) |
| Sets the distribution used by SetRandomState() to populate the rotation component of the floating-base state using uniformly random rotations. 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... | |
Adding new multibody elements | |
MultibodyPlant users will add modeling elements like bodies, joints, force elements, constraints, etc, using one of these methods. Once a user is done adding all modeling elements, the Finalize() method must be called before invoking any MultibodyPlant service to perform computations. An attempt to call any of these methods after a call to Finalize() on the plant, will result on an exception being thrown. See Finalize() for details. | |
| 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 optional< Isometry3< double >> &X_PF, const Body< T > &child, const optional< Isometry3< double >> &X_BM, Args &&... args) |
This method adds a Joint of type JointType between two bodies. 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) |
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... | |
| const WeldJoint< T > & | WeldFrames (const Frame< T > &A, const Frame< T > &B, const Isometry3< double > &X_AB=Isometry3< double >::Identity()) |
Welds frames A and B with relative pose X_AB. More... | |
Querying for multibody elements by name | |
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(). That is, these queries can be performed while new multibody elements are being added to the model. | |
| bool | HasBodyNamed (const std::string &name) const |
| bool | HasBodyNamed (const std::string &name, ModelInstanceIndex model_instance) const |
| bool | HasFrameNamed (const std::string &name) const |
| bool | HasFrameNamed (const std::string &name, ModelInstanceIndex model_instance) const |
| bool | HasJointNamed (const std::string &name) const |
| bool | HasJointNamed (const std::string &name, ModelInstanceIndex model_instance) const |
| bool | HasJointActuatorNamed (const std::string &name) const |
| bool | HasJointActuatorNamed (const std::string &name, ModelInstanceIndex model_instance) const |
| bool | HasModelInstanceNamed (const std::string &name) const |
Retrieving multibody elements by name | |
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. | |
| const Body< T > & | GetBodyByName (const std::string &name) const |
| These queries can be performed at any time during the lifetime of a MultibodyPlant, i.e. More... | |
| const Body< T > & | GetBodyByName (const std::string &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 Frame< T > & | GetFrameByName (const std::string &name) const |
Returns a constant reference to a frame that is identified by the string name in this model. More... | |
| const Frame< T > & | GetFrameByName (const std::string &name, ModelInstanceIndex model_instance) const |
Returns a constant reference to the frame that is uniquely identified by the string name in model_instance. More... | |
| const RigidBody< T > & | GetRigidBodyByName (const std::string &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 (const std::string &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... | |
| template<template< typename > class JointType = Joint> | |
| const JointType< T > & | GetJointByName (const std::string &name, optional< ModelInstanceIndex > model_instance=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 (const std::string &name, optional< ModelInstanceIndex > model_instance=nullopt) |
| A version of GetJointByName that returns a mutable reference. More... | |
| const JointActuator< T > & | GetJointActuatorByName (const std::string &name) const |
Returns a constant reference to an actuator that is identified by the string name in this MultibodyPlant. More... | |
| const JointActuator< T > & | GetJointActuatorByName (const std::string &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... | |
| ModelInstanceIndex | GetModelInstanceByName (const std::string &name) const |
Returns the index to the model instance that is uniquely identified by the string name in this MultibodyPlant. More... | |
Model instance accessors | |
Many of this class's methods expect vectors of tree state or joint actuator inputs which encompass the entire tree. Methods in this section are convenience accessors for the portion of those vectors which apply to a single model instance only. | |
| 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_array) 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 >> &model_v, EigenPtr< VectorX< T >> v_array) const |
Sets the vector of generalized velocities for model_instance in v using v_instance, leaving all other elements in the array untouched. More... | |
Accessing the state | |
| void | SetFreeBodyPoseInWorldFrame (systems::Context< T > *context, const Body< T > &body, const Isometry3< 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 Isometry3< T > &X_FB) const |
Updates context to store the pose X_FB of a given body B in a frame F. More... | |
| Isometry3< T > | CalcRelativeTransform (const systems::Context< T > &context, const Frame< T > &frame_A, const Frame< T > &frame_B) const |
Computes the relative transform X_AB(q) from a frame B to a frame A, as a function of the generalized positions q of the model. 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... | |
| const Isometry3< 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 |
Evaluate the spatial velocity V_WB of a body B in the world frame W. More... | |
| void | CalcPointsGeometricJacobianExpressedInWorld (const systems::Context< T > &context, const Frame< T > &frame_F, const Eigen::Ref< const MatrixX< T >> &p_FP_list, EigenPtr< MatrixX< T >> p_WP_list, EigenPtr< MatrixX< T >> Jv_WFp) const |
Given a list of points with fixed position vectors p_FP in a frame F, (that is, their time derivative DtF(p_FP) in frame F is zero), this method computes the geometric Jacobian Jv_WFp defined by: More... | |
| VectorX< T > | CalcBiasForPointsGeometricJacobianExpressedInWorld (const systems::Context< T > &context, const Frame< T > &frame_F, const Eigen::Ref< const MatrixX< T >> &p_FP_list) const |
Computes the bias term b_WFp associated with the translational acceleration a_WFp of a point P instantaneously moving with a frame F. More... | |
| void | CalcPointsGeometricJacobianExpressedInWorld (const systems::Context< T > &context, const Frame< T > &frame_F, const Eigen::Ref< const MatrixX< T >> &p_WP_list, EigenPtr< MatrixX< T >> Jv_WFp) const |
This is a variant to compute the geometric Jacobian Jv_WFp for a list of points P moving with frame_F, given that we know the position p_WP of each point in the list measured and expressed in the world frame W. More... | |
| void | CalcPointsAnalyticalJacobianExpressedInWorld (const systems::Context< T > &context, const Frame< T > &frame_F, const Eigen::Ref< const MatrixX< T >> &p_FP_list, EigenPtr< MatrixX< T >> p_WP_list, EigenPtr< MatrixX< T >> Jq_WFp) const |
Given a list of points with fixed position vectors p_FP in a frame F, (that is, their time derivative DtF(p_FP) in frame F is zero), this method computes the analytical Jacobian Jq_WFp(q). More... | |
| void | CalcFrameGeometricJacobianExpressedInWorld (const systems::Context< T > &context, const Frame< T > &frame_F, const Eigen::Ref< const Vector3< T >> &p_FP, EigenPtr< MatrixX< T >> Jv_WFp) const |
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. More... | |
| void | CalcRelativeFrameGeometricJacobian (const systems::Context< T > &context, const Frame< T > &frame_B, const Eigen::Ref< const Vector3< T >> &p_BP, const Frame< T > &frame_A, const Frame< T > &frame_E, EigenPtr< MatrixX< T >> Jv_ABp_E) const |
| Computes the geometric Jacobian for a point moving with a given frame. More... | |
| Vector6< T > | CalcBiasForFrameGeometricJacobianExpressedInWorld (const systems::Context< T > &context, const Frame< T > &frame_F, const Eigen::Ref< const Vector3< T >> &p_FP) const |
Given a frame Fp defined by shifting a frame F from its origin Fo to a new origin P, this method computes the bias term Ab_WFp associated with the spatial acceleration A_WFp a frame Fp instantaneously moving with a frame F at a fixed position p_FP. More... | |
| void | CalcJacobianSpatialVelocity (const systems::Context< T > &context, JacobianWrtVariable with_respect_to, const Frame< T > &frame_B, const Eigen::Ref< const Vector3< T >> &p_BP, const Frame< T > &frame_A, const Frame< T > &frame_E, EigenPtr< MatrixX< T >> Jw_ABp_E) const |
| Computes the Jacobian of spatial velocity for a frame instantaneously moving with a specified frame in the model. 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... | |
| T | CalcPotentialEnergy (const systems::Context< T > &context) const |
Computes and returns the total potential energy stored in this multibody model for the configuration given by context. More... | |
| T | CalcConservativePower (const systems::Context< T > &context) const |
| Computes and returns the power generated by conservative forces in the multibody model. More... | |
| void | CalcBiasTerm (const systems::Context< T > &context, EigenPtr< VectorX< T >> Cv) const |
Computes the bias term C(q, v)v containing Coriolis and gyroscopic effects of the multibody equations of motion: 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... | |
| 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< 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... | |
| 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... | |
| void | CalcMassMatrixViaInverseDynamics (const systems::Context< T > &context, EigenPtr< MatrixX< T >> H) 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... | |
Retrieving ports for communication with a SceneGraph. | |
| optional< geometry::SourceId > | get_source_id () const |
Returns the unique id identifying this plant as a source for a SceneGraph. 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_geometry_poses_output_port () const |
| Returns the output port of frames' poses to communicate with a SceneGraph. More... | |
Actuation input | |
The input vector of actuation values can be provided either as 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(). | |
| 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_actuation_input_port (ModelInstanceIndex model_instance) const |
| Returns a constant reference to the input port for external actuation for a specific model instance. More... | |
Continuous state output | |
Output ports are provided to access the continuous state of the whole plant and for individual model instances. | |
| const systems::OutputPort< T > & | get_continuous_state_output_port () const |
| Returns a constant reference to the output port for the full continuous state of the model. More... | |
| const systems::OutputPort< T > & | get_continuous_state_output_port (ModelInstanceIndex model_instance) const |
| Returns a constant reference to the output port for the continuous state of a specific model instance. More... | |
Contact by penalty method | |
Currently 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 non-penetration 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
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 user-supplied "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 damping coefficient in the simple law above, MultibodyPlant chooses the damping 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 asses they are satisfied with the level of inter-penetration 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 inter-penetrations, 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 inter-penetrations being reduced by the same factor of 0.5. In summary, users should choose the largest penetration allowance that results in inter-penetration 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 non-zero 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 error-controlled 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. | |
| 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 non-penetration among bodies. More... | |
| double | get_contact_penalty_method_time_scale () const |
Returns a time-scale estimate tc based on the requested penetration allowance δ set with set_penetration_allowance(). More... | |
Stribeck model of friction | |
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. See tangent_force for a detailed discussion of the Stribeck model. | |
| void | set_stiction_tolerance (double v_stiction=0.001) |
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. More... | |
Friends | |
| template<typename U > | |
| class | MultibodyPlant |
| class | MultibodyPlantTester |
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 Dynamics for an overview of concepts/notation.
MultibodyPlant provides a user-facing API to:
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]:
q̇ = N(q)v
(1) M(q)v̇ + 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:
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:
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:
Refer to the documentation provided in each of the methods above for further details.
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. Must be a valid Eigen scalar. |
Instantiated templates for the following kinds of T's are provided:
They are already available to link against in the containing library. No other values for T are currently supported.
|
delete |
|
delete |
|
explicit |
Default constructor creates a plant with a single "world" body.
Therefore, right after creation, num_bodies() returns one.
| [in] | time_step | An optional parameter indicating 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. Default: 0.0. |
| std::exception | if time_step is negative. |
|
inline |
Scalar-converting copy constructor. See System Scalar Conversion.
|
inline |
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 JointType<T>(args) is a valid constructor. |
| ForceElementType | The type of the ForceElement to add. This method can only be called once for elements of type UniformGravityFieldElement. That is, gravity can only be specified once. |
ForceElementType<T> specialized on the scalar type T of this MultibodyPlant. It will remain valid for the lifetime of this MultibodyPlant.
|
inline |
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.
|
inline |
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.
|
inline |
This method adds a Joint of type JointType between two bodies.
The two bodies connected by this Joint object are referred to as the parent and child bodies. Although the terms parent and child are sometimes used synonymously to describe the relationship between inboard and outboard bodies in multibody models, this usage is wholly unrelated and implies nothing about the inboard-outboard relationship between the bodies. 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 Isometry3<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 Isometry3<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(). |
|
inline |
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. |
this plant. | std::exception | if joint.num_velocities() > 1 since for now we only support actuators for single dof joints. |
|
inline |
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(). |
|
inline |
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.
|
inline |
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. |
|
inline |
Given a frame Fp defined by shifting a frame F from its origin Fo to a new origin P, this method computes the bias term Ab_WFp associated with the spatial acceleration A_WFp a frame Fp instantaneously moving with a frame F at a fixed position p_FP.
That is, the spatial acceleration of frame Fp can be computed as:
A_WFp = Jv_WFp(q)⋅v̇ + Ab_WFp(q, v)
where Ab_WFp(q, v) = J̇v_WFp(q, v)⋅v.
Jv_WFp(q).| [in] | context | The context containing the state of the model. It stores the generalized positions q and generalized velocities v. |
| [in] | frame_F | The position p_FP of frame Fp is measured and expressed in this frame F. |
| [in] | p_FP | The (fixed) position of the origin P of frame Fp as measured and expressed in frame F. |
context. The returned vector is of size 6, with the first three elements related to the bias in angular acceleration and the with the last three elements related to the bias in translational acceleration.
|
inline |
Computes the bias term b_WFp associated with the translational acceleration a_WFp of a point P instantaneously moving with a frame F.
That is, the translational acceleration of point P can be computed as:
a_WFp = Jv_WFp(q)⋅v̇ + b_WFp(q, v)
where b_WFp = J̇v_WFp(q, v)⋅v.
This method computes b_WFp for each point P in p_FP_list defined by its position p_FP in frame_F.
Jv_WFp(q).| [in] | context | The context containing the state of the model. It stores the generalized positions q and generalized velocities v. |
| [in] | frame_F | Points P in the list instantaneously move with this frame. |
| [in] | p_FP_list | A matrix with the fixed position of a list of points P measured and expressed in frame_F. Each column of this matrix contains the position vector p_FP for a point P measured and expressed in frame F. Therefore this input matrix lives in ℝ³ˣⁿᵖ with np the number of points in the list. |
context. The returned vector has size 3⋅np, with np the number of points in p_FP_list, and concatenates the bias terms for each point P in the list in the same order they are specified on input.| std::exception | if p_FP_list does not have 3 rows. |
|
inline |
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.
| [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 (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. |
|
inline |
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).
| 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(). |
|
inline |
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.
| [in] | context | The context containing the state of the model. It stores the generalized positions q. |
| [in] | frame_F | The position p_FP of frame Fp is measured and expressed in this frame F. |
| [in] | p_FP | The (fixed) position of the origin P of frame Fp as measured and expressed in frame F. |
| [out] | 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);
|
| std::exception | if J_WFp is nullptr or if it is not of size 6 x nv. |
|
inline |
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.
|
inline |
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].
| [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.
|
inline |
Computes the Jacobian of spatial velocity for a frame instantaneously moving with a specified frame in the model.
Consider a point P instantaneously moving with a frame B with position p_BP in that frame. Frame Bp is the frame defined by shifting frame B with origin at Bo to a new origin at point P. The spatial velocity V_ABp_E of frame Bp measured in a frame A and expressed in a frame E can be expressed as:
V_ABp_E(q, w) = Jw_ABp_E(q)⋅w
where w represents
with_respect_to is JacobianWrtVariable::kQDot.with_respect_to is JacobianWrtVariable::kV.This method computes Jw_ABp_E(q).
| [in] | context | The context containing the state of the model. It stores the generalized positions q. |
| [in] | with_respect_to | Enum indicating whether Jw_ABp_E converts generalized velocities or time-derivatives of generalized positions to spatial velocities. |
| [in] | frame_B | The position p_BP of point P is measured and expressed in this frame. |
| [in] | p_BP | The (fixed) position of the origin P of frame Bp as measured and expressed in frame B. |
| [in] | frame_A | The second frame in which the spatial velocity V_ABp is measured. |
| [in] | frame_E | Frame in which the velocity V_ABp_E, and therefore the Jacobian Jw_ABp_E is expressed. |
| [out] | Jw_ABp_E | The Jacobian Jw_ABp_E(q), function of the generalized positions q only. This Jacobian relates to the spatial velocity V_ABp_E of frame Bp in A and expressed in E by:
V_ABp_E(q, w) = Jw_ABp_E(q)⋅w Therefore Jw_ABp_E is a matrix of size 6 x nz, where nz is the number of elements in w. On input, matrix Jv_ABp_E must have size 6 x nz or this method throws an exception. Given a 6 x nz Jacobian J, let Jr be the 3 x nz rotational part (top 3 rows) and Jt be the translational part (bottom 3 rows). These can be obtained as follows: Jr_ABp_E = Jw_ABp_E.topRows<3>(); Jt_ABp_E = Jw_ABp_E.bottomRows<3>();
SpatialVelocity<double> V_ABp(Jw_ABp * w); |
| std::exception | if Jw_ABp_E is nullptr or if it is not of size 6 x nz. |
|
inline |
Performs the computation of the mass matrix M(q) of the model using inverse dynamics, where the generalized positions q are stored in context.
| [in] | context | The context containing the state of the model. |
| [out] | H | A valid (non-null) 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 i-th 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:
H.ᵢ(q) = M(q) * e_i
where H.ᵢ(q) (notice the dot for the rows index) denotes the i-th column in M(q).
|
inline |
Given a list of points with fixed position vectors p_FP in a frame F, (that is, their time derivative DtF(p_FP) in frame F is zero), this method computes the analytical Jacobian Jq_WFp(q).
The analytical Jacobian Jq_WFp(q) is defined by:
Jq_WFp(q) = d(p_WFp(q))/dq
where p_WFp(q) is the position of point P, which moves with frame F, in the world frame W.
| [in] | context | The context containing the state of the model. It stores the generalized positions q. |
| [in] | frame_F | The positions p_FP of each point in the input set are measured and expressed in this frame F and are constant (fixed) in this frame. |
| [in] | p_FP_list | A matrix with the fixed position of a set of points P measured and expressed in frame_F. Each column of this matrix contains the position vector p_FP for a point P measured and expressed in frame F. Therefore this input matrix lives in ℝ³ˣⁿᵖ with np the number of points in the set. |
| [out] | p_WP_list | The output positions of each point P now measured and expressed in computing the geometric Jacobian J_WP and therefore external storage must be provided. The output p_WP_list must have the same size as the input set p_FP_list or otherwise this method throws a std::runtime_error exception. That is p_WP_list must be in ℝ³ˣⁿᵖ. |
| [out] | Jq_WFp | The analytical Jacobian Jq_WFp(q), function of the generalized positions q only. We stack the positions of each point P in the world frame W into a column vector p_WFp = [p_WFp1; p_WFp2; ...] of size 3⋅np, with np the number of points in p_FP_list. Then the analytical Jacobian is defined as:
Jq_WFp(q) = ∇(p_WFp(q))
with ∇(⋅) the gradient operator with respect to the generalized positions q. Therefore Jq_WFp is a matrix of size 3⋅np x nq, with nq the number of generalized positions. On input, matrix Jq_WFp must have size 3⋅np x nq or this method throws a std::runtime_error exception. |
| std::exception | if the output p_WP_list is nullptr or does not have the same size as the input array p_FP_list. |
| std::exception | if Jq_WFp is nullptr or if it does not have the appropriate size, see documentation for Jq_WFp for details. |
|
inline |
Given a list of points with fixed position vectors p_FP in a frame F, (that is, their time derivative DtF(p_FP) in frame F is zero), this method computes the geometric Jacobian Jv_WFp defined by:
v_WP(q, v) = Jv_WFp(q)⋅v
where v_WP(q, v) is the translational velocity of point P in the world frame W and q and v are the vectors of generalized position and velocity, respectively.
| [in] | context | The context containing the state of the model. It stores the generalized positions q. |
| [in] | frame_F | The positions p_FP of each point in the input set are measured and expressed in this frame F and are constant (fixed) in this frame. |
| [in] | p_FP_list | A matrix with the fixed position of a set of points P measured and expressed in frame_F. Each column of this matrix contains the position vector p_FP for a point P measured and expressed in frame F. Therefore this input matrix lives in ℝ³ˣⁿᵖ with np the number of points in the set. |
| [out] | p_WP_list | The output positions of each point P now measured and expressed in computing the geometric Jacobian J_WP and therefore external storage must be provided. The output p_WP_list must have the same size as the input set p_FP_list or otherwise this method throws a std::runtime_error exception. That is p_WP_list must be in ℝ³ˣⁿᵖ. |
| [out] | Jv_WFp | The geometric Jacobian Jv_WFp(q), function of the generalized positions q only. This Jacobian relates the translational velocity v_WP of each point P in the input set by:
v_WP(q, v) = Jv_WFp(q)⋅v
so that v_WP is a column vector of size 3⋅np concatenating the velocity of all points P in the same order they were given in the input set. Therefore J_WFp is a matrix of size 3⋅np x nv, with nv the number of generalized velocities. On input, matrix J_WFp must have size 3⋅np x nv or this method throws a std::runtime_error exception. |
| std::exception | if the output p_WP_list is nullptr or does not have the same size as the input array p_FP_list. |
| std::exception | if Jv_WFp is nullptr or if it does not have the appropriate size, see documentation for Jv_WFp for details. |
|
inline |
This is a variant to compute the geometric Jacobian Jv_WFp for a list of points P moving with frame_F, given that we know the position p_WP of each point in the list measured and expressed in the world frame W.
The geometric Jacobian Jv_WFp is defined such that:
v_WP(q, v) = Jv_WFp(q)⋅v
where v_WP(q, v) is the translational velocity of point P in the world frame W and q and v are the vectors of generalized position and velocity, respectively. Since the spatial velocity of each point P is linear in the generalized velocities, the geometric Jacobian Jv_WFp is a function of the generalized coordinates q only.
| [in] | context | The context containing the state of the model. It stores the generalized positions q. |
| [in] | frame_F | Points P in the list instantaneously move with this frame. |
| [in] | p_WP_list | A matrix with the fixed position of a list of points P measured and expressed in the world frame W. Each column of this matrix contains the position vector p_WP for a point P measured and expressed in the world frame W. Therefore this input matrix lives in ℝ³ˣⁿᵖ with np the number of points in the list. |
| [out] | Jv_WFp | The geometric Jacobian Jv_WFp(q), function of the generalized positions q only. This Jacobian relates the translational velocity v_WP of each point P in the input list by:
`v_WP(q, v) = Jv_WFp(q)⋅v`
so that v_WP is a column vector of size 3⋅np concatenating the velocity of all points P in the same order they were given in the input list. Therefore J_WP is a matrix of size 3⋅np x nv, with nv the number of generalized velocities. On input, matrix J_WP must have size 3⋅np x nv or this method throws a std::runtime_error exception. |
| std::exception | if Jv_WFp is nullptr or if it does not have the appropriate size, see documentation for Jv_WFp for details. |
|
inline |
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.
|
inline |
Computes and returns the total potential energy stored in this multibody model for the configuration given by context.
| [in] | context | The context containing the state of the model. |
this multibody model.
|
inline |
Computes the geometric Jacobian for a point moving with a given frame.
Consider a point P instantaneously moving with a frame B with position p_BP in that frame. Frame Bp is the frame defined by shifting frame B with origin at Bo to a new origin at point P. The spatial velocity V_ABp_E of frame Bp measured in a frame A and expressed in a frame E relates to the generalized velocities of the system by the geometric Jacobian Jv_ABp_E(q) by:
V_ABp_E(q, v) = Jv_ABp_E(q)⋅v
This method computes the geometric Jacobian Jv_ABp_E(q).
| [in] | context | The context containing the state of the model. It stores the generalized positions q. |
| [in] | frame_B | The position p_BP of point P is measured and expressed in this frame. |
| [in] | p_BP | The (fixed) position of the origin P of frame Bp as measured and expressed in frame B. |
| [in] | frame_A | The second frame in which the spatial velocity V_ABp is measured and expressed. |
| [in] | frame_E | Frame in which the velocity V_ABp_E is expressed. |
| [out] | Jv_ABp_E | The geometric Jacobian Jv_ABp_E(q), function of the generalized positions q only. This Jacobian relates to the spatial velocity V_ABp_E of frame Bp in A and expressed in E by:
V_ABp_E(q, v) = Jv_ABp_E(q)⋅v
Therefore Jv_ABp_E is a matrix of size 6 x nv, with nv the number of generalized velocities. On input, matrix Jv_ABp_E must have size 6 x nv or this method throws an exception. Given a 6 x nv spatial Jacobian Jv, let Jvr be the 3 x nv rotational part (top 3 rows) and Jvt be the translational part (bottom 3 rows). These can be obtained as follows:
Jvr_ABp = Jv_ABp.topRows<3>();
Jvt_ABp = Jv_ABp.bottomRows<3>();
This ordering is consistent with the internal storage of the SpatialVelocity class. Therefore the following operations results in a valid spatial velocity:
SpatialVelocity<double> V_ABp(Jv_ABp * v);
|
| std::exception | if J_ABp is nullptr or if it is not of size 6 x nv. |
|
inline |
Computes the relative transform X_AB(q) from a frame B to a frame A, as a function of the generalized positions q of the model.
That is, the position p_AQ of a point Q measured and expressed in frame A can be computed from the position p_BQ of this point measured and expressed in frame B using the transformation p_AQ = X_AB⋅p_BQ.
| [in] | context | The context containing the state of the model. It stores the generalized positions q of the model. |
| [in] | frame_A | The target frame A in the computed relative transform X_AB. |
| [in] | frame_B | The source frame B in the computed relative transform X_AB. |
| X_AB | The relative transform from frame B to frame A, such that p_AQ = X_AB⋅p_BQ. |
| 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(). |
| 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 pre-finalize. |
|
inline |
Returns the friction coefficients provided during geometry registration for the given geometry id.
We call these the "default" coefficients but note that we mean user-supplied per-geometry default, not something more global.
| std::exception | if id does not correspond to a geometry in this model registered for contact modeling. |
|
inline |
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::exception | if Finalize() was not called on this model or if body_B does not belong to this model. |
|
inline |
Evaluate the spatial velocity V_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 spatial velocity is requested. |
| std::exception | if Finalize() was not called on this model or if body_B does not belong to this model. |
| void Finalize | ( | geometry::SceneGraph< T > * | scene_graph = nullptr | ) |
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.
| std::logic_error | if
|
|
inline |
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().
| 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_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() or if the model instance does not contain any actuators. |
| std::exception | if the model instance does not exist. |
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. |
|
inline |
Returns a time-scale estimate tc based on the requested penetration allowance δ set with set_penetration_allowance().
For the penalty method in use to enforce non-penetration, 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 this plant is not modeled as a discrete system with periodic updates. |
| std::exception | if called pre-finalize, see Finalize(). |
| const systems::OutputPort< T > & get_continuous_state_output_port | ( | ) | const |
Returns a constant reference to the output port for the full continuous state of the model.
this plant. | const systems::OutputPort< T > & get_continuous_state_output_port | ( | ModelInstanceIndex | model_instance | ) | const |
Returns a constant reference to the output port for the continuous state of a specific model instance.
this plant. | std::exception | if called before Finalize() or if the model instance does not have any state. |
| std::exception | if the model instance does not exist. |
|
inline |
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_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 output port is only available when modeling the plant as a discrete system with periodic updates, see is_discrete().
this plant. | std::exception | if this plant is not modeled as a discrete system with periodic updates. |
| std::exception | if called before Finalize() or if the model instance does not have any generalized velocities. |
| std::exception | if the model instance does not exist. |
| const OutputPort< T > & get_geometry_poses_output_port | ( | ) | const |
Returns the output port of frames' poses to communicate with a SceneGraph.
| std::exception | if this system was not registered 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 Registering geometry with a SceneGraph of this class's documentation for further details on collision geometry registration and connection with a SceneGraph.
| std::exception | if this system was not registered with a SceneGraph. |
|
inline |
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. |
|
inline |
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. |
|
inline |
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. |
|
inline |
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.
| 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:
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 called pre-finalize. |
| std::exception | if body is not part of this plant. |
|
inline |
These queries can be performed at any time during the lifetime of a MultibodyPlant, i.e.
there is no restriction on whether they must be called before or after Finalize(). This implies that these queries can be performed while new multibody elements are being added to the model.
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.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.
|
inline |
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.
|
inline |
If the body with body_index has geometry registered with it, it returns the geometry::FrameId associated with it.
Otherwise, it returns nullopt.
| std::exception | if called pre-finalize. |
|
inline |
If the body with body_index has geometry registered with it, it returns the geometry::FrameId associated with it.
Otherwise this method throws an exception.
| std::exception | if no geometry has been registered with the body indicated by body_index. |
| std::exception | if called pre-finalize. |
|
inline |
Given a geometry frame identifier, returns a pointer to the body associated with that id (nullptr if there is no such body).
|
inline |
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 post-finalize, see Finalize(). Post-finalize calls will always return the same value.
|
inline |
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.
|
inline |
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.
|
inline |
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 pre-finalize. |
|
inline |
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.
|
inline |
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.
|
inline |
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.
|
inline |
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.
|
inline |
Returns the name of a model_instance.
| std::logic_error | when model_instance does not correspond to a model in this model. |
|
inline |
A version of GetJointByName that returns a mutable reference.
|
inline |
(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. |
|
inline |
(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.
|
inline |
(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. |
|
inline |
(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.
|
inline |
See GetMutableVelocities() method above.
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.
| std::logic_error | if called pre-finalize. |
|
inline |
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. |
|
inline |
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.
|
inline |
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. |
|
inline |
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.
|
inline |
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().
Upper limit analog of GetPositionsLowerLimits, where any unbounded or unspecified limits will be +infinity.
|
inline |
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.
|
inline |
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.
|
inline |
Returns a const vector reference containing the generalized velocities.
|
inline |
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.
|
inline |
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().
| 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 post-finalize, see Finalize(). Post-finalize calls will always return the same value.
|
inline |
true if a body named name was added to the MultibodyPlant. | std::logic_error | if the body name occurs in multiple model instances. |
|
inline |
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. |
|
inline |
true if a frame named name was added to the model. | std::logic_error | if the frame name occurs in multiple model instances. |
|
inline |
true if a frame named name was added to model_instance. | std::exception | if model_instance is not valid for this model. |
|
inline |
true if an actuator named name was added to this model. | std::logic_error | if the actuator name occurs in multiple model instances. |
|
inline |
true if an actuator named name was added to model_instance. | std::exception | if model_instance is not valid for this model. |
|
inline |
true if a joint named name was added to this model. | std::logic_error | if the joint name occurs in multiple model instances. |
|
inline |
true if a joint named name was added to model_instance. | std::exception | if model_instance is not valid for this model. |
|
inline |
true if a model instance named name was added to this model.
|
inline |
Returns true if this MultibodyPlant was finalized with a call to Finalize().
Returns true if body is anchored (i.e.
the kinematic path between body and the world only contains weld joints.)
| std::exception | if called pre-finalize. |
| 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.
|
inline |
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.
|
inline |
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. |
|
inline |
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. |
|
inline |
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 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 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 (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(). |
|
inline |
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 (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(). |
|
inline |
Returns the total number of actuated degrees of freedom.
That is, the vector of actuation values u has this size. See AddJointActuator().
|
inline |
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().
|
inline |
Returns the number of joint actuators in the model.
|
inline |
Returns the number of bodies in the model, including the "world" body, which is always part of the model.
|
inline |
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 post-finalize, see Finalize(). Post-finalize calls will always return the same value.
|
inline |
Returns the number of ForceElement objects.
|
inline |
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.
|
inline |
Returns the number of joints in the model.
|
inline |
Returns the number of model instances in the model.
|
inline |
Returns the size of the multibody system state vector x = [q; v].
This will be num_positions() plus num_velocities().
|
inline |
Returns the size of the generalized position vector q for this model.
|
inline |
Returns the size of the generalized position vector q for a specific model instance.
|
inline |
Returns the size of the generalized velocity vector v for this model.
|
inline |
Returns the size of the generalized velocity vector v for a specific model instance.
|
inline |
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 post-finalize, see Finalize(). Post-finalize 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. 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 post-finalize. |
| std::exception | if scene_graph is the nullptr. |
| std::exception | if called more than once. |
| geometry::GeometryId RegisterCollisionGeometry | ( | const Body< T > & | body, |
| const Isometry3< double > & | X_BG, | ||
| const geometry::Shape & | shape, | ||
| const std::string & | name, | ||
| const CoulombFriction< double > & | coulomb_friction, | ||
| geometry::SceneGraph< T > * | scene_graph = nullptr |
||
| ) |
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] | coulomb_friction | Coulomb's law of friction coefficients to model friction on the surface of shape for the given body. |
| [out] | scene_graph | (Deprecated) A valid, non-null pointer to a SceneGraph on which geometry will get registered. |
| std::exception | if called post-finalize. |
| 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 Isometry3< double > & | X_BG, | ||
| const geometry::Shape & | shape, | ||
| const std::string & | name, | ||
| const geometry::IllustrationProperties & | properties, | ||
| geometry::SceneGraph< T > * | scene_graph = nullptr |
||
| ) |
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. |
| [out] | scene_graph | (Deprecated) A valid non nullptr to a SceneGraph on which geometry will get registered. |
| std::exception | if called post-finalize. |
| 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 Isometry3< double > & | X_BG, | ||
| const geometry::Shape & | shape, | ||
| const std::string & | name, | ||
| const Vector4< double > & | diffuse_color, | ||
| geometry::SceneGraph< T > * | scene_graph = nullptr |
||
| ) |
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.
| geometry::GeometryId RegisterVisualGeometry | ( | const Body< T > & | body, |
| const Isometry3< double > & | X_BG, | ||
| const geometry::Shape & | shape, | ||
| const std::string & | name, | ||
| geometry::SceneGraph< T > * | scene_graph = nullptr |
||
| ) |
Overload for visual geometry registration; it relies on the downstream geometry::IllustrationProperties consumer to provide default parameter values (see Geometry Roles for details).
| 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 non-penetration among bodies.
Refer to the section Contact by penalty method for further details.
|
inline |
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.
| std::exception | if v_stiction is non-positive. |
|
inline |
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. |
|
inlineoverride |
Sets the state so that generalized positions and velocities are zero.
| std::exception | if called pre-finalize. See Finalize(). |
|
inline |
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 pre-finalize. |
|
inline |
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 pre-finalize. |
state comes from this MultibodyPlant. | void SetFreeBodyPoseInAnchoredFrame | ( | systems::Context< T > * | context, |
| const Frame< T > & | frame_F, | ||
| const Body< T > & | body, | ||
| const Isometry3< 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 pre-finalize. |
| std::logic_error | if frame F is not anchored to the world. |
| void SetFreeBodyPoseInWorldFrame | ( | systems::Context< T > * | context, |
| const Body< T > & | body, | ||
| const Isometry3< 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 pre-finalize. |
|
inline |
Sets the distribution used by SetRandomState() to populate the x-y-z position component of the floating-base state.
| std::exception | if body is not a free body in the model. |
| std::exception | if called pre-finalize. |
|
inline |
Sets the distribution used by SetRandomState() to populate the rotation component of the floating-base state using uniformly random rotations.
| std::exception | if body is not a free body in the model. |
| std::exception | if called pre-finalize. |
|
inline |
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 pre-finalize. |
|
inline |
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 pre-finalize. |
state comes from this MultibodyPlant.
|
inline |
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(). |
|
inline |
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). |
|
inline |
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.
|
inline |
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(). |
|
inline |
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). |
|
inline |
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).
|
inlineoverride |
Assigns random values to all elements of the state, by drawing samples independently for each joint/floating-base (coming soon: and then solving a mathematical program to "project" these samples onto the registered system constraints).
|
inline |
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(). |
|
inline |
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.
|
inline |
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). |
|
inline |
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).
|
inline |
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 post- finalize, see Finalize().
|
inline |
Deprecated.
| const WeldJoint< T > & WeldFrames | ( | const Frame< T > & | A, |
| const Frame< T > & | B, | ||
| const Isometry3< double > & | X_AB = Isometry3<double>::Identity() |
||
| ) |
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().
|
inline |
Returns a constant reference to the world body.
|
inline |
Returns a constant reference to the world frame.
|
friend |
|
friend |