Drake
MinimumDistanceConstraint Class Referencefinal

Detailed Description

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.

The formulation of the constraint is

0 ≤ SmoothMax( φ((dᵢ - d_influence)/(d_influence - dₘᵢₙ)) / φ(-1) ) ≤ 1

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

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

Public Member Functions

 MinimumDistanceConstraint (const multibody::MultibodyPlant< double > *const plant, double minimum_distance, systems::Context< double > *plant_context, MinimumDistancePenaltyFunction penalty_function={}, double influence_distance_offset=1)
 Constructs a MinimumDistanceConstraint. More...
 
 MinimumDistanceConstraint (const multibody::MultibodyPlant< AutoDiffXd > *const plant, double minimum_distance, systems::Context< AutoDiffXd > *plant_context, MinimumDistancePenaltyFunction penalty_function={}, double influence_distance_offset=1)
 Overloaded constructor. More...
 
 ~MinimumDistanceConstraint () override
 
double minimum_distance () const
 Getter for the minimum distance. More...
 
double influence_distance () const
 Getter for the influence distance. More...
 
Does not allow copy, move, or assignment
 MinimumDistanceConstraint (const MinimumDistanceConstraint &)=delete
 
MinimumDistanceConstraintoperator= (const MinimumDistanceConstraint &)=delete
 
 MinimumDistanceConstraint (MinimumDistanceConstraint &&)=delete
 
MinimumDistanceConstraintoperator= (MinimumDistanceConstraint &&)=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...
 
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 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 > &lower_bound, const Eigen::Ref< const Eigen::VectorXd > &upper_bound)
 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...
 
void set_num_outputs (int num_outputs)
 

Constructor & Destructor Documentation

◆ MinimumDistanceConstraint() [1/4]

◆ MinimumDistanceConstraint() [2/4]

◆ MinimumDistanceConstraint() [3/4]

MinimumDistanceConstraint ( const multibody::MultibodyPlant< double > *const  plant,
double  minimum_distance,
systems::Context< double > *  plant_context,
MinimumDistancePenaltyFunction  penalty_function = {},
double  influence_distance_offset = 1 
)

Constructs a MinimumDistanceConstraint.

Parameters
plantThe multibody system on which the constraint will be evaluated.
minimum_distanceThe minimum allowed value, dₘᵢₙ, of the signed distance between any candidate pair of geometries.
penalty_functionThe penalty function formulation.
Default: QuadraticallySmoothedHinge
influence_distance_offsetThe difference (in meters) between the influence distance, d_influence, and the minimum distance, dₘᵢₙ (see class documentation). 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: 1 meter
Exceptions
std::invalid_argumentif plant has not registered its geometry with a SceneGraph object.
std::invalid_argumentif influence_distance_offset = ∞.
std::invalid_argumentif influence_distance_offset ≤ 0.

◆ MinimumDistanceConstraint() [4/4]

MinimumDistanceConstraint ( const multibody::MultibodyPlant< AutoDiffXd > *const  plant,
double  minimum_distance,
systems::Context< AutoDiffXd > *  plant_context,
MinimumDistancePenaltyFunction  penalty_function = {},
double  influence_distance_offset = 1 
)

Overloaded constructor.

Constructs the constraint using MultibodyPlant<AutoDiffXd>.

◆ ~MinimumDistanceConstraint()

Member Function Documentation

◆ influence_distance()

double influence_distance ( ) const

Getter for the influence distance.

◆ minimum_distance()

double minimum_distance ( ) const

Getter for the minimum distance.

◆ operator=() [1/2]

MinimumDistanceConstraint& operator= ( const MinimumDistanceConstraint )
delete

◆ operator=() [2/2]


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