Detailed Description

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

By tangential-configuration space, we mean the revolute joint angle θ is replaced by t = tan(θ/2). We refer to the algorithm as C-IRIS. For more details, refer to the paper

Certified Polyhedral Decomposition of Collision-Free Configuration Space by Hongkai Dai*, Alexandre Amice*, Peter Werner, Annan Zhang and Russ Tedrake.

A conference version is published at

Finding and Optimizing Certified, Collision-Free Regions in Configuration Space for Robot Manipulators by Alexandre Amice*, Hongkai Dai*, Peter Werner, Annan Zhang and Russ Tedrake.

#include <drake/geometry/optimization/cspace_free_polytope.h>

Classes
struct	BilinearAlternationOptions
	Options for bilinear alternation. More...

struct	BinarySearchOptions
	Options for binary search. More...

struct	FindPolytopeGivenLagrangianOptions
	Options for finding polytope with given Lagrangians. More...

struct	FindSeparationCertificateGivenPolytopeOptions

class	SearchResult
	Result on searching the C-space polytope and separating planes. More...

class	SeparatingPlaneLagrangians
	When searching for the separating plane, we want to certify that the numerator of a rational is non-negative in the C-space region C*s<=d, s_lower <= s <= s_upper. More...

struct	SeparationCertificate
	This struct stores the necessary information to search for the separating plane for the polytopic C-space region C*s <= d, s_lower <= s <= s_upper. More...

struct	SeparationCertificateProgram

struct	SeparationCertificateResult
	We certify that a pair of geometries is collision free in the C-space region {s \| Cs<=d, s_lower<=s<=s_upper} by finding the separating plane and the Lagrangian multipliers. More...

Public Types
enum	EllipsoidMarginCost { kSum, kGeometricMean }
	The cost used when fixing the Lagrangian multiplier and search for C and d in the C-space polytope {s \| C*s <=d, s_lower<=s<=s_upper}. More...

using	IgnoredCollisionPairs = std::unordered_set< SortedPair< geometry::GeometryId > >

Public Types inherited from CspaceFreePolytopeBase
using	IgnoredCollisionPairs = std::unordered_set< SortedPair< geometry::GeometryId > >

Public Member Functions
	~CspaceFreePolytope () override=default

	CspaceFreePolytope (const multibody::MultibodyPlant< double > plant, const geometry::SceneGraph< double > scene_graph, SeparatingPlaneOrder plane_order, const Eigen::Ref< const Eigen::VectorXd > &q_star, const Options &options=Options{})

bool	FindSeparationCertificateGivenPolytope (const Eigen::Ref< const Eigen::MatrixXd > &C, const Eigen::Ref< const Eigen::VectorXd > &d, const IgnoredCollisionPairs &ignored_collision_pairs, const FindSeparationCertificateGivenPolytopeOptions &options, std::unordered_map< SortedPair< geometry::GeometryId >, SeparationCertificateResult > *certificates) const
	Finds the certificates that the C-space polytope {s \| C*s<=d, s_lower <= s <= s_upper} is collision free. More...

void	AddCspacePolytopeContainment (solvers::MathematicalProgram *prog, const MatrixX< symbolic::Variable > &C, const VectorX< symbolic::Variable > &d, const Eigen::MatrixXd &s_inner_pts) const
	Adds the constraint that each column of s_inner_pts is in the polytope {s \| C*s<=d}. More...

std::vector< SearchResult >	SearchWithBilinearAlternation (const IgnoredCollisionPairs &ignored_collision_pairs, const Eigen::Ref< const Eigen::MatrixXd > &C_init, const Eigen::Ref< const Eigen::VectorXd > &d_init, const BilinearAlternationOptions &options) const
	Search for a collision-free C-space polytope. More...

