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)

~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 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)

◆ MinimumDistanceConstraint() [1/4]

 MinimumDistanceConstraint ( const MinimumDistanceConstraint & )
delete

◆ MinimumDistanceConstraint() [2/4]

 MinimumDistanceConstraint ( MinimumDistanceConstraint && )
delete

◆ 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
 plant The multibody system on which the constraint will be evaluated. minimum_distance The minimum allowed value, dₘᵢₙ, of the signed distance between any candidate pair of geometries. 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, 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_argument if plant has not registered its geometry with a SceneGraph object. std::invalid_argument if influence_distance_offset = ∞. std::invalid_argument if 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 )

Constructs the constraint using MultibodyPlant<AutoDiffXd>.

◆ ~MinimumDistanceConstraint()

 ~MinimumDistanceConstraint ( )
override

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

 MinimumDistanceConstraint& operator= ( MinimumDistanceConstraint && )
delete

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