Provides an abstraction for reasoning about geometry in optimization problems, and using optimization problems to solve geometry problems.
SceneGraph handles many types of geometry (see Shape). It is specialized to 3D and puts a strong emphasis on efficient implementation of a particular subset of geometry queries, like collision detection and signed-distance queries. SceneGraph also provides a lot of valuable tools for content management, including parsing geometries from file (via multibody::Parser) and Role.
MathematicalProgram has many relevant solvers::Cost / solvers::Constraint for reasoning about geometry (e.g., the LorentzCone or even LinearConstraint). The class and methods in this group add a level of modeling power above these individual constraints (there are many different types of constraints one would write given various optimization on these sets).
The geometry::optimization tools support:
Modules | |
Geodesic Convexity | |
A robot that has revolute joints without any limits has an inherently non-Euclidean configuration space, but one can still consider "geodesically-convex" sets, akin to convex sets in Euclidean space. | |
Classes | |
class | AffineBall |
Implements an ellipsoidal convex set represented as an affine scaling of the unit ball {Bu + center | |u|₂ ≤ 1}. More... | |
class | AffineSubspace |
An affine subspace (also known as a "flat", a "linear variety", or a "linear
manifold") is a vector subspace of some Euclidean space, potentially translated so as to not pass through the origin. More... | |
class | CartesianProduct |
The Cartesian product of convex sets is a convex set: S = X₁ × X₂ × ⋯ × Xₙ = {(x₁, x₂, ..., xₙ) | x₁ ∈ X₁, x₂ ∈ X₂, ..., xₙ ∈ Xₙ}. More... | |
class | ConvexHull |
Implements the convex hull of a set of convex sets. More... | |
class | ConvexSet |
Abstract base class for defining a convex set. More... | |
class | GraphOfConvexSets |
GraphOfConvexSets (GCS) implements the design pattern and optimization problems first introduced in the paper "Shortest Paths in Graphs of Convex Sets". More... | |
class | HPolyhedron |
Implements a polyhedral convex set using the half-space representation: {x| A x ≤ b} . More... | |
class | Hyperellipsoid |
Implements an ellipsoidal convex set represented by the quadratic form {x | (x-center)ᵀAᵀA(x-center) ≤ 1} . More... | |
class | Hyperrectangle |
Axis-aligned hyperrectangle in Rᵈ defined by its lower bounds and upper bounds as {x| lb ≤ x ≤ ub}. More... | |
class | Intersection |
A convex set that represents the intersection of multiple sets: S = X₁ ∩ X₂ ∩ ... More... | |
struct | IrisOptions |
Configuration options for the IRIS algorithm. More... | |
class | MinkowskiSum |
A convex set that represents the Minkowski sum of multiple sets: S = X₁ ⨁ X₂ ⨁ ... More... | |
class | Point |
A convex set that contains exactly one element. More... | |
class | Spectrahedron |
Implements a spectrahedron (the feasible set of a semidefinite program). More... | |
class | VPolytope |
A polytope described using the vertex representation. More... | |
Functions | |
HPolyhedron | Iris (const ConvexSets &obstacles, const Eigen::Ref< const Eigen::VectorXd > &sample, const HPolyhedron &domain, const IrisOptions &options=IrisOptions()) |
The IRIS (Iterative Region Inflation by Semidefinite programming) algorithm, as described in. More... | |
ConvexSets | MakeIrisObstacles (const QueryObject< double > &query_object, std::optional< FrameId > reference_frame=std::nullopt) |
Constructs ConvexSet representations of obstacles for IRIS in 3D using the geometry from a SceneGraph QueryObject. More... | |
HPolyhedron | IrisInConfigurationSpace (const multibody::MultibodyPlant< double > &plant, const systems::Context< double > &context, const IrisOptions &options=IrisOptions()) |
A variation of the Iris (Iterative Region Inflation by Semidefinite programming) algorithm which finds collision-free regions in the configuration space of plant . More... | |
void | SetEdgeContainmentTerminationCondition (IrisOptions *iris_options, const Eigen::Ref< const Eigen::VectorXd > &x_1, const Eigen::Ref< const Eigen::VectorXd > &x_2, const double epsilon=1e-3, const double tol=1e-6) |
Modifies the iris_options to facilitate finding a region that contains the edge between x_1 and x_2. More... | |
HPolyhedron drake::geometry::optimization::Iris | ( | const ConvexSets & | obstacles, |
const Eigen::Ref< const Eigen::VectorXd > & | sample, | ||
const HPolyhedron & | domain, | ||
const IrisOptions & | options = IrisOptions() |
||
) |
The IRIS (Iterative Region Inflation by Semidefinite programming) algorithm, as described in.
R. L. H. Deits and R. Tedrake, “Computing large convex regions of obstacle-free space through semidefinite programming,” Workshop on the Algorithmic Fundamentals of Robotics, Istanbul, Aug. 2014. http://groups.csail.mit.edu/robotics-center/public_papers/Deits14.pdf
This algorithm attempts to locally maximize the volume of a convex polytope representing obstacle-free space given a sample point and list of convex obstacles. Rather than compute the volume of the polytope directly, the algorithm maximizes the volume of an inscribed ellipsoid. It alternates between finding separating hyperplanes between the ellipsoid and the obstacles and then finding a new maximum-volume inscribed ellipsoid.
obstacles | is a vector of convex sets representing the occupied space. |
sample | provides a point in the space; the algorithm is initialized using a tiny sphere around this point. The algorithm is only guaranteed to succeed if this sample point is collision free (outside of all obstacles), but in practice the algorithm can often escape bad initialization (assuming the require_sample_point_is_contained option is false). |
domain | describes the total region of interest; computed IRIS regions will be inside this domain. It must be bounded, and is typically a simple bounding box (e.g. from HPolyhedron::MakeBox). |
The obstacles
, sample
, and the domain
must describe elements in the same ambient dimension (but that dimension can be any positive integer).
HPolyhedron drake::geometry::optimization::IrisInConfigurationSpace | ( | const multibody::MultibodyPlant< double > & | plant, |
const systems::Context< double > & | context, | ||
const IrisOptions & | options = IrisOptions() |
||
) |
A variation of the Iris (Iterative Region Inflation by Semidefinite programming) algorithm which finds collision-free regions in the configuration space of plant
.
options.num_collision_infeasible_samples
consecutive attempts.This method constructs a single Iris region in the configuration space of plant
.
plant | describes the kinematics of configuration space. It must be connected to a SceneGraph in a systems::Diagram. |
context | is a context of the plant . The context must have the positions of the plant set to the initialIRIS seed configuration. |
options | provides additional configuration options. In particular, increasing options.num_collision_infeasible_samples increases the chances that the IRIS regions are collision free but can also significantly increase the run-time of the algorithm. The same goes for options.num_additional_constraints_infeasible_samples . |
std::exception | if the sample configuration in context is infeasible. |
std::exception | if termination_func is invalid on the domain. See IrisOptions.termination_func for more details. |
ConvexSets drake::geometry::optimization::MakeIrisObstacles | ( | const QueryObject< double > & | query_object, |
std::optional< FrameId > | reference_frame = std::nullopt |
||
) |
Constructs ConvexSet representations of obstacles for IRIS in 3D using the geometry from a SceneGraph QueryObject.
All geometry in the scene with a proximity role, both anchored and dynamic, are consider to be fixed obstacles frozen in the poses captured in the context used to create the QueryObject.
When multiple representations are available for a particular geometry (e.g. a Box can be represented as either an HPolyhedron or a VPolytope), then this method will prioritize the representation that we expect is most performant for the current implementation of the IRIS algorithm.
void drake::geometry::optimization::SetEdgeContainmentTerminationCondition | ( | IrisOptions * | iris_options, |
const Eigen::Ref< const Eigen::VectorXd > & | x_1, | ||
const Eigen::Ref< const Eigen::VectorXd > & | x_2, | ||
const double | epsilon = 1e-3 , |
||
const double | tol = 1e-6 |
||
) |
Modifies the iris_options
to facilitate finding a region that contains the edge between x_1 and x_2.
It sets iris_options.starting_ellipse
to be a hyperellipsoid that contains the edge, is centered at the midpoint of the edge and extends in other directions by epsilon. It also sets iris_options.termination_func
such that IRIS iterations terminate when the edge is no longer contained in the IRIS region with tolerance tol.
std::exception | if x_1.size() != x_2.size(). |
std::exception | if epsilon <= 0. This is due to the fact that the hyperellipsoid for iris_options.starting_ellipse must have non-zero volume. |