Drake
Drake C++ Documentation

## 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  AngleBetweenVectorsCost
Implements a cost of the form c*(1-cosθ), where θ is the angle between two vectors a and b. 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  OrientationCost
Implements a cost of the form c * (1 - cos(θ)), where θ is the angle between the orientation of frame A and the orientation of frame B, and c is a cost scaling. 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  PolyhedronConstraint
Constrain the position of points P1, P2, ..., Pn to satisfy the constraint A. 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  PositionCost
Implements a cost of the form (p_AP - p_AQ)ᵀ C (p_AP - p_AQ), where point P is specified relative to frame A and point Q is specified relative to frame B, and the cost is evaluated 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  SpatialVelocityConstraint
Constrains the spatial velocity of a frame C, rigidly attached to a frame B, measured and expressed in frame A. 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  DirectCollocationConstraint
Implements the direct collocation constraints for a first-order hold on the input and a cubic polynomial representation of the state trajectories. More...

class  Constraint
A constraint is a function + lower and upper bounds. More...

lb ≤ .5 xᵀQx + bᵀx ≤ ub Without loss of generality, the class stores a symmetric matrix Q. More...

class  LorentzConeConstraint
Constraining the linear expression $$z=Ax+b$$ lies within the Lorentz cone. More...

class  RotatedLorentzConeConstraint
Constraining that the linear expression $$z=Ax+b$$ lies within rotated Lorentz cone. More...

class  EvaluatorConstraint< EvaluatorType >
A constraint that may be specified using another (potentially nonlinear) evaluator. More...

class  PolynomialConstraint
A constraint on the values of multivariate polynomials. More...

class  LinearConstraint
Implements a constraint of the form $$lb <= Ax <= ub$$. More...

class  LinearEqualityConstraint
Implements a constraint of the form $$Ax = b$$. More...

class  BoundingBoxConstraint
Implements a constraint of the form $$lb <= x <= ub$$. More...

class  LinearComplementarityConstraint
Implements a constraint of the form: More...

class  PositiveSemidefiniteConstraint
Implements a positive semidefinite constraint on a symmetric matrix S

$\text{ S is p.s.d }$

namely, all eigen values of S are non-negative. More...

class  LinearMatrixInequalityConstraint
Impose the matrix inequality constraint on variable x

$F_0 + x_1 F_1 + ... + x_n F_n \text{ is p.s.d}$

where p.s.d stands for positive semidefinite. More...

class  ExpressionConstraint
Impose a generic (potentially nonlinear) constraint represented as a vector of symbolic Expression. More...

class  ExponentialConeConstraint
An exponential cone constraint is a special type of convex cone constraint. More...

class  Cost
Provides an abstract base for all costs. More...

class  LinearCost
Implements a cost of the form

$a'x + b$

Implements a cost of the form

$.5 x'Qx + b'x + c$

class  L1NormCost
Implements a cost of the form ‖Ax + b‖₁. More...

class  L2NormCost
Implements a cost of the form ‖Ax + b‖₂. More...

class  LInfNormCost
Implements a cost of the form ‖Ax + b‖∞. More...

