Drake
Drake C++ Documentation
MinimumDistanceUpperBoundConstraint Class Referencefinal

## Detailed Description

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.

This constraint should be bound to decision variables corresponding to the configuration vector, q, of the associated MultibodyPlant.

The formulation of the constraint is

SmoothUnderMax( φ((dᵢ(q) - d_influence)/(d_influence - ub)) / φ(-1) ) ≥ 1


where dᵢ(q) is the signed distance of the i-th pair, ub is the upper bound of the minimum distance, d_influence is the "influence distance" (the distance below which a pair of geometries influences the constraint), φ is a solvers::MinimumValuePenaltyFunction. SmoothUnderMax(d) is smooth under approximation of max(d). We require that ub < d_influence. The input scaling (dᵢ(q) - d_influence)/(d_influence - ub) ensures that at the boundary of the feasible set (when dᵢ(q) == ub), we evaluate the penalty function at -1, where it is required to have a non-zero gradient.

#include <drake/multibody/inverse_kinematics/minimum_distance_upper_bound_constraint.h>

## Public Member Functions

MinimumDistanceUpperBoundConstraint (const multibody::MultibodyPlant< double > *const plant, double bound, systems::Context< double > *plant_context, double influence_distance_offset, solvers::MinimumValuePenaltyFunction penalty_function={})
Constructs a MinimumDistanceUpperBoundConstraint. More...

MinimumDistanceUpperBoundConstraint (const multibody::MultibodyPlant< AutoDiffXd > *const plant, double bound, systems::Context< AutoDiffXd > *plant_context, double influence_value_offset, solvers::MinimumValuePenaltyFunction penalty_function={})

MinimumDistanceUpperBoundConstraint (const planning::CollisionChecker *collision_checker, double bound, planning::CollisionCheckerContext *collision_checker_context, double influence_distance_offset, solvers::MinimumValuePenaltyFunction penalty_function={})

~MinimumDistanceUpperBoundConstraint () override

double distance_bound () const
Getter for the upper bound of the minimum distance. More...

double influence_distance () const
Getter for the influence distance. More...

Does not allow copy, move, or assignment
MinimumDistanceUpperBoundConstraint (const MinimumDistanceUpperBoundConstraint &)=delete

MinimumDistanceUpperBoundConstraintoperator= (const MinimumDistanceUpperBoundConstraint &)=delete

MinimumDistanceUpperBoundConstraint (MinimumDistanceUpperBoundConstraint &&)=delete

MinimumDistanceUpperBoundConstraintoperator= (MinimumDistanceUpperBoundConstraint &&)=delete

Public Member Functions inherited from Constraint
template<typename DerivedLB , typename DerivedUB >
Constraint (int num_constraints, int num_vars, const Eigen::MatrixBase< DerivedLB > &lb, const Eigen::MatrixBase< DerivedUB > &ub, const std::string &description="")
Constructs a constraint which has num_constraints rows, with an input num_vars x 1 vector. More...

Constraint (int num_constraints, int num_vars)
Constructs a constraint which has num_constraints rows, with an input num_vars x 1 vector, with no bounds. More...

bool CheckSatisfied (const Eigen::Ref< const Eigen::VectorXd > &x, double tol=1E-6) const
Return whether this constraint is satisfied by the given value, x. More...

bool CheckSatisfied (const Eigen::Ref< const AutoDiffVecXd > &x, double tol=1E-6) const

symbolic::Formula CheckSatisfied (const Eigen::Ref< const VectorX< symbolic::Variable >> &x) const

const Eigen::VectorXd & lower_bound () const

const Eigen::VectorXd & upper_bound () const

int num_constraints () const
Number of rows in the output constraint. More...

Constraint (const Constraint &)=delete

Constraintoperator= (const Constraint &)=delete

Constraint (Constraint &&)=delete

Constraintoperator= (Constraint &&)=delete

Public Member Functions inherited from EvaluatorBase
virtual ~EvaluatorBase ()

void Eval (const Eigen::Ref< const Eigen::VectorXd > &x, Eigen::VectorXd *y) const
Evaluates the expression. More...

void Eval (const Eigen::Ref< const AutoDiffVecXd > &x, AutoDiffVecXd *y) const
Evaluates the expression. More...

void Eval (const Eigen::Ref< const VectorX< symbolic::Variable >> &x, VectorX< symbolic::Expression > *y) const
Evaluates the expression. More...

void set_description (const std::string &description)
Set a human-friendly description for the evaluator. More...

const std::string & get_description () const
Getter for a human-friendly description for the evaluator. More...

std::ostream & Display (std::ostream &os, const VectorX< symbolic::Variable > &vars) const
Formats this evaluator into the given stream using vars for the bound decision variable names. More...

std::ostream & Display (std::ostream &os) const
Formats this evaluator into the given stream, without displaying the decision variables it is bound to. More...

std::string ToLatex (const VectorX< symbolic::Variable > &vars, int precision=3) const
Returns a LaTeX string describing this evaluator. More...

int num_vars () const
Getter for the number of variables, namely the number of rows in x, as used in Eval(x, y). More...

int num_outputs () const
Getter for the number of outputs, namely the number of rows in y, as used in Eval(x, y). More...

void SetGradientSparsityPattern (const std::vector< std::pair< int, int >> &gradient_sparsity_pattern)
Set the sparsity pattern of the gradient matrix ∂y/∂x (the gradient of y value in Eval, w.r.t x in Eval) . More...

const std::optional< std::vector< std::pair< int, int > > > & gradient_sparsity_pattern () const
Returns the vector of (row_index, col_index) that contains all the entries in the gradient of Eval function (∂y/∂x) whose value could be non-zero, namely if ∂yᵢ/∂xⱼ could be non-zero, then the pair (i, j) is in gradient_sparsity_pattern. More...

