Abstract base class for defining a convex set.
#include <drake/geometry/optimization/convex_set.h>
Public Member Functions | |
virtual | ~ConvexSet () |
std::unique_ptr< ConvexSet > | Clone () const |
Creates a unique deep copy of this set. More... | |
int | ambient_dimension () const |
Returns the dimension of the vector space in which the elements of this set are evaluated. More... | |
bool | IntersectsWith (const ConvexSet &other) const |
Returns true iff the intersection between this and other is non-empty. More... | |
bool | IsBounded (Parallelism parallelism=Parallelism::None()) const |
Returns true iff the set is bounded, e.g., there exists an element-wise finite lower and upper bound for the set. More... | |
bool | IsEmpty () const |
Returns true iff the set is empty. More... | |
std::optional< Eigen::VectorXd > | MaybeGetPoint () const |
If this set trivially contains exactly one point, returns the value of that point. More... | |
std::optional< Eigen::VectorXd > | MaybeGetFeasiblePoint () const |
Returns a feasible point within this convex set if it is nonempty, and nullopt otherwise. More... | |
bool | PointInSet (const Eigen::Ref< const Eigen::VectorXd > &x, double tol=0) const |
Returns true iff the point x is contained in the set. More... | |
std::pair< VectorX< symbolic::Variable >, std::vector< solvers::Binding< solvers::Constraint > > > | AddPointInSetConstraints (solvers::MathematicalProgram *prog, const Eigen::Ref< const solvers::VectorXDecisionVariable > &vars) const |
Adds a constraint to an existing MathematicalProgram enforcing that the point defined by vars is inside the set. More... | |
std::vector< solvers::Binding< solvers::Constraint > > | AddPointInNonnegativeScalingConstraints (solvers::MathematicalProgram *prog, const Eigen::Ref< const solvers::VectorXDecisionVariable > &x, const symbolic::Variable &t) const |
Let S be this convex set. More... | |
std::vector< solvers::Binding< solvers::Constraint > > | AddPointInNonnegativeScalingConstraints (solvers::MathematicalProgram *prog, const Eigen::Ref< const Eigen::MatrixXd > &A, const Eigen::Ref< const Eigen::VectorXd > &b, const Eigen::Ref< const Eigen::VectorXd > &c, double d, const Eigen::Ref< const solvers::VectorXDecisionVariable > &x, const Eigen::Ref< const solvers::VectorXDecisionVariable > &t) const |
Let S be this convex set. More... | |
std::pair< std::unique_ptr< Shape >, math::RigidTransformd > | ToShapeWithPose () const |
Constructs a Shape and a pose of the set in the world frame for use in the SceneGraph geometry ecosystem. More... | |
double | CalcVolume () const |
Computes the exact volume for the convex set. More... | |
SampledVolume | CalcVolumeViaSampling (RandomGenerator *generator, const double desired_rel_accuracy=1e-2, const int max_num_samples=1e4) const |
Calculates an estimate of the volume of the convex set using sampling and performing Monte Carlo integration. More... | |
std::optional< std::pair< std::vector< double >, Eigen::MatrixXd > > | Projection (const Eigen::Ref< const Eigen::MatrixXd > &points) const |
Computes in the L₂ norm the distance and the nearest point in this convex set to every column of points . More... | |
bool | has_exact_volume () const |
Returns true if the exact volume can be computed for this convex set instance. More... | |
Protected Member Functions | |
ConvexSet (int ambient_dimension, bool has_exact_volume) | |
For use by derived classes to construct a ConvexSet. More... | |
template<typename Archive > | |
void | Serialize (Archive *a) |
Implements non-virtual base class serialization. More... | |
virtual std::unique_ptr< ConvexSet > | DoClone () const =0 |
Non-virtual interface implementation for Clone(). More... | |
virtual std::optional< bool > | DoIsBoundedShortcut () const |
Non-virtual interface implementation for DoIsBoundedShortcut(). More... | |
virtual std::optional< bool > | DoIsBoundedShortcutParallel (Parallelism) const |
Non-virtual interface implementation for DoIsBoundedShortcutParallel(). More... | |
virtual std::vector< std::optional< double > > | DoProjectionShortcut (const Eigen::Ref< const Eigen::MatrixXd > &points, EigenPtr< Eigen::MatrixXd > projected_points) const |
Non-virtual interface implementation for DoProjectionShortcut(). More... | |
virtual bool | DoIsEmpty () const |
Non-virtual interface implementation for IsEmpty(). More... | |
virtual std::optional< Eigen::VectorXd > | DoMaybeGetPoint () const |
Non-virtual interface implementation for MaybeGetPoint(). More... | |
virtual std::optional< Eigen::VectorXd > | DoMaybeGetFeasiblePoint () const |
Non-virtual interface implementation for MaybeGetFeasiblePoint(). More... | |
virtual bool | DoPointInSet (const Eigen::Ref< const Eigen::VectorXd > &x, double tol) const |
Non-virtual interface implementation for PointInSet(). More... | |
virtual std::optional< bool > | DoPointInSetShortcut (const Eigen::Ref< const Eigen::VectorXd > &x, double tol) const |
A non-virtual interface implementation for PointInSet() that should be used when the PointInSet() can be computed more efficiently than solving a convex program. More... | |
virtual std::pair< VectorX< symbolic::Variable >, std::vector< solvers::Binding< solvers::Constraint > > > | DoAddPointInSetConstraints (solvers::MathematicalProgram *prog, const Eigen::Ref< const solvers::VectorXDecisionVariable > &vars) const =0 |
Non-virtual interface implementation for AddPointInSetConstraints(). More... | |
virtual std::vector< solvers::Binding< solvers::Constraint > > | DoAddPointInNonnegativeScalingConstraints (solvers::MathematicalProgram *prog, const Eigen::Ref< const solvers::VectorXDecisionVariable > &x, const symbolic::Variable &t) const =0 |
Non-virtual interface implementation for AddPointInNonnegativeScalingConstraints(). More... | |
virtual std::vector< solvers::Binding< solvers::Constraint > > | DoAddPointInNonnegativeScalingConstraints (solvers::MathematicalProgram *prog, const Eigen::Ref< const Eigen::MatrixXd > &A, const Eigen::Ref< const Eigen::VectorXd > &b, const Eigen::Ref< const Eigen::VectorXd > &c, double d, const Eigen::Ref< const solvers::VectorXDecisionVariable > &x, const Eigen::Ref< const solvers::VectorXDecisionVariable > &t) const =0 |
Non-virtual interface implementation for AddPointInNonnegativeScalingConstraints(). More... | |
virtual std::pair< std::unique_ptr< Shape >, math::RigidTransformd > | DoToShapeWithPose () const =0 |
Non-virtual interface implementation for ToShapeWithPose(). More... | |
virtual double | DoCalcVolume () const |
Non-virtual interface implementation for CalcVolume(). More... | |
std::optional< symbolic::Variable > | HandleZeroAmbientDimensionConstraints (solvers::MathematicalProgram *prog, const ConvexSet &set, std::vector< solvers::Binding< solvers::Constraint >> *constraints) const |
Instances of subclasses such as CartesianProduct and MinkowskiSum can have constituent sets with zero ambient dimension, which much be handled in a special manner when calling methods such as DoAddPointInSetConstraints. More... | |
virtual std::unique_ptr< ConvexSet > | DoAffineHullShortcut (std::optional< double > tol) const |
NVI implementation of DoAffineHullShortcut, which trivially returns null. More... | |
Implements CopyConstructible, CopyAssignable, MoveConstructible, MoveAssignable | |
ConvexSet (const ConvexSet &)=default | |
ConvexSet & | operator= (const ConvexSet &)=default |
ConvexSet (ConvexSet &&)=default | |
ConvexSet & | operator= (ConvexSet &&)=default |
Static Protected Member Functions | |
static std::unique_ptr< ConvexSet > | AffineHullShortcut (const ConvexSet &self, std::optional< double > tol) |
When there is a more efficient strategy to compute the affine hull of this set, returns affine hull as an AffineSubspace. More... | |
|
virtual |
For use by derived classes to construct a ConvexSet.
has_exact_volume | Derived classes should pass true if they've implemented DoCalcVolume() to return a value (at least sometimes). |
std::vector<solvers::Binding<solvers::Constraint> > AddPointInNonnegativeScalingConstraints | ( | solvers::MathematicalProgram * | prog, |
const Eigen::Ref< const solvers::VectorXDecisionVariable > & | x, | ||
const symbolic::Variable & | t | ||
) | const |
Let S be this convex set.
When S is bounded, this method adds the convex constraints to imply
x ∈ t S, t ≥ 0,
where x is a point in ℜⁿ (with n the ambient_dimension) and t is a scalar.
When S is unbounded, then the behavior is almost identical, except when t=0. In this case, the constraints imply t ≥ 0, x ∈ t S ⊕ rec(S), where rec(S) is the recession cone of S (the asymptotic directions in which S is not bounded) and ⊕ is the Minkowski sum. For t > 0, this is equivalent to x ∈ t S, but for t = 0, we have only x ∈ rec(S).
std::exception | if ambient_dimension() == 0 |
std::vector<solvers::Binding<solvers::Constraint> > AddPointInNonnegativeScalingConstraints | ( | solvers::MathematicalProgram * | prog, |
const Eigen::Ref< const Eigen::MatrixXd > & | A, | ||
const Eigen::Ref< const Eigen::VectorXd > & | b, | ||
const Eigen::Ref< const Eigen::VectorXd > & | c, | ||
double | d, | ||
const Eigen::Ref< const solvers::VectorXDecisionVariable > & | x, | ||
const Eigen::Ref< const solvers::VectorXDecisionVariable > & | t | ||
) | const |
Let S be this convex set.
When S is bounded, this method adds the convex constraints to imply
A * x + b ∈ (c' * t + d) S, c' * t + d ≥ 0,
where A is an n-by-m matrix (with n the ambient_dimension), b is a vector of size n, c is a vector of size p, x is a point in ℜᵐ, and t is a point in ℜᵖ.
When S is unbounded, then the behavior is almost identical, except when c' * t+d=0. In this case, the constraints imply
A * x + b ∈ (c' * t + d) S ⊕ rec(S), c' * t + d ≥ 0,
where rec(S) is the recession cone of S (the asymptotic directions in which S is not bounded) and ⊕ is the Minkowski sum. For c' * t + d > 0, this is equivalent to A * x + b ∈ (c' * t + d) S, but for c' * t + d = 0, we have only A * x + b ∈ rec(S).
std::exception | if ambient_dimension() == 0 |
std::pair<VectorX<symbolic::Variable>, std::vector<solvers::Binding<solvers::Constraint> > > AddPointInSetConstraints | ( | solvers::MathematicalProgram * | prog, |
const Eigen::Ref< const solvers::VectorXDecisionVariable > & | vars | ||
) | const |
Adds a constraint to an existing MathematicalProgram enforcing that the point defined by vars is inside the set.
prog
through this function. std::exception | if ambient_dimension() == 0 |
|
staticprotected |
When there is a more efficient strategy to compute the affine hull of this set, returns affine hull as an AffineSubspace.
When no efficient conversion exists, returns null. The default base class implementation returns null. This method is used by the AffineSubspace constructor to short-circuit the generic iterative approach. (This function is static to allow calling it from the AffineSubspace constructor, but is conceptially a normal member function.) The return type is ConvexSet to avoid a forward declaration; any non-null result must always have the AffineSubspace as its runtime type.
int ambient_dimension | ( | ) | const |
Returns the dimension of the vector space in which the elements of this set are evaluated.
Contrast this with the affine dimension
: the dimension of the smallest affine subset of the ambient space that contains our set. For example, if we define a set using A*x = b
, where A
has linearly independent rows, then the ambient dimension is the dimension of x
, but the affine dimension of the set is ambient_dimension() - rank(A)
.
double CalcVolume | ( | ) | const |
Computes the exact volume for the convex set.
std::exception | if has_exact_volume() returns false . |
if | ambient_dimension() == 0. |
SampledVolume CalcVolumeViaSampling | ( | RandomGenerator * | generator, |
const double | desired_rel_accuracy = 1e-2 , |
||
const int | max_num_samples = 1e4 |
||
) | const |
Calculates an estimate of the volume of the convex set using sampling and performing Monte Carlo integration.
generator | a random number generator. |
desired_rel_accuracy | the desired relative accuracy of the volume estimate in the sense that the estimated volume is likely to be within the interval defined by (1±2*desired_rel_accuracy)*true_volume with probability of at least* 0.95 according to the Law of Large Numbers. https://people.math.umass.edu/~lr7q/ps_files/teaching/math456/Chapter6.pdf The computation will terminate when the relative error is less than rel_accuracy or when the maximum number of samples is reached. |
max_num_samples | the maximum number of samples to use. |
desired_rel_accuracy
is in the range [0,1]. if | ambient_dimension() == 0. |
if | the minimum axis-aligned bounding box of the set cannot be computed. |
std::unique_ptr<ConvexSet> Clone | ( | ) | const |
Creates a unique deep copy of this set.
|
protectedpure virtual |
Non-virtual interface implementation for AddPointInNonnegativeScalingConstraints().
|
protectedpure virtual |
Non-virtual interface implementation for AddPointInNonnegativeScalingConstraints().
Subclasses must override to add the constraints needed to keep the point A * x + b in the non-negative scaling of the set. Note that subclasses do not need to add the constraint c * t + d ≥ 0 as it is already added.
|
protectedpure virtual |
Non-virtual interface implementation for AddPointInSetConstraints().
|
protectedvirtual |
NVI implementation of DoAffineHullShortcut, which trivially returns null.
Derived classes that have efficient algorithms should override this method.
|
protectedvirtual |
Non-virtual interface implementation for CalcVolume().
This will only be called if has_exact_volume() returns true and ambient_dimension() > 0
|
protectedpure virtual |
Non-virtual interface implementation for Clone().
|
protectedvirtual |
Non-virtual interface implementation for DoIsBoundedShortcut().
Trivially returns std::nullopt. This allows a derived class to implement its own boundedness checks, to potentially avoid the more expensive base class checks.
|
protectedvirtual |
Non-virtual interface implementation for DoIsBoundedShortcutParallel().
Trivially returns std::nullopt. This allows a derived class to implement its own boundedness checks that leverage parallelization, to potentially avoid the more expensive base class checks.
|
protectedvirtual |
Non-virtual interface implementation for IsEmpty().
The default implementation solves a feasibility optimization problem, but derived classes can override with a custom (more efficient) implementation. Zero-dimensional sets are considered to be nonempty by default. Sets which can be zero-dimensional and empty must handle this behavior in their derived implementation of DoIsEmpty.
|
protectedvirtual |
Non-virtual interface implementation for MaybeGetFeasiblePoint().
The default implementation solves a feasibility optimization problem, but derived classes can override with a custom (more efficient) implementation.
|
protectedvirtual |
Non-virtual interface implementation for MaybeGetPoint().
The default implementation returns nullopt. Sets that can model a single point should override with a custom implementation.
|
protectedvirtual |
Non-virtual interface implementation for PointInSet().
|
protectedvirtual |
A non-virtual interface implementation for PointInSet() that should be used when the PointInSet() can be computed more efficiently than solving a convex program.
For example, membership in a VPolytope cannot be verified without solving a linear program and so no shortcut implementation should be provided. On the other hand, membership in an HPolyhedron can be checked by checking the inequality Ax ≤ b and so a shortcut is possible.
|
protectedvirtual |
Non-virtual interface implementation for DoProjectionShortcut().
This allows a derived class to implement a method which computes the projection of some, but not necessarily all, of the points
more efficiently than the generic implementation.
The default implementation checks whether each column of points
is in the set using DoPointInSetShortcut. Points in the set are given a distance of 0 and are projected to themselves.
[in] | points | are the points which we wish to project to the convex set. |
[in,out] | projected_points | are the projection of points onto the convex set. |
distances
which is the same size as points.cols()
.These are the distances from points
to the convex set. If distances[i] has a value, then projected_points->col(i) is the projection of points.col(i) onto the set. If distances[i] is nullopt, then the projection of points.col(i) has not yet been computed, and so the value at projected_points->col(i) is meaningless.
|
protectedpure virtual |
Non-virtual interface implementation for ToShapeWithPose().
|
protected |
Instances of subclasses such as CartesianProduct and MinkowskiSum can have constituent sets with zero ambient dimension, which much be handled in a special manner when calling methods such as DoAddPointInSetConstraints.
If the set is empty, a trivially infeasible constraint must be added. We also warn the user when this happens, since they probably didn't intend it to occur. If the set is nonempty, then it's the unique zero-dimensional vector space {0}, and no additional variables or constraints are needed. If a new variable is created, return it, to optionally be stored (as in AddPointInSetConstraints), or not be stored (as in DoAddPointInNonnegativeScalingConstraints).
bool has_exact_volume | ( | ) | const |
Returns true if the exact volume can be computed for this convex set instance.
bool IntersectsWith | ( | const ConvexSet & | other | ) | const |
Returns true iff the intersection between this
and other
is non-empty.
std::exception | if the ambient dimension of other is not the same as that of this . |
bool IsBounded | ( | Parallelism | parallelism = Parallelism::None() | ) | const |
Returns true iff the set is bounded, e.g., there exists an element-wise finite lower and upper bound for the set.
Note: for some derived classes, this check is trivial, but for others it can require solving a number of (typically small) optimization problems. Each derived class documents the cost of its boundedness test and whether it honors the request for parallelism. (Derived classes which do not have a specialized check will, by default, honor parallelism requests.) Note that the overhead of multithreading may lead to slower runtimes for simple, low-dimensional sets, but can enable major speedups for more challenging problems.
parallelism | requests the number of cores to use when solving mathematical programs to check boundedness, subject to whether a particular derived class honors parallelism. |
bool IsEmpty | ( | ) | const |
Returns true iff the set is empty.
Note: for some derived classes, this check is trivial, but for others, it can require solving a (typically small) optimization problem. Check the derived class documentation for any notes. Zero-dimensional sets must be handled specially. There are two possible sets in a zero-dimensional space – the empty set, and the whole set (which is simply the "zero vector space", {0}.) For more details, see: https://en.wikipedia.org/wiki/Examples_of_vector_spaces#Trivial_or_zero_vector_space Zero-dimensional sets are considered to be nonempty by default. Sets which can be zero-dimensional and empty must handle this behavior in their derived implementation of DoIsEmpty. An example of such a subclass is VPolytope.
std::optional<Eigen::VectorXd> MaybeGetFeasiblePoint | ( | ) | const |
Returns a feasible point within this convex set if it is nonempty, and nullopt otherwise.
std::optional<Eigen::VectorXd> MaybeGetPoint | ( | ) | const |
If this set trivially contains exactly one point, returns the value of that point.
Otherwise, returns nullopt. By "trivially", we mean that representation of the set structurally maps to a single point; if checking for point-ness would require solving an optimization program, returns nullopt. In other words, this is a relatively cheap function to call.
bool PointInSet | ( | const Eigen::Ref< const Eigen::VectorXd > & | x, |
double | tol = 0 |
||
) | const |
Returns true iff the point x is contained in the set.
If the ambient dimension is zero, then if the set is nonempty, the point is trivially in the set, and if the set is empty, the point is trivially not in the set.
std::optional<std::pair<std::vector<double>, Eigen::MatrixXd> > Projection | ( | const Eigen::Ref< const Eigen::MatrixXd > & | points | ) | const |
Computes in the L₂ norm the distance and the nearest point in this convex set to every column of points
.
If this set is empty, we return nullopt.
if | the internal convex optimization solver fails. |
|
protected |
Implements non-virtual base class serialization.
std::pair<std::unique_ptr<Shape>, math::RigidTransformd> ToShapeWithPose | ( | ) | const |
Constructs a Shape and a pose of the set in the world frame for use in the SceneGraph geometry ecosystem.
std::exception | if ambient_dimension() != 3 or if the functionality for a particular set has not been implemented yet. |