Drake
drake::multibody Namespace Reference

Namespaces

 benchmarks
 
 constraint
 
 hydroelastics
 
 test
 
 test_utilities
 

Classes

struct  AddMultibodyPlantSceneGraphResult
 Temporary result from AddMultibodyPlantSceneGraph. More...
 
class  AngleBetweenVectorsConstraint
 Constrains that the angle between a vector a and another vector b is between [θ_lower, θ_upper]. More...
 
class  ArticulatedBodyInertia
 Articulated Body Inertia is the inertia that a body appears to have when it is the base (or root) of a rigid-body system, also referred to as Articulated Body in the context of articulated body algorithms. More...
 
class  BallRpyJoint
 This Joint allows two bodies to rotate freely relative to one another. More...
 
class  Body
 Body provides the general abstraction of a body with an API that makes no assumption about whether a body is rigid or deformable and neither does it make any assumptions about the underlying physical model or approximation. More...
 
class  BodyFrame
 A BodyFrame is a material Frame that serves as the unique reference frame for a Body. More...
 
class  ContactResults
 A container class storing the contact results information for each contact pair for a given state of the simulation. More...
 
class  ContactResultsToLcmSystem
 A System that encodes ContactResults into a lcmt_contact_results_for_viz message. More...
 
struct  ContactWrench
 Stores the contact wrench (spatial force) from Body A to Body B applied at point Cb. More...
 
class  ContactWrenchEvaluator
 
class  ContactWrenchFromForceInWorldFrameEvaluator
 The contact wrench is τ_AB_W = 0, f_AB_W = λ Namely we assume that λ is the contact force from A to B, applied directly at B's witness point. More...
 
class  CoulombFriction
 Parameters for Coulomb's Law of Friction, namely: More...
 
class  DistanceConstraint
 Constrains the distance between a pair of geometries to be within a range [distance_lower, distance_upper]. More...
 
class  DoorHinge
 This ForceElement models a revolute DoorHinge joint that could exhibit different force/torque characteristics at different states due to the existence of different type of torques on the joint. More...
 
struct  DoorHingeConfig
 Configuration structure for the DoorHinge. More...
 
struct  ExternallyAppliedSpatialForce
 
class  FixedOffsetFrame
 FixedOffsetFrame represents a material frame F whose pose is fixed with respect to a parent material frame P. More...
 
class  ForceElement
 A ForceElement allows modeling state and time dependent forces in a MultibodyTree model. More...
 
class  Frame
 Frame is an abstract class representing a material frame (also called a physical frame), meaning that it is associated with a material point of a Body. More...
 
class  FrameBase
 FrameBase is an abstract representation of the concept of a frame in multibody dynamics. More...
 
class  GaussianTriangleQuadratureRule
 
class  GazeTargetConstraint
 Constrains a target point T to be within a cone K. More...
 
class  HydroelasticContactInfo
 A class containing information regarding contact and contact response between two geometries attached to a pair of bodies. More...
 
struct  HydroelasticQuadraturePointData
 Results from intermediate calculations used during the quadrature routine. More...
 
class  InverseKinematics
 Solves an inverse kinematics (IK) problem on a MultibodyPlant, to find the postures of the robot satisfying certain constraints. More...
 
class  Joint
 A Joint models the kinematical relationship which characterizes the possible relative motion between two bodies. More...
 
class  JointActuator
 The JointActuator class is mostly a simple bookkeeping structure to represent an actuator acting on a given Joint. More...
 
class  LinearBushingRollPitchYaw
 This ForceElement models a massless flexible bushing that connects a frame A of a link (body) L0 to a frame C of a link (body) L1. More...
 
class  LinearSpringDamper
 This ForceElement models a spring-damper attached between two points on two different bodies. More...
 