std::optional< SearchResult >	BinarySearch (const IgnoredCollisionPairs &ignored_collision_pairs, const Eigen::Ref< const Eigen::MatrixXd > &C, const Eigen::Ref< const Eigen::VectorXd > &d_init, const Eigen::Ref< const Eigen::VectorXd > &s_center, const BinarySearchOptions &options) const
	Binary search on d such that the C-space polytope {s \| C*s<=d, s_lower<=s<=s_upper} is collision free. More...

std::unique_ptr< solvers::MathematicalProgram >	InitializePolytopeSearchProgram (const IgnoredCollisionPairs &ignored_collision_pairs, const std::unordered_map< SortedPair< geometry::GeometryId >, SeparationCertificateResult > &certificates, bool search_s_bounds_lagrangians, MatrixX< symbolic::Variable > C, VectorX< symbolic::Variable > d, std::unordered_map< int, SeparationCertificate > *new_certificates=nullptr) const
	Constructs a program to search for the C-space polytope {s \| C*s<=d, s_lower<=s<=s_upper} such that this polytope is collision free. More...

SeparationCertificateProgram	MakeIsGeometrySeparableProgram (const SortedPair< geometry::GeometryId > &geometry_pair, const Eigen::Ref< const Eigen::MatrixXd > &C, const Eigen::Ref< const Eigen::VectorXd > &d) const
	Constructs the MathematicalProgram which searches for a separation certificate for a pair of geometries for a C-space polytope. More...

std::optional< SeparationCertificateResult >	SolveSeparationCertificateProgram (const SeparationCertificateProgram &certificate_program, const FindSeparationCertificateGivenPolytopeOptions &options) const
	Solves a SeparationCertificateProgram with the given options. More...

Does not allow copy, move, or assignment
	CspaceFreePolytope (const CspaceFreePolytope &)=delete

CspaceFreePolytope &	operator= (const CspaceFreePolytope &)=delete

	CspaceFreePolytope (CspaceFreePolytope &&)=delete

CspaceFreePolytope &	operator= (CspaceFreePolytope &&)=delete

Public Member Functions inherited from CspaceFreePolytopeBase
virtual	~CspaceFreePolytopeBase ()

const multibody::RationalForwardKinematics &	rational_forward_kin () const
	Getter for the rational forward kinematics object that computes the forward kinematics as rational functions. More...

const std::unordered_map< SortedPair< geometry::GeometryId >, int > &	map_geometries_to_separating_planes () const
	separating_planes()[map_geometries_to_separating_planes.at(geometry1_id, geometry2_id)] is the separating plane that separates geometry 1 and geometry 2. More...

const std::vector< CSpaceSeparatingPlane< symbolic::Variable > > &	separating_planes () const
	All the separating planes between each pair of geometries. More...

const Vector3< symbolic::Variable > &	y_slack () const
	Get the slack variable used for non-polytopic collision geometries. More...

	CspaceFreePolytopeBase (const CspaceFreePolytopeBase &)=delete

CspaceFreePolytopeBase &	operator= (const CspaceFreePolytopeBase &)=delete

	CspaceFreePolytopeBase (CspaceFreePolytopeBase &&)=delete

CspaceFreePolytopeBase &	operator= (CspaceFreePolytopeBase &&)=delete

Friends
class	CspaceFreePolytopeTester

Additional Inherited Members
Protected Types inherited from CspaceFreePolytopeBase
enum	SForPlane { kAll, kOnChain }
	When we set up the separating plane {x \| a(s)ᵀx + b(s) = 0} between a pair of geometries, we need to determine which s are used in a(s) and b(s). More...

Protected Member Functions inherited from CspaceFreePolytopeBase
	CspaceFreePolytopeBase (const multibody::MultibodyPlant< double > plant, const geometry::SceneGraph< double > scene_graph, SeparatingPlaneOrder plane_order, SForPlane s_for_plane_enum, const Options &options=Options{})
	Constructor. More...

template<typename T >
void	CalcSBoundsPolynomial (const VectorX< T > &s_lower, const VectorX< T > &s_upper, VectorX< symbolic::Polynomial > s_minus_s_lower, VectorX< symbolic::Polynomial > s_upper_minus_s) const
	Computes s-s_lower and s_upper - s as polynomials of s. More...

