Implements a polyhedral convex set using the half-space representation: {x| A x ≤ b}
.
Note: This set may be unbounded.
By convention, we treat a zero-dimensional HPolyhedron as nonempty.
#include <drake/geometry/optimization/hpolyhedron.h>
Public Member Functions | |
HPolyhedron () | |
Constructs a default (zero-dimensional, nonempty) polyhedron. More... | |
HPolyhedron (const Eigen::Ref< const Eigen::MatrixXd > &A, const Eigen::Ref< const Eigen::VectorXd > &b) | |
Constructs the polyhedron. More... | |
HPolyhedron (const QueryObject< double > &query_object, GeometryId geometry_id, std::optional< FrameId > reference_frame=std::nullopt) | |
Constructs a new HPolyhedron from a SceneGraph geometry and pose in the reference_frame frame, obtained via the QueryObject. More... | |
HPolyhedron (const VPolytope &vpoly, double tol=1e-9) | |
Constructs a new HPolyedron from a VPolytope object. More... | |
HPolyhedron (const solvers::MathematicalProgram &prog) | |
Constructs a new HPolyhedron describing the feasible set of a linear program prog . More... | |
~HPolyhedron () final | |
const Eigen::MatrixXd & | A () const |
Returns the half-space representation matrix A. More... | |
const Eigen::VectorXd & | b () const |
Returns the half-space representation vector b. More... | |
bool | ContainedIn (const HPolyhedron &other, double tol=1E-9) const |
Returns true iff this HPolyhedron is entirely contained in the HPolyhedron other. More... | |
HPolyhedron | Intersection (const HPolyhedron &other, bool check_for_redundancy=false, double tol=1E-9) const |
Constructs the intersection of two HPolyhedron by adding the rows of inequalities from other . More... | |
std::set< int > | FindRedundant (double tol=1E-9) const |
Finds the redundant inequalities in this polyhedron. More... | |
HPolyhedron | ReduceInequalities (double tol=1E-9) const |
Reduces some (not necessarily all) redundant inequalities in the HPolyhedron. More... | |
HPolyhedron | SimplifyByIncrementalFaceTranslation (double min_volume_ratio=0.1, bool do_affine_transformation=true, int max_iterations=10, const Eigen::MatrixXd &points_to_contain=Eigen::MatrixXd(), const std::vector< drake::geometry::optimization::HPolyhedron > &intersecting_polytopes=std::vector< HPolyhedron >(), bool keep_whole_intersection=false, double intersection_padding=1e-4, int random_seed=0) const |
Returns an inner approximation of this , aiming to use fewer faces. More... | |
HPolyhedron | MaximumVolumeInscribedAffineTransformation (const HPolyhedron &circumbody) const |
Solves a semi-definite program to compute the maximum-volume affine transformation of this , subject to being a subset of circumbody , and subject to the transformation matrix being positive semi-definite. More... | |
Hyperellipsoid | MaximumVolumeInscribedEllipsoid () const |
Solves a semi-definite program to compute the inscribed ellipsoid. More... | |
Eigen::VectorXd | ChebyshevCenter () const |
Solves a linear program to compute the center of the largest inscribed ball in the polyhedron. More... | |
HPolyhedron | Scale (double scale, std::optional< Eigen::VectorXd > center=std::nullopt) const |
Results a new HPolyhedron that is a scaled version of this , by scaling the distance from each face to the center by a factor of pow(scale, 1/ambient_dimension()) , to have units of volume: More... | |
HPolyhedron | CartesianProduct (const HPolyhedron &other) const |
Returns the Cartesian product of this and other . More... | |
HPolyhedron | CartesianPower (int n) const |
Returns the n -ary Cartesian power of this . More... | |
HPolyhedron | PontryaginDifference (const HPolyhedron &other) const |
Returns the Pontryagin (Minkowski) Difference of this and other . More... | |
Eigen::VectorXd | UniformSample (RandomGenerator *generator, const Eigen::Ref< const Eigen::VectorXd > &previous_sample, int mixing_steps=10, const std::optional< Eigen::Ref< const Eigen::MatrixXd >> &subspace=std::nullopt, double tol=1e-8) const |
Draw an (approximately) uniform sample from the set using the hit and run Markov-chain Monte-Carlo strategy described in https://link.springer.com/article/10.1007/s101070050099. More... | |
Eigen::VectorXd | UniformSample (RandomGenerator *generator, int mixing_steps=10, const std::optional< Eigen::Ref< const Eigen::MatrixXd >> &subspace=std::nullopt, double tol=1e-8) const |
Variant of UniformSample that uses the ChebyshevCenter() as the previous_sample as a feasible point to start the Markov chain sampling. More... | |
template<typename Archive > | |
void | Serialize (Archive *a) |
Passes this object to an Archive. More... | |
bool | IsBounded (Parallelism parallelism=Parallelism::None()) const |
Returns true iff the set is bounded, e.g. More... | |
double | CalcVolume () const |
Implements CopyConstructible, CopyAssignable, MoveConstructible, MoveAssignable | |
HPolyhedron (const HPolyhedron &)=default | |
HPolyhedron & | operator= (const HPolyhedron &)=default |
HPolyhedron (HPolyhedron &&)=default | |
HPolyhedron & | operator= (HPolyhedron &&)=default |
Public Member Functions inherited from ConvexSet | |
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... | |
Static Public Member Functions | |
static HPolyhedron | MakeBox (const Eigen::Ref< const Eigen::VectorXd > &lb, const Eigen::Ref< const Eigen::VectorXd > &ub) |
Constructs a polyhedron as an axis-aligned box from the lower and upper corners. More... | |
static HPolyhedron | MakeUnitBox (int dim) |
Constructs the L∞-norm unit box in dim dimensions, {x | |x|∞ <= 1 }. More... | |
static HPolyhedron | MakeL1Ball (int dim) |
Constructs the L1-norm unit ball in dim dimensions, {x | |x|₁ <= 1 }. More... | |
Additional Inherited Members | |
Protected Member Functions inherited from ConvexSet | |
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::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 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 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... | |
ConvexSet (const ConvexSet &)=default | |
ConvexSet & | operator= (const ConvexSet &)=default |
ConvexSet (ConvexSet &&)=default | |
ConvexSet & | operator= (ConvexSet &&)=default |
Static Protected Member Functions inherited from ConvexSet | |
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... | |
|
default |
|
default |
HPolyhedron | ( | ) |
Constructs a default (zero-dimensional, nonempty) polyhedron.
HPolyhedron | ( | const Eigen::Ref< const Eigen::MatrixXd > & | A, |
const Eigen::Ref< const Eigen::VectorXd > & | b | ||
) |
Constructs the polyhedron.
HPolyhedron | ( | const QueryObject< double > & | query_object, |
GeometryId | geometry_id, | ||
std::optional< FrameId > | reference_frame = std::nullopt |
||
) |
Constructs a new HPolyhedron from a SceneGraph geometry and pose in the reference_frame
frame, obtained via the QueryObject.
If reference_frame
frame is std::nullopt, then it will be expressed in the world frame.
std::exception | the geometry is not a convex polytope. |
|
explicit |
Constructs a new HPolyedron from a VPolytope object.
This function will use qhull. If the VPolytope is empty, then the HPolyhedron will also be empty. If the HPolyhedron is not full-dimensional, we perform computations in a coordinate system of its affine hull. tol
specifies the numerical tolerance used in the computation of the affine hull. (See the documentation of AffineSubspace.) A tighter tolerance can be used with commercial solvers (e.g. Gurobi and Mosek).
std::exception | if vpoly is empty and zero dimensional. |
|
explicit |
Constructs a new HPolyhedron describing the feasible set of a linear program prog
.
The i
th dimension in this representation corresponds to the i
th decision variable of prog
. Note that if prog
is infeasible, then the constructed HPolyhedron will be empty.
std::exception | if prog has constraints which are not of type linear inequality, linear equality, or bounding box. |
|
final |
const Eigen::MatrixXd& A | ( | ) | const |
Returns the half-space representation matrix A.
const Eigen::VectorXd& b | ( | ) | const |
Returns the half-space representation vector b.
double CalcVolume |
Not | implemented. |
HPolyhedron CartesianPower | ( | int | n | ) | const |
Returns the n
-ary Cartesian power of this
.
The n-ary Cartesian power of a set H is the set H ⨉ H ⨉ ... ⨉ H, where H is repeated n times.
HPolyhedron CartesianProduct | ( | const HPolyhedron & | other | ) | const |
Returns the Cartesian product of this
and other
.
Eigen::VectorXd ChebyshevCenter | ( | ) | const |
Solves a linear program to compute the center of the largest inscribed ball in the polyhedron.
This is often the recommended way to find some interior point of the set, for example, as a step towards computing the convex hull or a vertex-representation of the set.
Note that the Chebyshev center is not necessarily unique, and may not conform to the point that one might consider the "visual center" of the set. For example, for a long thin rectangle, any point in the center line segment illustrated below would be a valid center point. The solver may return any point on that line segment.
┌──────────────────────────────────┐ │ │ │ ──────────────────────────── │ │ │ └──────────────────────────────────┘
To find the visual center, consider using the more expensive MaximumVolumeInscribedEllipsoid() method, and then taking the center of the returned Hyperellipsoid.
std::exception | if the solver fails to solve the problem. |
bool ContainedIn | ( | const HPolyhedron & | other, |
double | tol = 1E-9 |
||
) | const |
Returns true iff this HPolyhedron is entirely contained in the HPolyhedron other.
This is done by checking whether every inequality in other
is redundant when added to this.
tol | We check if this polyhedron is contained in other.A().row(i).dot(x) <= other.b()(i) + tol. The larger tol value is, the more relaxation we add to the containment. If tol is negative, then we check if a shrinked other contains this polyheron. |
Finds the redundant inequalities in this polyhedron.
Returns a set ℑ, such that if we remove the rows of A * x <= b in ℑ, the remaining inequalities still define the same polyhedron, namely {x | A*x<=b} = {x | A.row(i)*x<=b(i), ∀i ∉ ℑ}. This function solves a series of linear programs. We say the jᵗʰ row A.row(j)*x <= b(j) is redundant, if {x | A.row(i) * x <= b(i), ∀i ∉ ℑ} implies that A.row(j) * x <= b(j) + tol. Note that we do NOT guarantee that we find all the redundant rows.
HPolyhedron Intersection | ( | const HPolyhedron & | other, |
bool | check_for_redundancy = false , |
||
double | tol = 1E-9 |
||
) | const |
Constructs the intersection of two HPolyhedron by adding the rows of inequalities from other
.
If check_for_redundancy
is true then only adds the rows of other
other.A().row(i).dot(x)<=other.b()(i) to this HPolyhedron if the inequality other.A().row(i).dot(x)<=other.b()(i)+tol is not implied by the inequalities from this HPolyhedron. A positive tol means it is more likely to deem a constraint being redundant and remove it. A negative tol means it is less likely to remove a constraint.
bool IsBounded |
Returns true iff the set is bounded, e.g.
there exists an element-wise finite lower and upper bound for the set. For HPolyhedron, while there are some fast checks to confirm a set is unbounded, confirming boundedness requires solving a linear program (based on Stiemke’s theorem of alternatives).
parallelism | Ignored – the linear program solver will determine the number of threads to use. |
|
static |
Constructs a polyhedron as an axis-aligned box from the lower and upper corners.
|
static |
Constructs the L1-norm unit ball in dim
dimensions, {x | |x|₁ <= 1 }.
This set is also known as the cross-polytope and is described by the 2ᵈⁱᵐ signed unit vectors.
|
static |
Constructs the L∞-norm unit box in dim
dimensions, {x | |x|∞ <= 1 }.
This is an axis-aligned box, centered at the origin, with edge length 2.
HPolyhedron MaximumVolumeInscribedAffineTransformation | ( | const HPolyhedron & | circumbody | ) | const |
Solves a semi-definite program to compute the maximum-volume affine transformation of this
, subject to being a subset of circumbody
, and subject to the transformation matrix being positive semi-definite.
The latter condition is necessary for convexity of the program. We use the containment condition stated in Lemma 1 of "Linear
Encodings for Polytope Containment Problems" by Sadra Sadraddini and Russ Tedrake, extended to apply to the affine transformation of this
. We solve
max_{T,t} log det (T) s.t. T ≽ 0 t + TX ⊆ Y
where X is this
, and Y is circumbody
.
circumbody | is an HPolyhedron that must contain the returned inbody. |
std::exception | if the solver fails to solve the problem. |
Hyperellipsoid MaximumVolumeInscribedEllipsoid | ( | ) | const |
Solves a semi-definite program to compute the inscribed ellipsoid.
This is also known as the inner Löwner-John ellipsoid. From Section 8.4.2 in Boyd and Vandenberghe, 2004, we solve
max_{C,d} log det (C) s.t. |aᵢC|₂ ≤ bᵢ - aᵢd, ∀i C ≽ 0
where aᵢ and bᵢ denote the ith row. This defines the ellipsoid E = { Cx + d | |x|₂ ≤ 1}.
std::exception | if the solver fails to solve the problem. |
|
default |
|
default |
HPolyhedron PontryaginDifference | ( | const HPolyhedron & | other | ) | const |
Returns the Pontryagin (Minkowski) Difference of this
and other
.
This is the set A ⊖ B = { a|a+ B ⊆ A }. The result is an HPolyhedron with the same number of inequalities as A. Requires that this
and other
both be bounded and have the same ambient dimension. This method may throw a runtime error if this
or other
are ill-conditioned.
HPolyhedron ReduceInequalities | ( | double | tol = 1E-9 | ) | const |
Reduces some (not necessarily all) redundant inequalities in the HPolyhedron.
This is not guaranteed to give the minimal representation of the polyhedron but is a relatively fast way to reduce the number of inequalities.
tol | For a constraint c'x<=d, if the halfspace c'x<=d + tol contains the hpolyhedron generated by the rest of the constraints, then we remove this inequality. A positive tol means it is more likely to remove a constraint, a negative tol means it is less likely to remote a constraint. |
HPolyhedron Scale | ( | double | scale, |
std::optional< Eigen::VectorXd > | center = std::nullopt |
||
) | const |
Results a new HPolyhedron that is a scaled version of this
, by scaling the distance from each face to the center
by a factor of pow(scale, 1/ambient_dimension())
, to have units of volume:
scale = 0
will result in a point,0 < scale < 1
shrinks the region,scale = 1
returns a copy of the this
, and1 < scale
grows the region.If center
is not provided, then the value returned by ChebyshevCenter() will be used.
this
does not need to be bounded, nor have volume. center
does not need to be in the set.
scale
>= 0. center
has size equal to the ambient dimension. void Serialize | ( | Archive * | a | ) |
Passes this object to an Archive.
Refer to YAML Serialization for background.
HPolyhedron SimplifyByIncrementalFaceTranslation | ( | double | min_volume_ratio = 0.1 , |
bool | do_affine_transformation = true , |
||
int | max_iterations = 10 , |
||
const Eigen::MatrixXd & | points_to_contain = Eigen::MatrixXd() , |
||
const std::vector< drake::geometry::optimization::HPolyhedron > & | intersecting_polytopes = std::vector< HPolyhedron >() , |
||
bool | keep_whole_intersection = false , |
||
double | intersection_padding = 1e-4 , |
||
int | random_seed = 0 |
||
) | const |
Returns an inner approximation of this
, aiming to use fewer faces.
Proceeds by incrementally translating faces inward and removing other faces that become redundant upon doing so.
min_volume_ratio | is a lower bound for the ratio of the volume of the returned inbody and the volume of this . |
do_affine_transformation | specifies whether to call MaximumVolumeInscribedAffineTransformation(), to take an affine transformation of the inner approximation to maximize its volume. The affine transformation is reverted if the resulting inner approximation violates conditions related to points_to_contain or intersecting_polytopes . |
max_iterations | is the maximum number of times to loop through all faces. |
points_to_contain | is an optional matrix whose columns are points that must be contained in the returned inbody. |
intersecting_polytopes | is an optional list of HPolyhedrons that must intersect with the returned inbody. |
keep_whole_intersection | specifies whether the face translation step of the algorithm is prohibited from reducing the intersections with the HPolyhedrons in intersecting_polytopes . Regardless of the value of this parameter, the intersections may be reduced by the affine transformation step if do_affine_transformation is true. |
intersection_padding | is a distance by which each hyperplane is translated back outward after satisfing intersection constraints, subject to not surpassing the original hyperplane position. In the case where keep_whole_intersection is false, using a non-zero value for this parameter prevents intersections from being single points. |
random_seed | is a seed for a random number generator used to shuffle the ordering of hyperplanes in between iterations. |
min_volume_ratio
> 0. max_iterations
> 0. intersection_padding
>= 0. points_to_contain
are points contained within this
. intersecting_polytopes
intersect with this
. Eigen::VectorXd UniformSample | ( | RandomGenerator * | generator, |
const Eigen::Ref< const Eigen::VectorXd > & | previous_sample, | ||
int | mixing_steps = 10 , |
||
const std::optional< Eigen::Ref< const Eigen::MatrixXd >> & | subspace = std::nullopt , |
||
double | tol = 1e-8 |
||
) | const |
Draw an (approximately) uniform sample from the set using the hit and run Markov-chain Monte-Carlo strategy described in https://link.springer.com/article/10.1007/s101070050099.
To draw many samples from the uniform distribution, pass the output of one iteration in as the previous_sample
to the next, with mixing_steps
set to a relatively low number. When drawing a single sample, mixing_steps
should be set relatively high in order to obtain an approximately uniformly random point. The distribution of samples will converge to the true uniform distribution at a geometric rate in the total number of hit-and-run steps which is mixing_steps
* the number of times this function is called. If a subspace
is provided, the random samples are constrained to lie in the affine subspace through previous_sample
, spanned by the columns of subspace
. To obtain uniform samples, subspace should have orthonormal, columns. This enables drawing uniform samples from an HPolyhedron which is not full-dimensional – one can pass the basis of the affine hull of the HPolyhedron, which can be computed with the AffineSubspace class. tol
is a numerical tolerance for checking if any halfspaces in the given HPolyhedron are implied by the subspace
definition (and therefore can be ignored by the hit-and-run sampler).
std::exception | if previous_sample is not in the set. |
Eigen::VectorXd UniformSample | ( | RandomGenerator * | generator, |
int | mixing_steps = 10 , |
||
const std::optional< Eigen::Ref< const Eigen::MatrixXd >> & | subspace = std::nullopt , |
||
double | tol = 1e-8 |
||
) | const |
Variant of UniformSample that uses the ChebyshevCenter() as the previous_sample as a feasible point to start the Markov chain sampling.