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

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

 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.