int	GetSeparatingPlaneIndex (const SortedPair< geometry::GeometryId > &pair) const
	Returns the index of the plane which will separate the geometry pair. More...

const symbolic::Variables &	get_s_set () const

const geometry::SceneGraph< double > &	scene_graph () const

const std::map< multibody::BodyIndex, std::vector< std::unique_ptr< CIrisCollisionGeometry > > > &	link_geometries () const

SeparatingPlaneOrder	plane_order () const

const std::unordered_map< SortedPair< multibody::BodyIndex >, std::array< VectorX< symbolic::Monomial >, 4 > > &	map_body_to_monomial_basis_array () const
	Maps a pair of body (body1, body2) to an array of monomial basis `monomial_basis_array`. More...

bool	with_cross_y () const
	Check Options::with_cross_y for more details. More...

const std::unordered_map< SortedPair< multibody::BodyIndex >, std::vector< int > > &	map_body_pair_to_s_on_chain () const
	For a pair of bodies body_pair, returns the indices of all s on the kinematics chain from body_pair.first() to body_pair.second(). More...

VectorX< symbolic::Variable >	GetSForPlane (const SortedPair< multibody::BodyIndex > &body_pair, SForPlane s_for_plane_enum) const
	Returns a vector of s variable used in a(s), b(s), which parameterize the separating plane {x \| a(s)ᵀx+b(s) = 0}. More...

void	SolveCertificationForEachPlaneInParallel (const std::vector< int > &active_plane_indices, const std::function< std::pair< bool, int >(int)> &solve_plane_sos, Parallelism parallelism, bool verbose, bool terminate_at_failure) const
	For each pair of geometries, solve the certification problem to find their separation plane in parallel. More...

int	GetGramVarSizeForPolytopeSearchProgram (const std::vector< PlaneSeparatesGeometries > &plane_geometries_vec, const IgnoredCollisionPairs &ignored_collision_pairs, const std::function< int(const symbolic::RationalFunction &rational, const std::array< VectorX< symbolic::Monomial >, 4 > &monomial_basis_array)> &count_gram_per_rational) const
	Get the total size of all the decision variables for the Gram matrices, so as to search for the polytope with given Lagrangian multipliers. More...

Member Typedef Documentation

◆ IgnoredCollisionPairs

using IgnoredCollisionPairs = std::unordered_set<SortedPair<geometry::GeometryId> >

Member Enumeration Documentation

◆ EllipsoidMarginCost

enum EllipsoidMarginCost

The cost used when fixing the Lagrangian multiplier and search for C and d in the C-space polytope {s | C*s <=d, s_lower<=s<=s_upper}.

We denote δᵢ as the margin between the i'th face C.row(i)<=d(i) to the inscribed ellipsoid.

Enumerator
kSum	Maximize ∑ᵢδᵢ
kGeometricMean	Maximize the geometric mean power(∏ᵢ (δᵢ + ε), 1/n) where n is C.rows(), ε=FindPolytopeGivenLagrangianOptions.ellipsoid_margin_epsilon.

Constructor & Destructor Documentation

◆ CspaceFreePolytope() [1/3]

CspaceFreePolytope ( const CspaceFreePolytope & )

delete

◆ CspaceFreePolytope() [2/3]

CspaceFreePolytope ( CspaceFreePolytope && )

delete

◆ ~CspaceFreePolytope()

~CspaceFreePolytope ( )

overridedefault

◆ CspaceFreePolytope() [3/3]

CspaceFreePolytope	(	const multibody::MultibodyPlant< double > *	plant,
		const geometry::SceneGraph< double > *	scene_graph,
		SeparatingPlaneOrder	plane_order,
		const Eigen::Ref< const Eigen::VectorXd > &	q_star,
		const Options &	options = `Options{}`
	)

Parameters

