Drake

Detailed Description

Most simple costs and constraints can be added directly to a MathematicalProgram through the MathematicalProgram::AddCost() and MathematicalProgram::AddConstraint() interfaces and their specializations.

We also provide a number of classes for common and/or more complex costs and constraints, such as those built on the multibody::MultibodyPlant API.

Classes

class  AngleBetweenVectorsConstraint
 Constrains that the angle between a vector a and another vector b is between [θ_lower, θ_upper]. More...
 
class  ComPositionConstraint
 Impose the constraint p_EScm(q) - p_EC = 0, where p_EScm(q) is a function that computes the center-of-mass (COM) position from robot generalized position q, expressed in a frame E. More...
 
class  DistanceConstraint
 Constrains the distance between a pair of geometries to be within a range [distance_lower, distance_upper]. More...
 
class  GazeTargetConstraint
 Constrains a target point T to be within a cone K. 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  OrientationConstraint
 Constrains that the angle difference θ between the orientation of frame A and the orientation of frame B to satisfy θ ≤ θ_bound. 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  UnitQuaternionConstraint
 Constrains the quaternion to have a unit length. More...
 
class  CentroidalMomentumConstraint
 Impose the constraint CentroidalMomentum(q, v) - h_WC = 0 with decision variables [q;v;h_WC] or CentroidalAngularMomentum(q, v) - k_WC = 0 with decision variables [q; v; k_WC] h_WC is the 6D spatial momentum (linear and angular momentum about the center of mass C) expressed in the world frame (W). More...
 
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  ManipulatorEquationConstraint
 A Constraint to impose the manipulator equation: 0 = (Buₙ₊₁ + ∑ᵢ (Jᵢ_WBᵀ(qₙ₊₁)ᵀ * Fᵢ_AB_W(λᵢ,ₙ₊₁)) More...
 
class  QuaternionEulerIntegrationConstraint
 If we have a body with orientation quaternion z₁ at time t₁, and a quaternion z₂ at time t₂ = t₁ + h, with the angular velocity ω (expressed in the world frame), we impose the constraint that the body rotates at a constant velocity ω from quaternion z₁ to quaternion z₂ within time interval h. More...
 
class  StaticEquilibriumConstraint
 Impose the static equilibrium constraint 0 = τ_g + Bu + ∑J_WBᵀ(q) * Fapp_B_W. 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  Constraint
 A constraint is a function + lower and upper bounds. More...
 
class  Cost
 Provides an abstract base for all costs. More...
 
class  SystemConstraintAdapter
 This class is a factory class to generate SystemConstraintWrapper. More...
 
class  DirectCollocationConstraint
 Implements the direct collocation constraints for a first-order hold on the input and a cubic polynomial representation of the state trajectories. 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...
 

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.