Drake

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 centerofmass (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 firstorder 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...  
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.
contact_wrench_evaluator  Evaluates the contact wrench between a pair of geometries. 
complementarity_tolerance  The tolerance on the complementarity constraint. 
q_vars  The variable for the generalized position q in prog . 
v_vars  The variable for the generalized velocity v in prog . 
lambda_vars  The variables to parameterize the contact wrench between this pair of geometry. 
prog  The optimization program to which the sliding friction complementarity constraint is imposed. 
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.
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.
contact_wrench_evaluator  The evaluator to compute the contact wrench expressed in the world frame.  
complementarity_tolerance  ε in the documentation above.  
q_vars  The decision variable for the generalized configuration q.  
lambda_vars  The decision variable to parameterize the contact wrench.  
[in,out]  prog  The optimization program to which the constraint is added. 
q_vars
and lambda_vars
have been added to prog
before calling this function.