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	MinimumDistanceLowerBoundConstraint
	Constrain min(d) >= lb, namely 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 lb. More...

class	MinimumDistanceUpperBoundConstraint
	Constrain min(d) <= ub, namely at least one signed distance between a candidate pairs of geometries (according to the logic of SceneGraphInspector::GetCollisionCandidates()) to be no larger than a specified ub. 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	PointToLineDistanceConstraint
	Constrain that the distance between a point P on frame B1 and another line L on frame B2 is within a range [distance_lower, distance_upper]. 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	MidPointIntegrationConstraint
	Implements the midpoint integration. 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	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...

class	PerspectiveQuadraticCost
	If \( z = Ax + b,\) implements a cost of the form: \[ (z_1^2 + z_2^2 + ... + z_{n-1}^2) / z_0. \] Note that this cost is convex when we additionally constrain z_0 > 0. 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	ExpressionCost
	Impose a generic (potentially nonlinear) cost represented as a symbolic Expression. More...

class	SystemConstraintAdapter
	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< QuadraticCost >	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). More...

std::shared_ptr< QuadraticCost >	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) More...

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

std::tuple< Binding< LinearConstraint >, std::vector< Binding< RotatedLorentzConeConstraint > >, VectorXDecisionVariable >	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. 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

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

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

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

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

d is positive.
Q1, Q2, x, x₀ are all of the consistent size.
p and y are of the consistent size.
lower_bound ≤ upper_bound.

Exceptions

std::exception when 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_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.

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

FF	The forwarded function type (e.g., `const F&,`F&&`, ...). The class`F` 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).

Detailed Description

Classes

Functions

Function Documentation

◆ AddRelaxNonConvexQuadraticConstraintInTrustRegion()

◆ AddSlidingFrictionComplementarityExplicitContactConstraint()

◆ AddSlidingFrictionComplementarityImplicitContactConstraint()

◆ AddStaticFrictionConeComplementarityConstraint()

◆ Make2NormSquaredCost()

◆ MakeFunctionCost()

◆ MakeQuadraticErrorCost()