class  ManipulatorEquationConstraint
 A Constraint to impose the manipulator equation: 0 = (Buₙ₊₁ + ∑ᵢ (Jᵢ_WBᵀ(qₙ₊₁)ᵀ * Fᵢ_AB_W(λᵢ,ₙ₊₁)) More...
 
class  MinimumDistanceConstraint
 Constrain the signed distance between all candidate pairs of geometries (according to the logic of SceneGraphInspector::GetCollisionCandidates()) to be no smaller than a specified minimum distance. More...
 
class  MultibodyElement
 A class representing an element (subcomponent) of a MultibodyPlant or (internally) a MultibodyTree. More...
 
class  MultibodyForces
 A class to hold a set of forces applied to a MultibodyTree system. More...
 
class  MultibodyPlant
 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...
 
class  MultibodyPlantTester
 
class  OrientationConstraint
 Constrains that the angle difference θ between the orientation of frame A and the orientation of frame B to satisfy θ ≤ θ_bound. More...
 
class  PackageMap
 Maps ROS package names to their full path on the local file system. More...
 
class  Parser
 Parses SDF and URDF input files into a MultibodyPlant and (optionally) a SceneGraph. More...
 
class  PointPairContactInfo
 A class containing information regarding contact response between two bodies including: More...
 
class  PointToPointDistanceConstraint
 Constrain that the distance between a point P1 on frame B1 and another point P2 on frame B2 is within a range [distance_lower, distance_upper]. More...
 
class  PositionConstraint
 Constrains the position of a point Q, rigidly attached to a frame B, to be within a bounding box measured and expressed in frame A. More...
 
class  PrismaticJoint
 This Joint allows two bodies to translate relative to one another along a common axis. More...
 
class  Propeller
 A System that connects to the MultibodyPlant in order to model the effects of one or more controlled propellers acting on a Body. More...
 
struct  PropellerInfo
 Parameters that describe the kinematic frame and force-production properties of a single propeller. More...
 
class  RevoluteJoint
 This Joint allows two bodies to rotate relatively to one another around a common axis. More...
 
class  RevoluteSpring
 This ForceElement models a torsional spring attached to a RevoluteJoint and applies a torque to that joint. More...
 
class  RigidBody
 The term rigid body implies that the deformations of the body under consideration are so small that they have no significant effect on the overall motions of the body and therefore deformations can be neglected. More...
 
class  RotationalInertia
 This class describes the mass distribution (inertia properties) of a body or composite body about a particular point. More...
 
struct  SignedDistanceWithTimeDerivative
 The struct containing the signed distance and its time derivative between a pair of geometries. More...
 
class  SpatialAcceleration
 This class is used to represent a spatial acceleration that combines rotational (angular acceleration) and translational (linear acceleration) components. More...
 
class  SpatialForce
 This class is used to represent a spatial force (also called a wrench) that combines both rotational (torque) and translational force components. More...
 
class  SpatialInertia
 This class represents the physical concept of a Spatial Inertia. More...
 
class  SpatialMomentum
 This class is used to represent the spatial momentum of a particle, system of particles or body (whether rigid or soft.) The linear momentum l_NS of a system of particles S in a reference frame N is defined by: More...
 
class  SpatialVector
 This class is used to represent physical quantities that correspond to spatial vectors such as spatial velocities, spatial accelerations and spatial forces. More...
 
class  SpatialVelocity
 This class is used to represent a spatial velocity (also called a twist) that combines rotational (angular) and translational (linear) velocity components. More...
 
class  StaticEquilibriumConstraint
 Impose the static equilibrium constraint 0 = τ_g + Bu + ∑J_WBᵀ(q) * Fapp_B_W. More...
 
class  StaticEquilibriumProblem
 Finds the static equilibrium pose of a multibody system through optimization. More...
 
class  StaticFrictionConeConstraint
 Formulates the nonlinear friction cone constraint |fₜ| ≤ μ*fₙ, where fₜ is the tangential contact force, fₙ is the normal contact force, and μ is the friction coefficient. More...
 
class  TamsiSolver
 
struct  TamsiSolverIterationStats
 Struct used to store information about the iteration process performed by TamsiSolver. More...
 
struct  TamsiSolverParameters
 These are the parameters controlling the iteration process of the TamsiSolver solver. More...
 
class  TriangleQuadrature
 A class for integrating a function using numerical quadrature over triangular domains. More...
 
class  TriangleQuadratureRule
 A "rule" (weights and quadrature points) for computing quadrature over triangular domains. More...
 
class  UniformGravityFieldElement
 This ForceElement allows modeling the effect of a uniform gravity field as felt by bodies on the surface of the Earth. More...
 
class  UnitInertia
 This class is used to represent rotational inertias for unit mass bodies. More...
 
class  UniversalJoint
 This joint models a universal joint allowing two bodies to rotate relative to one another with two degrees of freedom. More...
 
class  WeldJoint
 This Joint fixes the relative pose between two frames as if "welding" them together. More...
 

Typedefs

using MinimumDistancePenaltyFunction = solvers::MinimumValuePenaltyFunction
 Computes the penalty function φ(x) and its derivatives dφ(x)/dx. More...
 
using FrameIndex = TypeSafeIndex< class FrameTag >
 Type used to identify frames by index in a multibody tree system. More...
 
using BodyIndex = TypeSafeIndex< class BodyTag >
 Type used to identify bodies by index in a multibody tree system. More...
 
using ForceElementIndex = TypeSafeIndex< class ForceElementTag >
 Type used to identify force elements by index within a multibody tree system. More...
 
using JointIndex = TypeSafeIndex< class JointElementTag >
 Type used to identify joints by index within a multibody tree system. More...
 
using JointActuatorIndex = TypeSafeIndex< class JointActuatorElementTag >
 Type used to identify actuators by index within a multibody tree system. More...
 
using ModelInstanceIndex = TypeSafeIndex< class ModelInstanceTag >
 Type used to identify model instances by index within a multibody tree system. More...
 

Enumerations

enum  ContactModel { kHydroelasticsOnly, kPointContactOnly, kHydroelasticWithFallback }
 Enumeration for contact model options. More...
 
enum  TamsiSolverResult { kSuccess = 0, kMaxIterationsReached = 1, kLinearSolverFailed = 2 }
 The result from TamsiSolver::SolveWithGuess() used to report the success or failure of the solver. More...
 
enum  JacobianWrtVariable { kQDot, kV }
 Enumeration that indicates whether the Jacobian is partial differentiation with respect to q̇ (time-derivatives of generalized positions) or with respect to v (generalized velocities). More...
 

Functions

std::pair< solvers::Binding< internal::SlidingFrictionComplementarityNonlinearConstraint >, solvers::Binding< StaticFrictionConeConstraint > > AddSlidingFrictionComplementarityExplicitContactConstraint (const ContactWrenchEvaluator *contact_wrench_evaluator, double complementarity_tolerance, const Eigen::Ref< const VectorX< symbolic::Variable >> &q_vars, const Eigen::Ref< const VectorX< symbolic::Variable >> &v_vars, const Eigen::Ref< const VectorX< symbolic::Variable >> &lambda_vars, solvers::MathematicalProgram *prog)
 For a pair of geometries in explicit contact, adds the sliding friction complementarity constraint explained in sliding_friction_complementarity_constraint to an optimization program. More...
 
std::pair< solvers::Binding< internal::SlidingFrictionComplementarityNonlinearConstraint >, solvers::Binding< internal::StaticFrictionConeComplementarityNonlinearConstraint > > AddSlidingFrictionComplementarityImplicitContactConstraint (const ContactWrenchEvaluator *contact_wrench_evaluator, double complementarity_tolerance, const Eigen::Ref< const VectorX< symbolic::Variable >> &q_vars, const Eigen::Ref< const VectorX< symbolic::Variable >> &v_vars, const Eigen::Ref< const VectorX< symbolic::Variable >> &lambda_vars, solvers::MathematicalProgram *prog)
 For a pair of geometries in implicit contact (they may or may not be in contact, adds the sliding friction complementarity constraint explained in sliding_friction_complementarity_constraint. More...
 
solvers::Binding< internal::StaticFrictionConeComplementarityNonlinearConstraint > AddStaticFrictionConeComplementarityConstraint (const ContactWrenchEvaluator *contact_wrench_evaluator, double complementarity_tolerance, const Eigen::Ref< const VectorX< symbolic::Variable >> &q_vars, const Eigen::Ref< const VectorX< symbolic::Variable >> &lambda_vars, solvers::MathematicalProgram *prog)
 Adds the complementarity constraint on the static friction force between a pair of contacts |ft_W| <= μ * n_Wᵀ * f_W (static friction force in the friction cone). More...
 
SignedDistanceWithTimeDerivative CalcDistanceAndTimeDerivative (const multibody::MultibodyPlant< double > &plant, const SortedPair< geometry::GeometryId > &geometry_pair, const systems::Context< double > &context)
 Given a pair of geometries and the generalized position/velocity of the plant, compute the signed distance between the pair of geometries and the time derivative of the signed distance. More...
 
systems::lcm::LcmPublisherSystemConnectContactResultsToDrakeVisualizer (systems::DiagramBuilder< double > *builder, const MultibodyPlant< double > &multibody_plant, lcm::DrakeLcmInterface *lcm=nullptr)
 Extends a Diagram with the required components to publish contact results to drake_visualizer. More...
 
systems::lcm::LcmPublisherSystemConnectContactResultsToDrakeVisualizer (systems::DiagramBuilder< double > *builder, const MultibodyPlant< double > &multibody_plant, const systems::OutputPort< double > &contact_results_port, lcm::DrakeLcmInterface *lcm=nullptr)
 Implements ConnectContactResultsToDrakeVisualizer, but using explicitly specified contact_results_port and geometry_input_port arguments. More...
 
template<typename T >
CoulombFriction< T > CalcContactFrictionFromSurfaceProperties (const CoulombFriction< T > &surface_properties1, const CoulombFriction< T > &surface_properties2)
 Given the surface properties of two different surfaces, this method computes the Coulomb's law coefficients of friction characterizing the interaction by friction of the given surface pair. More...
 
template<typename T >
bool operator== (const HydroelasticQuadraturePointData< T > &data1, const HydroelasticQuadraturePointData< T > &data2)
 Returns true if all of the corresponding individual fields of data1 and data2 are equal (i.e., using their corresponding operator==() functions). More...
 
BodyIndex world_index ()
 For every MultibodyTree the world body always has this unique index and it is always zero. More...
 
ModelInstanceIndex world_model_instance ()
 Returns the model instance containing the world body. More...
 
ModelInstanceIndex default_model_instance ()
 Returns the model instance which contains all tree elements with no explicit model instance specified. More...
 

Typedef Documentation

◆ BodyIndex

using BodyIndex = TypeSafeIndex<class BodyTag>

Type used to identify bodies by index in a multibody tree system.

◆ ForceElementIndex

using ForceElementIndex = TypeSafeIndex<class ForceElementTag>

Type used to identify force elements by index within a multibody tree system.

◆ FrameIndex

using FrameIndex = TypeSafeIndex<class FrameTag>

Type used to identify frames by index in a multibody tree system.

◆ JointActuatorIndex

using JointActuatorIndex = TypeSafeIndex<class JointActuatorElementTag>

Type used to identify actuators by index within a multibody tree system.

◆ JointIndex

using JointIndex = TypeSafeIndex<class JointElementTag>

Type used to identify joints by index within a multibody tree system.

◆ MinimumDistancePenaltyFunction

Computes the penalty function φ(x) and its derivatives dφ(x)/dx.

Valid penalty functions must meet the following criteria:

  1. φ(x) ≥ 0 ∀ x ∈ ℝ.
  2. dφ(x)/dx ≤ 0 ∀ x ∈ ℝ.
  3. φ(x) = 0 ∀ x ≥ 0.
  4. dφ(x)/dx < 0 ∀ x < 0.

◆ ModelInstanceIndex

using ModelInstanceIndex = TypeSafeIndex<class ModelInstanceTag>

Type used to identify model instances by index within a multibody tree system.

Enumeration Type Documentation

◆ ContactModel

enum ContactModel
strong

Enumeration for contact model options.

Enumerator
kHydroelasticsOnly 

Contact forces are computed using the Hydroelastic model.

Conctact between unsupported geometries will cause a runtime exception.

kPointContactOnly 

Contact forces are computed using a point contact model, see Numerical Approximation of Point Contact.

kHydroelasticWithFallback 

Contact forces are computed using the hydroelastic model, where possible.

For most other unsupported colliding pairs, the point model from kPointContactOnly is used. See geometry::QueryObject:ComputeContactSurfacesWithFallback for more details.

◆ JacobianWrtVariable

enum JacobianWrtVariable
strong

Enumeration that indicates whether the Jacobian is partial differentiation with respect to q̇ (time-derivatives of generalized positions) or with respect to v (generalized velocities).

Enumerator
kQDot 

J = ∂V/∂q̇

kV 

J = ∂V/∂v.

◆ TamsiSolverResult

enum TamsiSolverResult
strong

The result from TamsiSolver::SolveWithGuess() used to report the success or failure of the solver.

Enumerator
kSuccess 

Successful computation.

kMaxIterationsReached 

The maximum number of iterations was reached.

kLinearSolverFailed 

The linear solver used within the Newton-Raphson loop failed.

This might be caused by a divergent iteration that led to an invalid Jacobian matrix.

Function Documentation

◆ AddSlidingFrictionComplementarityExplicitContactConstraint()

std::pair<solvers::Binding< internal::SlidingFrictionComplementarityNonlinearConstraint>, solvers::Binding<StaticFrictionConeConstraint> > drake::multibody::AddSlidingFrictionComplementarityExplicitContactConstraint ( const ContactWrenchEvaluator contact_wrench_evaluator,
double  complementarity_tolerance,
const Eigen::Ref< const VectorX< symbolic::Variable >> &  q_vars,
const Eigen::Ref< const VectorX< symbolic::Variable >> &  v_vars,
const Eigen::Ref< const VectorX< symbolic::Variable >> &  lambda_vars,
solvers::MathematicalProgram prog 
)

For a pair of geometries in explicit contact, adds the sliding friction complementarity constraint explained in sliding_friction_complementarity_constraint to an optimization program.

This function adds the slack variables (f_static, f_sliding, c), and impose all the constraints in sliding_friction_complementarity_constraint.

Parameters
contact_wrench_evaluatorEvaluates the contact wrench between a pair of geometries.
complementarity_toleranceThe tolerance on the complementarity constraint.
q_varsThe variable for the generalized position q in prog.
v_varsThe variable for the generalized velocity v in prog.
lambda_varsThe variables to parameterize the contact wrench between this pair of geometry.
progThe optimization program to which the sliding friction complementarity constraint is imposed.
Returns
(sliding_friction_complementarity_constraint, static_friction_cone_constraint), the pair of constraint that imposes (1)-(4) and (6) in sliding_friction_complementarity_constraint.

◆ AddSlidingFrictionComplementarityImplicitContactConstraint()

std::pair<solvers::Binding< internal::SlidingFrictionComplementarityNonlinearConstraint>, solvers::Binding< internal::StaticFrictionConeComplementarityNonlinearConstraint> > drake::multibody::AddSlidingFrictionComplementarityImplicitContactConstraint ( const ContactWrenchEvaluator contact_wrench_evaluator,
double  complementarity_tolerance,
const Eigen::Ref< const VectorX< symbolic::Variable >> &  q_vars,
const Eigen::Ref< const VectorX< symbolic::Variable >> &  v_vars,
const Eigen::Ref< const VectorX< symbolic::Variable >> &  lambda_vars,
solvers::MathematicalProgram prog 
)

For a pair of geometries in implicit contact (they may or may not be in contact, adds the sliding friction complementarity constraint explained in sliding_friction_complementarity_constraint.

The input arguments are the same as those in AddSlidingFrictionComplementarityExplicitContactConstraint(). The difference is that the returned argument includes the nonlinear complementarity binding 0 ≤ φ(q) ⊥ fₙ≥ 0, which imposes the constraint for implicit contact.

◆ AddStaticFrictionConeComplementarityConstraint()

solvers::Binding<internal::StaticFrictionConeComplementarityNonlinearConstraint> drake::multibody::AddStaticFrictionConeComplementarityConstraint ( const ContactWrenchEvaluator contact_wrench_evaluator,
double  complementarity_tolerance,
const Eigen::Ref< const VectorX< symbolic::Variable >> &  q_vars,
const Eigen::Ref< const VectorX< symbolic::Variable >> &  lambda_vars,
solvers::MathematicalProgram prog 
)

Adds the complementarity constraint on the static friction force between a pair of contacts |ft_W| <= μ * n_Wᵀ * f_W (static friction force in the friction cone).

fn_W * sdf = 0 (complementarity condition) sdf >= 0 (no penetration) where sdf stands for signed distance function, ft_W stands for the tangential friction force expressed in the world frame.

Mathematically, we add the following constraints to the optimization program

f_Wᵀ * ((μ² + 1)* n_W * n_Wᵀ - I) * f_W ≥ 0                    (1)
n_Wᵀ * f_W = α                                                 (2)
sdf(q) = β                                                     (3)
0 ≤ α * β ≤ ε                                                  (4)
α ≥ 0                                                          (5)
β ≥ 0                                                          (6)

the slack variables α and β are added to the optimization program as well.

Parameters
contact_wrench_evaluatorThe evaluator to compute the contact wrench expressed in the world frame.
complementarity_toleranceε in the documentation above.
q_varsThe decision variable for the generalized configuration q.
lambda_varsThe decision variable to parameterize the contact wrench.
[in,out]progThe optimization program to which the constraint is added.
Returns
binding The binding containing the nonlinear constraints (1)-(4).
Precondition
Both q_vars and lambda_vars have been added to prog before calling this function.

◆ CalcContactFrictionFromSurfaceProperties()

CoulombFriction<T> drake::multibody::CalcContactFrictionFromSurfaceProperties ( const CoulombFriction< T > &  surface_properties1,
const CoulombFriction< T > &  surface_properties2 
)

Given the surface properties of two different surfaces, this method computes the Coulomb's law coefficients of friction characterizing the interaction by friction of the given surface pair.

The surface properties are specified by individual Coulomb's law coefficients of friction. As outlined in the class's documentation for CoulombFriction, friction coefficients characterize a surface pair and not individual surfaces. However, we find it useful in practice to associate the abstract idea of friction coefficients to a single surface. Please refer to the documentation for CoulombFriction for details on this topic.

More specifically, this method computes the contact coefficients for the given surface pair as:

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

where the operation above is performed separately on the static and dynamic friction coefficients.

Parameters
[in]surface_properties1Surface properties for surface 1. Specified as an individual set of Coulomb's law coefficients of friction.
[in]surface_properties2Surface properties for surface 2. Specified as an individual set of Coulomb's law coefficients of friction.
Returns
the combined friction coefficients for the interacting surfaces.

◆ CalcDistanceAndTimeDerivative()

SignedDistanceWithTimeDerivative drake::multibody::CalcDistanceAndTimeDerivative ( const multibody::MultibodyPlant< double > &  plant,
const SortedPair< geometry::GeometryId > &  geometry_pair,
const systems::Context< double > &  context 
)

Given a pair of geometries and the generalized position/velocity of the plant, compute the signed distance between the pair of geometries and the time derivative of the signed distance.

This function is similar to QueryObject::ComputeSignedDistancePairClosestPoints(), but it also provides the time derivative of the signed distance.

Parameters
plantThe plant on which the geometries are attached. This plant must have been connected to a SceneGraph.
geometry_pairThe pair of geometries whose distance and time derivative are computed.
contextThe context of the plant. This must store both q and v. This context must have been extracted from the diagram context which contains both MultibodyPlant and SceneGraph contexts.
[out]distanceThe signed distance between the pair of geometry.
[out]distance_time_derivativeThe time derivative of the signed distance.

◆ default_model_instance()

ModelInstanceIndex drake::multibody::default_model_instance ( )

Returns the model instance which contains all tree elements with no explicit model instance specified.

◆ operator==()

bool drake::multibody::operator== ( const HydroelasticQuadraturePointData< T > &  data1,
const HydroelasticQuadraturePointData< T > &  data2 
)

Returns true if all of the corresponding individual fields of data1 and data2 are equal (i.e., using their corresponding operator==() functions).

◆ world_index()

BodyIndex drake::multibody::world_index ( )

For every MultibodyTree the world body always has this unique index and it is always zero.

◆ world_model_instance()

ModelInstanceIndex drake::multibody::world_model_instance ( )

Returns the model instance containing the world body.

For every MultibodyTree the world body always has this unique model instance and it is always zero (as described in #3088).