Drake
Drake C++ Documentation
MinimumDistanceLowerBoundConstraint Class Referencefinal

Detailed Description

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.

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

The formulation of the constraint is

SmoothOverMax( φ((dᵢ(q) - d_influence)/(d_influence - lb)) / φ(-1) ) ≤ 1

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

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

Public Member Functions

 MinimumDistanceLowerBoundConstraint (const multibody::MultibodyPlant< double > *const plant, double bound, systems::Context< double > *plant_context, solvers::MinimumValuePenaltyFunction penalty_function={}, double influence_distance_offset=0.01)
 Constructs a MinimumDistanceLowerBoundConstraint. More...
 
 MinimumDistanceLowerBoundConstraint (const multibody::MultibodyPlant< AutoDiffXd > *const plant, double bound, systems::Context< AutoDiffXd > *plant_context, solvers::MinimumValuePenaltyFunction penalty_function={}, double influence_distance_offset=0.01)
 Overloaded constructor. More...
 
 MinimumDistanceLowerBoundConstraint (const planning::CollisionChecker *collision_checker, double bound, planning::CollisionCheckerContext *collision_checker_context, solvers::MinimumValuePenaltyFunction penalty_function={}, double influence_distance_offset=0.01)
 Overloaded constructor. More...
 
 ~MinimumDistanceLowerBoundConstraint () override
 
double distance_bound () const
 Getter for the lower bound of the minimum distance. More...
 
double influence_distance () const
 Getter for the influence distance. More...
 
Does not allow copy, move, or assignment
 MinimumDistanceLowerBoundConstraint (const MinimumDistanceLowerBoundConstraint &)=delete
 
MinimumDistanceLowerBoundConstraintoperator= (const MinimumDistanceLowerBoundConstraint &)=delete
 
 MinimumDistanceLowerBoundConstraint (MinimumDistanceLowerBoundConstraint &&)=delete
 
MinimumDistanceLowerBoundConstraintoperator= (MinimumDistanceLowerBoundConstraint &&)=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...
 
bool is_thread_safe () const
 Returns whether it is safe to call Eval in parallel. 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)
 
void set_is_thread_safe (bool is_thread_safe)
 

Constructor & Destructor Documentation

◆ MinimumDistanceLowerBoundConstraint() [1/5]

◆ MinimumDistanceLowerBoundConstraint() [2/5]

◆ MinimumDistanceLowerBoundConstraint() [3/5]

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

Constructs a MinimumDistanceLowerBoundConstraint.

Parameters
plantThe multibody system on which the constraint will be evaluated. plant cannot be a nullptr. plant must outlive this constraint.
boundThe minimum allowed value, lb, of the signed distance between any candidate pair of geometries.
penalty_functionThe penalty function formulation.
Default: QuadraticallySmoothedHinge
plant_contextThe 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());
influence_distance_offsetThe difference (in meters) between the influence distance, d_influence, and the minimum distance, lb (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. Smaller values may improve performance, as fewer pairs of geometries need to be considered in each constraint evaluation.
Default: 0.01 meter. The chosen influence_distance_offset can significantly affect the runtime and optimization performance of using this constraint. Larger values result in more expensive collision checks (since more potential collision candidates must be considered) and may result in worse optimization performance (the optimizer may not be able to find a configuration that satisfies the constraint). In work at TRI, we have used much lower values (e.g. 1e-6) for influence_distance_offset with good results.
Exceptions
std::exceptionif plant has not registered its geometry with a SceneGraph object.
std::exceptionif influence_distance_offset = ∞.
std::exceptionif influence_distance_offset ≤ 0.

◆ MinimumDistanceLowerBoundConstraint() [4/5]

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

Overloaded constructor.

Constructs the constraint using MultibodyPlant<AutoDiffXd>.

◆ MinimumDistanceLowerBoundConstraint() [5/5]

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

Overloaded constructor.

Constructs the constraint with CollisionChecker instead of MultibodyPlant.

Parameters
collision_checkercollision_checker must outlive this constraint.
collision_checker_contextThe context for the collision checker. See CollisionChecker class for more details.

◆ ~MinimumDistanceLowerBoundConstraint()

Member Function Documentation

◆ distance_bound()

double distance_bound ( ) const

Getter for the lower bound of the minimum distance.

◆ influence_distance()

double influence_distance ( ) const

Getter for the influence distance.

◆ operator=() [1/2]

◆ operator=() [2/2]


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