plant	The plant for which we compute the C-space free polytopes. It must outlive this CspaceFreePolytope object.
scene_graph	The scene graph that has been connected with `plant`. It must outlive this CspaceFreePolytope object.
plane_order	The order of the polynomials in the plane to separate a pair of collision geometries.
q_star	Refer to RationalForwardKinematics for its meaning.

Note: CspaceFreePolytope knows nothing about contexts. The plant and scene_graph must be fully configured before instantiating this class.

Member Function Documentation

◆ AddCspacePolytopeContainment()

void AddCspacePolytopeContainment	(	solvers::MathematicalProgram *	prog,
		const MatrixX< symbolic::Variable > &	C,
		const VectorX< symbolic::Variable > &	d,
		const Eigen::MatrixXd &	s_inner_pts
	)		const

Adds the constraint that each column of s_inner_pts is in the polytope {s | C*s<=d}.

◆ BinarySearch()

std::optional<SearchResult> BinarySearch	(	const IgnoredCollisionPairs &	ignored_collision_pairs,
		const Eigen::Ref< const Eigen::MatrixXd > &	C,
		const Eigen::Ref< const Eigen::VectorXd > &	d_init,
		const Eigen::Ref< const Eigen::VectorXd > &	s_center,
		const BinarySearchOptions &	options
	)		const

Binary search on d such that the C-space polytope {s | C*s<=d, s_lower<=s<=s_upper} is collision free.

We scale the polytope {s | C*s<=d_init} about its center s_center and search the scaling factor.

Precondition: s_center is in the polytope {s | C*s<=d_init, s_lower<=s<=s_upper}

◆ FindSeparationCertificateGivenPolytope()

bool FindSeparationCertificateGivenPolytope	(	const Eigen::Ref< const Eigen::MatrixXd > &	C,
		const Eigen::Ref< const Eigen::VectorXd > &	d,
		const IgnoredCollisionPairs &	ignored_collision_pairs,
		const FindSeparationCertificateGivenPolytopeOptions &	options,
		std::unordered_map< SortedPair< geometry::GeometryId >, SeparationCertificateResult > *	certificates
	)		const

Finds the certificates that the C-space polytope {s | C*s<=d, s_lower <= s <= s_upper} is collision free.

Parameters

	C	The C-space polytope is {s \| C*s<=d, s_lower<=s<=s_upper}
	d	The C-space polytope is {s \| C*s<=d, s_lower<=s<=s_upper}
	ignored_collision_pairs	We will ignore the pair of geometries in `ignored_collision_pairs`.
[out]	certificates	Contains the certificate we successfully found for each pair of geometries. Notice that depending on `options`, the program could search for the certificate for each geometry pair in parallel, and will terminate the search once it fails to find the certificate for any pair.

Return values

success If true, then we have certified that the C-space polytope {s | C*s<=d, s_lower<=s<=s_upper} is collision free. Otherwise success=false.

◆ InitializePolytopeSearchProgram()

std::unique_ptr<solvers::MathematicalProgram> InitializePolytopeSearchProgram	(	const IgnoredCollisionPairs &	ignored_collision_pairs,
		const std::unordered_map< SortedPair< geometry::GeometryId >, SeparationCertificateResult > &	certificates,
		bool	search_s_bounds_lagrangians,
		MatrixX< symbolic::Variable > *	C,
		VectorX< symbolic::Variable > *	d,
		std::unordered_map< int, SeparationCertificate > *	new_certificates = `nullptr`
	)		const

Constructs a program to search for the C-space polytope {s | C*s<=d, s_lower<=s<=s_upper} such that this polytope is collision free.

This program treats C and d as decision variables, and searches for the separating planes between each pair of geometries. Note that this program doesn't contain any cost yet.

Parameters

	certificates	The return of FindSeparationCertificateGivenPolytope().
	search_s_bounds_lagrangians	Set to true if we search for the Lagrangian multiplier for the bounds s_lower <=s<=s_upper.