EvaluatorBase (const EvaluatorBase &)=delete

EvaluatorBaseoperator= (const EvaluatorBase &)=delete

EvaluatorBase (EvaluatorBase &&)=delete

EvaluatorBaseoperator= (EvaluatorBase &&)=delete

## Additional Inherited Members

Protected Member Functions inherited from Constraint
void UpdateLowerBound (const Eigen::Ref< const Eigen::VectorXd > &new_lb)
Updates the lower bound. More...

void UpdateUpperBound (const Eigen::Ref< const Eigen::VectorXd > &new_ub)
Updates the upper bound. More...

void set_bounds (const Eigen::Ref< const Eigen::VectorXd > &new_lb, const Eigen::Ref< const Eigen::VectorXd > &new_ub)
Set the upper and lower bounds of the constraint. More...

virtual bool DoCheckSatisfied (const Eigen::Ref< const Eigen::VectorXd > &x, const double tol) const

virtual bool DoCheckSatisfied (const Eigen::Ref< const AutoDiffVecXd > &x, const double tol) const

virtual symbolic::Formula DoCheckSatisfied (const Eigen::Ref< const VectorX< symbolic::Variable >> &x) const

Protected Member Functions inherited from EvaluatorBase
EvaluatorBase (int num_outputs, int num_vars, const std::string &description="")
Constructs a evaluator. More...

virtual std::ostream & DoDisplay (std::ostream &os, const VectorX< symbolic::Variable > &vars) const
NVI implementation of Display. More...

virtual std::string DoToLatex (const VectorX< symbolic::Variable > &vars, int precision) const

void set_num_outputs (int num_outputs)

## ◆ MinimumDistanceUpperBoundConstraint() [1/5]

 MinimumDistanceUpperBoundConstraint ( const MinimumDistanceUpperBoundConstraint & )
delete

## ◆ MinimumDistanceUpperBoundConstraint() [2/5]

 MinimumDistanceUpperBoundConstraint ( MinimumDistanceUpperBoundConstraint && )
delete

## ◆ MinimumDistanceUpperBoundConstraint() [3/5]

 MinimumDistanceUpperBoundConstraint ( const multibody::MultibodyPlant< double > *const plant, double bound, systems::Context< double > * plant_context, double influence_distance_offset, solvers::MinimumValuePenaltyFunction penalty_function = {} )

Constructs a MinimumDistanceUpperBoundConstraint.

Parameters
 plant The multibody system on which the constraint will be evaluated. bound ub in the class documentation. The upper bound minimum signed distance between any candidate pair of geometries. plant_context The context of plant. The context should be obtained as a subsystem context from the diagram context, where the diagram (that contains both the MultibodyPlant and SceneGraph) creates the diagram context. plant_context cannot be a nullptr. plant_context must outlive this constraint. An example code of getting the plant context is auto diagram_context = diagram.CreateDefaultContext();auto plant_context = plant.GetMyMutableContextFromRoot(diagram_context.get()); penalty_function The penalty function formulation. Default: QuadraticallySmoothedHinge influence_distance_offset The difference (in meters) between the influence distance, d_influence, and the minimum distance_upper, ub (see class documentation), namely influence_distance = bound + influence_distance_offset. This value must be finite and strictly positive, as it is used to scale the signed distances between pairs of geometries. Larger value might increase the possibility of finding a solution through gradient based nonlinear optimization. This is because a geometry pair with distance larger than influence_distance is ignored, so is its gradient; hence the gradient-based optimizer doesn't know to actively reduce the distance between that pair. We strongly suggest to use a different (and larger) influence_distance_offset as the one used in MinimumValueLowerBoundConstraint.
Exceptions
 std::exception if plant has not registered its geometry with a SceneGraph object. std::exception if influence_distance_offset = ∞. std::exception if influence_distance_offset ≤ 0.

## ◆ MinimumDistanceUpperBoundConstraint() [4/5]

 MinimumDistanceUpperBoundConstraint ( const multibody::MultibodyPlant< AutoDiffXd > *const plant, double bound, systems::Context< AutoDiffXd > * plant_context, double influence_value_offset, solvers::MinimumValuePenaltyFunction penalty_function = {} )

Constructs the constraint using MultibodyPlant<AutoDiffXd>.

## ◆ MinimumDistanceUpperBoundConstraint() [5/5]

 MinimumDistanceUpperBoundConstraint ( const planning::CollisionChecker * collision_checker, double bound, planning::CollisionCheckerContext * collision_checker_context, double influence_distance_offset, solvers::MinimumValuePenaltyFunction penalty_function = {} )

Constructs the constraint with CollisionChecker instead of MultibodyPlant.

Parameters
 collision_checker collision_checker must outlive this constraint. collision_checker_context The context for the collision checker. See CollisionChecker class for more details.

## ◆ ~MinimumDistanceUpperBoundConstraint()

 ~MinimumDistanceUpperBoundConstraint ( )
override

## ◆ distance_bound()

 double distance_bound ( ) const

Getter for the upper bound of the minimum distance.

## ◆ influence_distance()

 double influence_distance ( ) const

Getter for the influence distance.

## ◆ operator=() [1/2]

 MinimumDistanceUpperBoundConstraint& operator= ( const MinimumDistanceUpperBoundConstraint & )
delete

## ◆ operator=() [2/2]

 MinimumDistanceUpperBoundConstraint& operator= ( MinimumDistanceUpperBoundConstraint && )
delete

The documentation for this class was generated from the following file: