CspaceFreeBox Class Reference

Detailed Description

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

Note that we don't guarantee to find the largest box.

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

Classes
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 box q_box_lower <= q <= q_box_upper (or equivalently s_box_lower <= s <= s_box_upper). More...
struct	SeparationCertificate
	This struct stores the necessary information to search for the separating plane for the polytopic C-space box q_box_lower <= q <= q_box_upper. More...
struct	SeparationCertificateProgram
struct	SeparationCertificateResult
	We certify that a pair of geometries is collision free in the C-space box {q | q_box_lower<=q<=q_box_upper} by finding the separating plane and the Lagrangian multipliers. More...

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

Public Member Functions
	~CspaceFreeBox () override
	CspaceFreeBox (const multibody::MultibodyPlant< double > *plant, const geometry::SceneGraph< double > *scene_graph, SeparatingPlaneOrder plane_order, const Options &options=Options{})
bool	FindSeparationCertificateGivenBox (const Eigen::Ref< const Eigen::VectorXd > &q_box_lower, const Eigen::Ref< const Eigen::VectorXd > &q_box_upper, const IgnoredCollisionPairs &ignored_collision_pairs, const FindSeparationCertificateOptions &options, std::unordered_map< SortedPair< geometry::GeometryId >, SeparationCertificateResult > *certificates) const
	Finds the certificates that the C-space box {q | q_box_lower <= q <= q_box_upper} is collision free. More...
Does not allow copy, move, or assignment
	CspaceFreeBox (const CspaceFreeBox &)=delete
CspaceFreeBox &	operator= (const CspaceFreeBox &)=delete
	CspaceFreeBox (CspaceFreeBox &&)=delete
CspaceFreeBox &	operator= (CspaceFreeBox &&)=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	CspaceFreeBoxTester

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

Constructor & Destructor Documentation

CspaceFreeBox() [1/3]

CspaceFreeBox ( const CspaceFreeBox &  )

delete

CspaceFreeBox() [2/3]

CspaceFreeBox ( CspaceFreeBox &&  )

delete

~CspaceFreeBox()

~CspaceFreeBox ( )

override

CspaceFreeBox() [3/3]

CspaceFreeBox	(	const multibody::MultibodyPlant< double > *	plant,
		const geometry::SceneGraph< double > *	scene_graph,
		SeparatingPlaneOrder	plane_order,
		const Options &	options = Options{}
	)

Parameters

plant	The plant for which we compute the C-space free boxes. It must outlive this CspaceFreeBox object.
scene_graph	The scene graph that has been connected with plant. It must outlive this CspaceFreeBox object.
plane_order	The order of the polynomials in the plane to separate a pair of collision geometries.

Note

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

Member Function Documentation

FindSeparationCertificateGivenBox()

bool FindSeparationCertificateGivenBox	(	const Eigen::Ref< const Eigen::VectorXd > &	q_box_lower,
		const Eigen::Ref< const Eigen::VectorXd > &	q_box_upper,
		const IgnoredCollisionPairs &	ignored_collision_pairs,
		const FindSeparationCertificateOptions &	options,
		std::unordered_map< SortedPair< geometry::GeometryId >, SeparationCertificateResult > *	certificates
	)		const

Finds the certificates that the C-space box {q | q_box_lower <= q <= q_box_upper} is collision free.

Parameters

	q_box_lower	The lower bound of the C-space box.
	q_box_upper	The upper bound of the C-space box.
	ignored_collision_pairs	We 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. At termination, the pair of geometries whose optimization hasn't been finished will not show up in certificates.

Return values

success

If true, then we have certified that the C-space box {q | q_box_lower<=q<=q_box_upper} is collision free. Otherwise success=false.

operator=() [1/2]

CspaceFreeBox& operator= ( const CspaceFreeBox &  )

delete

operator=() [2/2]

CspaceFreeBox& operator= ( CspaceFreeBox &&  )

delete

Friends And Related Function Documentation

CspaceFreeBoxTester

friend class CspaceFreeBoxTester

friend

---

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

• drake/geometry/optimization/cspace_free_box.h