Detailed Description

These algorithms help construct regions of configuration space that are collision free.

Classes
class	CIrisCollisionGeometry
	This class contains the necessary information about the collision geometry used in C-IRIS. More...

class	CspaceFreeBox
	This class tries to find large axis-aligned bounding boxes in the configuration space, such that all configurations in the boxes are collision free. More...

class	CspaceFreePolytope
	This class tries to find large convex polytopes in the tangential-configuration space, such that all configurations in the convex polytopes is collision free. More...

class	CspaceFreePolytopeBase
	This virtual class is the base of CspaceFreePolytope and CspaceFreeBox. More...

struct	IrisOptions
	Configuration options for the IRIS algorithm. More...

class	CommonSampledIrisOptions
	Various options which are common to the sampling-based algorithms IrisNp2 and IrisZo for generating collision free polytopes in configuration space. More...

class	IrisParameterizationFunction
	Ordinarily, IRIS algorithms grow collision free regions in the robot's configuration space C. More...

struct	RaySamplerOptions
	RaySamplerOptions contains settings specific to the kRaySampler strategy for drawing the initial samples. More...

class	IrisNp2Options
	IrisNp2Options collects all parameters for the IRIS-NP2 algorithm. More...

class	IrisZoOptions
	IrisZoOptions collects all parameters for the IRIS-ZO algorithm. More...