If z = Ax + b, implements a cost of the form: (z_1^2 + z_2^2 + ... More...

class  EvaluatorCost< EvaluatorType >
A cost that may be specified using another (potentially nonlinear) evaluator. More...

class  PolynomialCost
Implements a cost of the form P(x, y...) where P is a multivariate polynomial in x, y, ... More...

This class is a factory class to generate SystemConstraintWrapper. 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::shared_ptr< QuadraticCostMakeQuadraticErrorCost (const Eigen::Ref< const Eigen::MatrixXd > &Q, const Eigen::Ref< const Eigen::VectorXd > &x_desired)
Creates a cost term of the form (x-x_desired)'Q(x-x_desired). More...

std::shared_ptr< QuadraticCostMake2NormSquaredCost (const Eigen::Ref< const Eigen::MatrixXd > &A, const Eigen::Ref< const Eigen::VectorXd > &b)
Creates a quadratic cost of the form |Ax-b|²=(Ax-b)ᵀ(Ax-b) More...

template<typename FF >
std::shared_ptr< CostMakeFunctionCost (FF &&f)
Converts an input of type F to a nonlinear cost. More...

std::tuple< Binding< LinearConstraint >, std::vector< Binding< RotatedLorentzConeConstraint > >, VectorXDecisionVariableAddRelaxNonConvexQuadraticConstraintInTrustRegion (MathematicalProgram *prog, const Eigen::Ref< const VectorXDecisionVariable > &x, const Eigen::Ref< const Eigen::MatrixXd > &Q1, const Eigen::Ref< const Eigen::MatrixXd > &Q2, const Eigen::Ref< const VectorXDecisionVariable > &y, const Eigen::Ref< const Eigen::VectorXd > &p, double lower_bound, double upper_bound, const Eigen::Ref< const Eigen::VectorXd > &linearization_point, double trust_region_gap)
For a non-convex quadratic constraint lb ≤ xᵀQ₁x - xᵀQ₂x + pᵀy ≤ ub where Q₁, Q₂ are both positive semidefinite matrices. More...

## Function Documentation

 std::tuple, std::vector >, VectorXDecisionVariable> drake::solvers::AddRelaxNonConvexQuadraticConstraintInTrustRegion ( MathematicalProgram * prog, const Eigen::Ref< const VectorXDecisionVariable > & x, const Eigen::Ref< const Eigen::MatrixXd > & Q1, const Eigen::Ref< const Eigen::MatrixXd > & Q2, const Eigen::Ref< const VectorXDecisionVariable > & y, const Eigen::Ref< const Eigen::VectorXd > & p, double lower_bound, double upper_bound, const Eigen::Ref< const Eigen::VectorXd > & linearization_point, double trust_region_gap )

For a non-convex quadratic constraint lb ≤ xᵀQ₁x - xᵀQ₂x + pᵀy ≤ ub where Q₁, Q₂ are both positive semidefinite matrices.

y is a vector that can overlap with x. We relax this non-convex constraint by several convex constraints. The steps are

1. Introduce two new variables z₁, z₂, to replace xᵀQ₁x and xᵀQ₂x respectively. The constraint becomes
     lb ≤ z₁ - z₂ + pᵀy ≤ ub              (1)

2. Ideally, we would like to enforce z₁ = xᵀQ₁x and z₂ = xᵀQ₂x through convex constraints. To this end, we first bound z₁ and z₂ from below, as
     z₁ ≥ xᵀQ₁x                            (2)
z₂ ≥ xᵀQ₂x                            (3)

These two constraints are second order cone constraints.
3. To bound z₁ and z₂ from above, we linearize the quadratic forms xᵀQ₁x and xᵀQ₂x at a point x₀. Due to the convexity of the quadratic form, we know that given a positive scalar d > 0, there exists a neighbourhood N(x₀) around x₀, s.t ∀ x ∈ N(x₀)
   xᵀQ₁x ≤ 2 x₀ᵀQ₁(x - x₀) + x₀ᵀQ₁x₀ + d   (4)
xᵀQ₂x ≤ 2 x₀ᵀQ₂(x - x₀) + x₀ᵀQ₂x₀ + d   (5)

Notice N(x₀) is the intersection of two ellipsoids, as formulated in (4) and (5). Therefore, we also enforce the linear constraints
     z₁ ≤ 2 x₀ᵀQ₁(x - x₀) + x₀ᵀQ₁x₀ + d    (6)
z₂ ≤ 2 x₀ᵀQ₂(x - x₀) + x₀ᵀQ₂x₀ + d    (7)

So we relax the original non-convex constraint, with the convex constraints (1)-(3), (6) and (7).

The trust region is the neighbourhood N(x₀) around x₀, such that the inequalities (4), (5) are satisfied ∀ x ∈ N(x₀).

The positive scalar d controls both how much the constraint relaxation is (the original constraint can be violated by at most d), and how big the trust region is.

If there is a solution satisfying the relaxed constraint, this solution can violate the original non-convex constraint by at most d; on the other hand, if there is not a solution satisfying the relaxed constraint, it proves that the original non-convex constraint does not have a solution in the trust region.

This approach is outlined in section III of On Time Optimization of Centroidal Momentum Dynamics by Brahayam Ponton, Alexander Herzog, Stefan Schaal and Ludovic Righetti, ICRA, 2018

The special cases are when Q₁ = 0 or Q₂ = 0.

1. When Q₁ = 0, the original constraint becomes lb ≤ -xᵀQ₂x + pᵀy ≤ ub If ub = +∞, then the original constraint is the convex rotated Lorentz cone constraint xᵀQ₂x ≤ pᵀy - lb. The user should not call this function to relax this convex constraint.
Exceptions
 std::exception if Q₁ = 0 and ub = +∞. If ub < +∞, then we introduce a new variable z, with the constraints lb ≤ -z + pᵀy ≤ ub z ≥ xᵀQ₂x z ≤ 2 x₀ᵀQ₂(x - x₀) + x₀ᵀQ₂x₀ + d
2. When Q₂ = 0, the constraint becomes lb ≤ xᵀQ₁x + pᵀy ≤ ub If lb = -∞, then the original constraint is the convex rotated Lorentz cone constraint xᵀQ₁x ≤ ub - pᵀy. The user should not call this function to relax this convex constraint.
Exceptions
 std::exception if Q₂ = 0 and lb = -∞. If lb > -∞, then we introduce a new variable z, with the constraints lb ≤ z + pᵀy ≤ ub z ≥ xᵀQ₁x z ≤ 2 x₀ᵀQ₁(x - x₀) + x₀ᵀQ₁x₀ + d
3. If both Q₁ and Q₂ are zero, then the original constraint is a convex linear constraint lb ≤ pᵀx ≤ ub. The user should not call this function to relax this convex constraint. Throw a runtime error.
Parameters
 prog The MathematicalProgram to which the relaxed constraints are added. x The decision variables which appear in the original non-convex constraint. Q1 A positive semidefinite matrix. Q2 A positive semidefinite matrix. y A vector, the variables in the linear term of the quadratic form. p A vector, the linear coefficients of the quadratic form. linearization_point The vector x₀ in the documentation above. lower_bound The left-hand side of the original non-convex constraint. upper_bound The right-hand side of the original non-convex constraint. trust_region_gap The user-specified positive scalar, d in the documentation above. This gap determines both the maximal constraint violation and the size of the trust region.
Return values
 linear_constraint includes (1)(6)(7) rotated_lorentz_cones are (2) (3) When either Q1 or Q2 is zero, rotated_lorentz_cones contains only one rotated Lorentz cone, either (2) or (3). z is the newly added variable.
Precondition
1. Q1, Q2 are positive semidefinite.
1. d is positive.
2. Q1, Q2, x, x₀ are all of the consistent size.
3. p and y are of the consistent size.
4. lower_bound ≤ upper_bound.
Exceptions
 std::exception when the precondition is not satisfied.

 std::pair, solvers::Binding > 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_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.
Returns
(sliding_friction_complementarity_constraint, static_friction_cone_constraint), the pair of constraint that imposes (1)-(4) and (6) in sliding_friction_complementarity_constraint.

 std::pair, 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 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_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.
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.

## ◆ Make2NormSquaredCost()

 std::shared_ptr drake::solvers::Make2NormSquaredCost ( const Eigen::Ref< const Eigen::MatrixXd > & A, const Eigen::Ref< const Eigen::VectorXd > & b )

Creates a quadratic cost of the form |Ax-b|²=(Ax-b)ᵀ(Ax-b)

## ◆ MakeFunctionCost()

 std::shared_ptr drake::solvers::MakeFunctionCost ( FF && f )

Converts an input of type F to a nonlinear cost.

Template Parameters
 FF The forwarded function type (e.g., const F&,F&&, ...). The classF` should have functions numInputs(), numOutputs(), and eval(x, y).