[out]	C	The C-space polytope is parameterized as {s \| C*s<=d, s_lower<=s<=s_upper}.
[out]	d	The C-space polytope is parameterized as {s \| C*s<=d, s_lower<=s<=s_upper}.
[out]	new_certificates	The new certificates to certify the new C-space polytope {s \| C*s<=d, s_lower<=s<=s_upper} is collision free. If new_certificates=nullptr, then we don't update it. This is used for testing.

◆ MakeIsGeometrySeparableProgram()

SeparationCertificateProgram MakeIsGeometrySeparableProgram	(	const SortedPair< geometry::GeometryId > &	geometry_pair,
		const Eigen::Ref< const Eigen::MatrixXd > &	C,
		const Eigen::Ref< const Eigen::VectorXd > &	d
	)		const

Constructs the MathematicalProgram which searches for a separation certificate for a pair of geometries for a C-space polytope.

Search for the separation certificate for a pair of geometries for a C-space polytope {s | C*s<=d, s_lower<=s<=s_upper}.

Exceptions

an	error if this `geometry_pair` doesn't need separation certificate (for example, they are on the same body).

◆ operator=() [1/2]

CspaceFreePolytope& operator= ( CspaceFreePolytope && )

delete

◆ operator=() [2/2]

CspaceFreePolytope& operator= ( const CspaceFreePolytope & )

delete

◆ SearchWithBilinearAlternation()

std::vector<SearchResult> SearchWithBilinearAlternation	(	const IgnoredCollisionPairs &	ignored_collision_pairs,
		const Eigen::Ref< const Eigen::MatrixXd > &	C_init,
		const Eigen::Ref< const Eigen::VectorXd > &	d_init,
		const BilinearAlternationOptions &	options
	)		const

Search for a collision-free C-space polytope.

{s | C*s<=d, s_lower<=s<=s_upper} through bilinear alternation. The goal is to maximize the volume the C-space polytope. Since we can't compute the polytope volume in the closed form, we use the volume of the maximal inscribed ellipsoid as a surrogate function of the polytope volume.

Parameters

ignored_collision_pairs	The pairs of geometries that we ignore when searching for separation certificates.
C_init	The initial value of C.
d_init	The initial value of d.
options	The options for the bilinear alternation.

Return values

results Stores the certification result in each iteration of the bilinear alternation.

◆ SolveSeparationCertificateProgram()

std::optional<SeparationCertificateResult> SolveSeparationCertificateProgram	(	const SeparationCertificateProgram &	certificate_program,
		const FindSeparationCertificateGivenPolytopeOptions &	options
	)		const

Solves a SeparationCertificateProgram with the given options.

Returns: result If we find the separation certificate, then result contains the separation plane and the Lagrangian polynomials; otherwise result is empty.

Friends And Related Function Documentation

◆ CspaceFreePolytopeTester

friend class CspaceFreePolytopeTester

friend

The documentation for this class was generated from the following file:

drake/geometry/optimization/cspace_free_polytope.h

Detailed Description

Classes

Public Types

Public Member Functions

Friends

Additional Inherited Members

Member Typedef Documentation

◆ IgnoredCollisionPairs

Member Enumeration Documentation

◆ EllipsoidMarginCost

Constructor & Destructor Documentation

◆ CspaceFreePolytope() [1/3]

◆ CspaceFreePolytope() [2/3]

◆ ~CspaceFreePolytope()

◆ CspaceFreePolytope() [3/3]

Member Function Documentation

◆ AddCspacePolytopeContainment()

◆ BinarySearch()

◆ FindSeparationCertificateGivenPolytope()

◆ InitializePolytopeSearchProgram()

◆ MakeIsGeometrySeparableProgram()

◆ operator=() [1/2]

◆ operator=() [2/2]

◆ SearchWithBilinearAlternation()

◆ SolveSeparationCertificateProgram()

Friends And Related Function Documentation

◆ CspaceFreePolytopeTester