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  QuadraticConstraint
 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 \]

. More...

 
class  QuadraticCost
 Implements a cost of the form

\[ .5 x'Qx + b'x + c \]

. More...

 
class  L2NormCost
 Implements a cost of the form

\[ |Ax + b|₂ \]

. 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...
 
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...
 
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

◆ AddRelaxNonConvexQuadraticConstraintInTrustRegion()

std::tuple<Binding<LinearConstraint>, std::vector<Binding<RotatedLorentzConeConstraint> >, 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::exceptionif 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::exceptionif 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
    progThe MathematicalProgram to which the relaxed constraints are added.
    xThe decision variables which appear in the original non-convex constraint.
    Q1A positive semidefinite matrix.
    Q2A positive semidefinite matrix.
    yA vector, the variables in the linear term of the quadratic form.
    pA vector, the linear coefficients of the quadratic form.
    linearization_pointThe vector x₀ in the documentation above.
    lower_boundThe left-hand side of the original non-convex constraint.
    upper_boundThe right-hand side of the original non-convex constraint.
    trust_region_gapThe 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,rotated_lorentz_cones,z>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::exceptionwhen the precondition is not satisfied.

◆ 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.

◆ Make2NormSquaredCost()

std::shared_ptr<QuadraticCost> 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<Cost> drake::solvers::MakeFunctionCost ( FF &&  f)

Converts an input of type F to a nonlinear cost.

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

◆ MakeQuadraticErrorCost()

std::shared_ptr<QuadraticCost> drake::solvers::MakeQuadraticErrorCost ( 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).