Typedefs
typedef std::map< std::string, HPolyhedron >	IrisRegions
	Defines a standardized representation for (named) IrisRegions, which can be serialized in both C++ and Python. 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	IrisNp (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...

void	IrisInConfigurationSpaceFromCliqueCover (const CollisionChecker &checker, const IrisFromCliqueCoverOptions &options, RandomGenerator generator, std::vector< geometry::optimization::HPolyhedron > sets, const planning::graph_algorithms::MaxCliqueSolverBase *max_clique_solver=nullptr)
	Cover the configuration space in Iris regions using the Visibility Clique Cover Algorithm as described in. More...

geometry::optimization::HPolyhedron	IrisNp2 (const SceneGraphCollisionChecker &checker, const geometry::optimization::Hyperellipsoid &starting_ellipsoid, const geometry::optimization::HPolyhedron &domain, const IrisNp2Options &options=IrisNp2Options())
	The IRIS-NP2 (Iterative Regional Inflation by Semidefinite and Nonlinear Programming 2) algorithm, as described in. More...

geometry::optimization::HPolyhedron	IrisZo (const CollisionChecker &checker, const geometry::optimization::Hyperellipsoid &starting_ellipsoid, const geometry::optimization::HPolyhedron &domain, const IrisZoOptions &options=IrisZoOptions())
	The IRIS-ZO (Iterative Regional Inflation by Semidefinite programming - Zero Order) algorithm, as described in. More...

Variables
std::variant< geometry::optimization::IrisOptions, IrisNp2Options, IrisZoOptions >	iris_options {geometry::optimization::IrisOptions{.iteration_limit = 1}}
	The options used on internal calls to Iris. More...

Typedef Documentation

◆ IrisRegions

typedef std::map<std::string, HPolyhedron> IrisRegions

Defines a standardized representation for (named) IrisRegions, which can be serialized in both C++ and Python.

Function Documentation

◆ Iris()

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.

Parameters

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

Note: Some members of options are only applicable to IrisNp. The members relevant for this function are starting_ellipse, termination_func, bounding_region, verify_domain_boundedness, require_sample_point_is_contained, iteration_limit, termination_threshold, relative_termination_threshold.

◆ IrisInConfigurationSpaceFromCliqueCover()

void drake::planning::IrisInConfigurationSpaceFromCliqueCover	(	const CollisionChecker &	checker,
		const IrisFromCliqueCoverOptions &	options,
		RandomGenerator *	generator,
		std::vector< geometry::optimization::HPolyhedron > *	sets,
		const planning::graph_algorithms::MaxCliqueSolverBase *	max_clique_solver = `nullptr`
	)

Cover the configuration space in Iris regions using the Visibility Clique Cover Algorithm as described in.

P. Werner, A. Amice, T. Marcucci, D. Rus, R. Tedrake "Approximating Robot Configuration Spaces with few Convex Sets using Clique Covers of Visibility Graphs" In 2024 IEEE Internation Conference on Robotics and Automation. https://arxiv.org/abs/2310.02875

Parameters

checker	The collision checker containing the plant and its associated scene_graph.
generator	There are points in the algorithm requiring randomness. The generator controls this source of randomness.
sets	[in/out] initial sets covering the space (potentially empty). The cover is written into this vector.
max_clique_solver	The min clique cover problem is approximatley solved by repeatedly solving max clique on the uncovered graph and adding this largest clique to the cover. The max clique problem is solved by this solver. If parallelism is set to allow more than 1 thread, then the solver must be implemented in C++.

If nullptr is passed as the max_clique_solver, then max clique will be solved using an instance of MaxCliqueSolverViaGreedy, which is a fast heuristic. If higher quality cliques are desired, consider changing the solver to an instance of MaxCliqueSolverViaMip. Currently, the padding in the collision checker is not forwarded to the algorithm, and therefore the final regions do not necessarily respect this padding. Effectively, this means that the regions are generated as if the padding is set to 0. This behavior may be adjusted in the future at the resolution of #18830.

Note: that MaxCliqueSolverViaMip requires the availability of a Mixed-Integer Linear Programming solver (e.g. Gurobi and/or Mosek). We recommend enabling those solvers if possible because they produce higher quality cliques (https://drake.mit.edu/bazel.html#proprietary_solvers). The method will throw if max_clique_solver cannot solve the max clique problem.; If IrisNp2Options is used, then the collision checker must be a SceneGraphCollisionChecker.

Exceptions

std::exception	Parameterizations are not currently supported for `IrisZo` and `IrisNp2` when running `IrisFromCliqueCover`. This method will throw if options.iris_options is of type `IrisZoOptions` or `IrisNp2Options` and specifies a parametrization function. See the documentation of `IrisZoOptions` and `IrisNp2Options` for more information about subspace parametrization.
std::exception	If the options.iris_options.prog_with_additional_constraints is not nullptr i.e. if a prog with additional constraints is provided.

◆ IrisNp()

HPolyhedron drake::geometry::optimization::IrisNp	(	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.

See also: Iris for details on the original algorithm. This variant uses nonlinear optimization (instead of convex optimization) to find collisions in configuration space; each potential collision is probabilistically "certified" by restarting the nonlinear optimization from random initial seeds inside the candidate IRIS region until it fails to find a collision in options.num_collision_infeasible_samples consecutive attempts.

This method constructs a single Iris region in the configuration space of plant.

See also: planning::IrisInConfigurationSpaceFromCliqueCover for a method to automatically cover the configuration space with multiple Iris regions.

Parameters

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`.

Exceptions

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.

◆ IrisNp2()

geometry::optimization::HPolyhedron drake::planning::IrisNp2	(	const SceneGraphCollisionChecker &	checker,
		const geometry::optimization::Hyperellipsoid &	starting_ellipsoid,
		const geometry::optimization::HPolyhedron &	domain,
		const IrisNp2Options &	options = `IrisNp2Options()`
	)

The IRIS-NP2 (Iterative Regional Inflation by Semidefinite and Nonlinear Programming 2) algorithm, as described in.

[Werner et al., 2024] P. Werner, T. Cohn*, R. H. Jiang*, T. Seyde, M. Simchowitz, R. Tedrake, and D. Rus, "Faster Algorithms for Growing Collision-Free Convex Polytopes in Robot Configuration Space," * Denotes equal contribution.

https://groups.csail.mit.edu/robotics-center/public_papers/Werner24.pdf

This algorithm constructs probabilistically collision-free polytopes in robot configuration space using a scene graph collision checker. The sets are constructed by identifying collisions with sampling and nonlinear programming. The produced polytope P is probabilistically collision-free in the sense that one gets to control the probability δ that the fraction of the volume-in-collision is larger than ε

Pr[λ(P\Cfree)/λ(P) > ε] ≤ δ.

Parameters

starting_ellipsoid	provides the initial ellipsoid around which to grow the region. This is typically a small ball around a collision-free configuration (e.g. Hyperellipsoid::MakeHyperSphere(radius, seed_point)). The center of this ellipsoid is required to be collision-free.
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 representing joint limits (e.g. from HPolyhedron::MakeBox).
options	contains algorithm parameters such as the desired collision-free fraction, confidence level, and various algorithmic settings.

The starting_ellipsoid and domain must describe elements in the same ambient dimension as the configuration space of the robot, unless a parameterization is specified (in which case, they must match options.parameterization_dimension).

Returns: A HPolyhedron representing the computed collision-free region in configuration space.

Warning: This feature is considered to be experimental and may change or be removed at any time, without any deprecation notice ahead of time.

Exceptions

if	the center of `starting_ellipsoid` is in collision, or violates any of the user-specified constraints in `options.prog_with_additional_constraints`.

Note: This can be a long running function that needs to solve many QPs. If you have a solver which requires a license, consider acquiring the license before solving this function. See AcquireLicense for more details.

IrisNp2 is still in development, so certain features of SceneGraphCollisionChecker and parts of [Werner et al., 2024] are not yet supported.

Exceptions

if	any collision pairs in `checker` have negative padding.
if	any collision geometries have been been added in `checker`.

◆ IrisZo()

geometry::optimization::HPolyhedron drake::planning::IrisZo	(	const CollisionChecker &	checker,
		const geometry::optimization::Hyperellipsoid &	starting_ellipsoid,
		const geometry::optimization::HPolyhedron &	domain,
		const IrisZoOptions &	options = `IrisZoOptions()`
	)

The IRIS-ZO (Iterative Regional Inflation by Semidefinite programming - Zero Order) algorithm, as described in.

P. Werner, T. Cohn*, R. H. Jiang*, T. Seyde, M. Simchowitz, R. Tedrake, and D. Rus, "Faster Algorithms for Growing Collision-Free Convex Polytopes in Robot Configuration Space," * Denotes equal contribution.

https://groups.csail.mit.edu/robotics-center/public_papers/Werner24.pdf

This algorithm constructs probabilistically collision-free polytopes in robot configuration space while only relying on a collision checker. The sets are constructed using a simple parallel zero-order optimization strategy. The produced polytope P is probabilistically collision-free in the sense that one gets to control the probability δ that the fraction of the volume-in-collision is larger than ε

Pr[λ(P\Cfree)/λ(P) > ε] ≤ δ.

Parameters

starting_ellipsoid	provides the initial ellipsoid around which to grow the region. This is typically a small ball around a collision-free configuration (e.g. Hyperellipsoid::MakeHyperSphere(radius, seed_point)). The center of this ellipsoid is required to be collision-free.
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 representing joint limits (e.g. from HPolyhedron::MakeBox).
options	contains algorithm parameters such as the desired collision-free fraction, confidence level, and various algorithmic settings.

The starting_ellipsoid and domain must describe elements in the same ambient dimension as the configuration space of the robot, unless a parameterization is specified (in which case, they must match options.parameterization_dimension).

Returns: A HPolyhedron representing the computed collision-free region in configuration space.

Warning: This feature is considered to be experimental and may change or be removed at any time, without any deprecation notice ahead of time.

Exceptions

if	the center of `starting_ellipsoid` is in collision, or violates any of the user-specified constraints in `options.prog_with_additional_constraints`.

Note: This can be a long running function that needs to solve many QPs. If you have a solver which requires a license, consider acquiring the license before solving this function. See AcquireLicense for more details.

◆ MakeIrisObstacles()

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.

◆ SetEdgeContainmentTerminationCondition()

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.

Exceptions

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.

Variable Documentation

◆ iris_options

std::variant<geometry::optimization::IrisOptions, IrisNp2Options, IrisZoOptions> iris_options {geometry::optimization::IrisOptions{.iteration_limit = 1}}

The options used on internal calls to Iris.

The type of this option determines which variant of Iris is called. Currently, it is recommended to only run Iris for one iteration when building from a clique so as to avoid discarding the information gained from the clique.

Note that IrisOptions can optionally include a meshcat instance to provide debugging visualization. If this is provided IrisFromCliqueCover will provide debug visualization in meshcat showing where in configuration space it is drawing from. However, if the parallelism option is set to allow more than 1 thread, then the debug visualizations of internal Iris calls will be disabled. This is due to a limitation of drawing to meshcat from outside the main thread.

Note: some of these variants specify a parallelism parameter. In IrisInConfigurationSpaceFromCliqueCover, the iris_options.parallelism is ignored and the value of parallelism specified by this.parallelism will be used instead.

Detailed Description

Classes

Typedefs

Functions

Variables

Typedef Documentation

◆ IrisRegions

Function Documentation

◆ Iris()

◆ IrisInConfigurationSpaceFromCliqueCover()

◆ IrisNp()

◆ IrisNp2()

◆ IrisZo()

◆ MakeIrisObstacles()

◆ SetEdgeContainmentTerminationCondition()

Variable Documentation

◆ iris_options