Detailed Description

Constrain lb <= min(d) <= ub, 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, and the minimal distance is 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

SmoothOverMax( φ((dᵢ(q) - d_influence)/(d_influence - lb)) / φ(-1) ) ≤ 1
SmoothUnderMax( φ((dᵢ(q) - d_influence)/(d_influence - ub)) / φ(-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) and SmoothUnderMax(d) is smooth over and under 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_constraint.h>

Public Member Functions
	MinimumDistanceConstraint (const multibody::MultibodyPlant< double > const plant, double minimum_distance, systems::Context< double > plant_context, solvers::MinimumValuePenaltyFunction penalty_function={}, double influence_distance_offset=1)
	Constructs a MinimumDistanceConstraint. More...

	MinimumDistanceConstraint (const multibody::MultibodyPlant< double > const plant, double minimum_distance_lower, double minimum_distance_upper, systems::Context< double > plant_context, solvers::MinimumValuePenaltyFunction penalty_function, double influence_distance)
	Overloaded constructor. More...

	MinimumDistanceConstraint (const multibody::MultibodyPlant< AutoDiffXd > const plant, double minimum_distance, systems::Context< AutoDiffXd > plant_context, solvers::MinimumValuePenaltyFunction penalty_function={}, double influence_distance_offset=1)
	Overloaded constructor. More...

	MinimumDistanceConstraint (const multibody::MultibodyPlant< AutoDiffXd > const plant, double minimum_distance_lower, double minimum_distance_upper, systems::Context< AutoDiffXd > plant_context, solvers::MinimumValuePenaltyFunction penalty_function, double influence_distance)
	Overloaded constructor. More...

	MinimumDistanceConstraint (const planning::CollisionChecker collision_checker, double minimum_distance, planning::CollisionCheckerContext collision_checker_context, solvers::MinimumValuePenaltyFunction penalty_function={}, double influence_distance_offset=1)
	Overloaded constructor. More...

	MinimumDistanceConstraint (const planning::CollisionChecker collision_checker, double minimum_distance_lower, double minimum_distance_upper, planning::CollisionCheckerContext collision_checker_context, solvers::MinimumValuePenaltyFunction penalty_function, double influence_distance)
	Overloaded constructor. More...

	~MinimumDistanceConstraint () override

double	minimum_distance_lower () const
	Getter for the lower bound of the minimum distance. More...

double	minimum_distance_upper () 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
	MinimumDistanceConstraint (const MinimumDistanceConstraint &)=delete

MinimumDistanceConstraint &	operator= (const MinimumDistanceConstraint &)=delete

	MinimumDistanceConstraint (MinimumDistanceConstraint &&)=delete

MinimumDistanceConstraint &	operator= (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

Constraint &	operator= (const Constraint &)=delete

	Constraint (Constraint &&)=delete

Constraint &	operator= (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

EvaluatorBase &	operator= (const EvaluatorBase &)=delete

	EvaluatorBase (EvaluatorBase &&)=delete

EvaluatorBase &	operator= (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)

Constructor & Destructor Documentation

◆ MinimumDistanceConstraint() [1/8]

MinimumDistanceConstraint ( const MinimumDistanceConstraint & )

delete

◆ MinimumDistanceConstraint() [2/8]

MinimumDistanceConstraint ( MinimumDistanceConstraint && )

delete

◆ MinimumDistanceConstraint() [3/8]

MinimumDistanceConstraint	(	const multibody::MultibodyPlant< double > *const	plant,
		double	minimum_distance,
		systems::Context< double > *	plant_context,
		solvers::MinimumValuePenaltyFunction	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, lb, 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, lb (see class documentation), namely influence_distance = minimum_distance_lower + 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: 1 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::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.

◆ MinimumDistanceConstraint() [4/8]

MinimumDistanceConstraint	(	const multibody::MultibodyPlant< double > *const	plant,
		double	minimum_distance_lower,
		double	minimum_distance_upper,
		systems::Context< double > *	plant_context,
		solvers::MinimumValuePenaltyFunction	penalty_function,
		double	influence_distance
	)

Overloaded constructor.

lower <= min(distance) <= upper.

Parameters

minimum_distance_lower	The lower bound of the minimum distance. lower <= min(distance).
minimum_distance_upper	The upper bound of the minimum distance. min(distance) <= upper. If minimum_distance_upper is finite, then it must be smaller than influence_distance_offset.

◆ MinimumDistanceConstraint() [5/8]

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

Overloaded constructor.

Constructs the constraint using MultibodyPlant<AutoDiffXd>.

◆ MinimumDistanceConstraint() [6/8]

MinimumDistanceConstraint	(	const multibody::MultibodyPlant< AutoDiffXd > *const	plant,
		double	minimum_distance_lower,
		double	minimum_distance_upper,
		systems::Context< AutoDiffXd > *	plant_context,
		solvers::MinimumValuePenaltyFunction	penalty_function,
		double	influence_distance
	)

Overloaded constructor.

Constructs the constraint using MultibodyPlant<AutoDiffXd>. lower <= min(distance) <= upper.

Parameters

minimum_distance_lower	The lower bound of the minimum distance. lower <= min(distance). We must have minimum_distance_lower <= influence_distance.
minimum_distance_upper	The upper bound of the minimum distance. min(distance) <= upper. If minimum_distance_upper is finite, then it must be smaller than influence_distance.
collision_checker_context	The context for the collision checker.

◆ MinimumDistanceConstraint() [7/8]

MinimumDistanceConstraint	(	const planning::CollisionChecker *	collision_checker,
		double	minimum_distance,
		planning::CollisionCheckerContext *	collision_checker_context,
		solvers::MinimumValuePenaltyFunction	penalty_function = `{}`,
		double	influence_distance_offset = `1`
	)

Overloaded constructor.

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.

◆ MinimumDistanceConstraint() [8/8]

MinimumDistanceConstraint	(	const planning::CollisionChecker *	collision_checker,
		double	minimum_distance_lower,
		double	minimum_distance_upper,
		planning::CollisionCheckerContext *	collision_checker_context,
		solvers::MinimumValuePenaltyFunction	penalty_function,
		double	influence_distance
	)

Overloaded constructor.

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.

◆ ~MinimumDistanceConstraint()

~MinimumDistanceConstraint ( )

override

Member Function Documentation

◆ influence_distance()

double influence_distance ( ) const

Getter for the influence distance.

◆ minimum_distance_lower()

double minimum_distance_lower ( ) const

Getter for the lower bound of the minimum distance.

◆ minimum_distance_upper()

double minimum_distance_upper ( ) const

Getter for the upper bound of 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 file:

drake/multibody/inverse_kinematics/minimum_distance_constraint.h

Detailed Description

Public Member Functions

Additional Inherited Members

Constructor & Destructor Documentation

◆ MinimumDistanceConstraint() [1/8]

◆ MinimumDistanceConstraint() [2/8]

◆ MinimumDistanceConstraint() [3/8]

◆ MinimumDistanceConstraint() [4/8]

◆ MinimumDistanceConstraint() [5/8]

◆ MinimumDistanceConstraint() [6/8]

◆ MinimumDistanceConstraint() [7/8]

◆ MinimumDistanceConstraint() [8/8]

◆ ~MinimumDistanceConstraint()

Member Function Documentation

◆ influence_distance()

◆ minimum_distance_lower()

◆ minimum_distance_upper()

◆ operator=() [1/2]

◆ operator=() [2/2]