Geodesic Convexity

Detailed Description

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.

In practice, this only requires that the width of the set along each dimension corresponding to an unbounded revolute joint be strictly less than π. These functions are primarily used by GcsTrajectoryOptimization to make motion plans for these types of robots.

Functions
bool	CheckIfSatisfiesConvexityRadius (const geometry::optimization::ConvexSet &convex_set, const std::vector< int > &continuous_revolute_joints)
	Given a convex set, and a list of indices corresponding to continuous revolute joints, checks whether or not the set satisfies the convexity radius. More...
geometry::optimization::ConvexSets	PartitionConvexSet (const geometry::optimization::ConvexSet &convex_set, const std::vector< int > &continuous_revolute_joints, const double epsilon=1e-5)
	Partitions a convex set into (smaller) convex sets whose union is the original set and that each respect the convexity radius as in CheckIfSatisfiesConvexityRadius. More...
geometry::optimization::ConvexSets	PartitionConvexSet (const geometry::optimization::ConvexSets &convex_sets, const std::vector< int > &continuous_revolute_joints, const double epsilon=1e-5)
	Function overload to take in a list of convex sets, and partition all so as to respect the convexity radius. More...
std::pair< std::vector< std::pair< int, int > >, std::vector< Eigen::VectorXd > >	ComputePairwiseIntersections (const ConvexSets &convex_sets_A, const ConvexSets &convex_sets_B, const std::vector< int > &continuous_revolute_joints, bool preprocess_bbox=true, Parallelism parallelism=Parallelism::Max())
	Computes the pairwise intersections between two lists of convex sets, returning a list of edges, and a list of their corresponding offsets. More...
std::pair< std::vector< std::pair< int, int > >, std::vector< Eigen::VectorXd > >	ComputePairwiseIntersections (const ConvexSets &convex_sets_A, const ConvexSets &convex_sets_B, const std::vector< int > &continuous_revolute_joints, const std::vector< geometry::optimization::Hyperrectangle > &bboxes_A, const std::vector< geometry::optimization::Hyperrectangle > &bboxes_B, Parallelism parallelism=Parallelism::Max())
	Overload of ComputePairwiseIntersections allowing the user to supply axis- aligned bounding boxes if they're known a priori, to save on computation time. More...
std::pair< std::vector< std::pair< int, int > >, std::vector< Eigen::VectorXd > >	ComputePairwiseIntersections (const ConvexSets &convex_sets, const std::vector< int > &continuous_revolute_joints, bool preprocess_bbox=true, Parallelism parallelism=Parallelism::Max())
	Convenience overload to compute pairwise intersections within a list of convex sets. More...
std::pair< std::vector< std::pair< int, int > >, std::vector< Eigen::VectorXd > >	ComputePairwiseIntersections (const ConvexSets &convex_sets, const std::vector< int > &continuous_revolute_joints, const std::vector< geometry::optimization::Hyperrectangle > &bboxes, Parallelism parallelism=Parallelism::Max())
	Overload of ComputePairwiseIntersections allowing the user to supply axis- aligned bounding boxes if they're known a priori, to save on computation time. More...

Function Documentation

CheckIfSatisfiesConvexityRadius()

bool drake::geometry::optimization::CheckIfSatisfiesConvexityRadius	(	const geometry::optimization::ConvexSet &	convex_set,
		const std::vector< int > &	continuous_revolute_joints
	)

Given a convex set, and a list of indices corresponding to continuous revolute joints, checks whether or not the set satisfies the convexity radius.

See §6.5.3 of "A Panoramic View of Riemannian Geometry", Marcel Berger for a general definition of convexity radius. When dealing with continuous revolute joints, respecting the convexity radius entails that each convex set has a width of stricty less than π along each dimension corresponding to a continuous revolute joint.

Exceptions

std::exception

if continuous_revolute_joints has repeated entries, or if any entry is outside the interval [0, convex_set.ambient_dimension()).

ComputePairwiseIntersections() [1/4]

std::pair<std::vector<std::pair<int, int> >, std::vector<Eigen::VectorXd> > drake::geometry::optimization::ComputePairwiseIntersections	(	const ConvexSets &	convex_sets_A,
		const ConvexSets &	convex_sets_B,
		const std::vector< int > &	continuous_revolute_joints,
		bool	preprocess_bbox = true,
		Parallelism	parallelism = Parallelism::Max()
	)

Computes the pairwise intersections between two lists of convex sets, returning a list of edges, and a list of their corresponding offsets.

Each edge is a tuple in the form [index_A, index_B], where index_A is the index of the set in convex_sets_A and index_B is the index of the set in convex_sets_B. The corresponding entry in the list of offsets (i.e., the entry at the same index) is the translation that is applied to all the points in the index_A'th set in convex_sets_A to align them with the index_B'th set in convex_sets_B. This translation may only have non-zero entries along the dimensions corresponding to continuous_revolute_joints. All non-zero entries are integer multiples of 2π as the translation of the sets still represents the same configurations for the indices in continuous_revolute_joints.

Parameters

convex_sets_A	is a vector of convex sets. Pairwise intersections will be computed between convex_sets_A and convex_sets_B.
convex_sets_B	is the other vector of convex sets.
continuous_revolute_joints	is a list of joint indices corresponding to continuous revolute joints.
preprocess_bbox	is a flag for whether the function should precompute axis-aligned bounding boxes (AABBs) for every set. This can speed up the pairwise intersection checks, by determining some sets to be disjoint without needing to solve an optimization problem. However, it does require some overhead to compute those bounding boxes.
parallelism	specifies the number of threads to use.

Exceptions

if	continuous_revolute_joints has repeated entries, or if any entry is outside the interval [0, ambient_dimension), where ambient_dimension is the ambient dimension of the convex sets in convex_sets_A and convex_sets_B.
if	convex_sets_A or convex_sets_B are empty.

ComputePairwiseIntersections() [2/4]

std::pair<std::vector<std::pair<int, int> >, std::vector<Eigen::VectorXd> > drake::geometry::optimization::ComputePairwiseIntersections	(	const ConvexSets &	convex_sets_A,
		const ConvexSets &	convex_sets_B,
		const std::vector< int > &	continuous_revolute_joints,
		const std::vector< geometry::optimization::Hyperrectangle > &	bboxes_A,
		const std::vector< geometry::optimization::Hyperrectangle > &	bboxes_B,
		Parallelism	parallelism = Parallelism::Max()
	)

Overload of ComputePairwiseIntersections allowing the user to supply axis- aligned bounding boxes if they're known a priori, to save on computation time.

Parameters

bboxes_A	is a vector of Hyperrectangles, allowing the user to manually pass in the AABBs of each set in convex_sets_A to avoid recomputation.
bboxes_B	serves the same role to convex_sets_B as bboxes_A does to convex_sets_A.

Warning

The function does not check that the entries of bboxes_A are indeed the AABBs corresponding to the sets in convex_sets_A (and likewise for bboxes_B).

Exceptions

if	convex_sets_A.size() != bboxes_A.size()
if	convex_sets_B.size() != bboxes_B.size()
if	not all entries of convex_sets_A, convex_sets_B, bboxes_A, and bboxes_B have the same ambient dimension.

ComputePairwiseIntersections() [3/4]

std::pair<std::vector<std::pair<int, int> >, std::vector<Eigen::VectorXd> > drake::geometry::optimization::ComputePairwiseIntersections	(	const ConvexSets &	convex_sets,
		const std::vector< int > &	continuous_revolute_joints,
		bool	preprocess_bbox = true,
		Parallelism	parallelism = Parallelism::Max()
	)

Convenience overload to compute pairwise intersections within a list of convex sets.

Equivalent to calling ComputePairwiseIntersections(convex_sets, convex_sets, continuous_revolute_joints).

Parameters

convex_sets	is a vector of convex sets. Pairwise intersections will be computed within convex_sets.
continuous_revolute_joints	is a list of joint indices corresponding to continuous revolute joints.
preprocess_bbox	is a flag for whether the function should precompute axis-aligned bounding boxes for every set. This can speed up the pairwise intersection checks, by determining some sets to be disjoint without needing to solve an optimization problem.
parallelism	specifies the number of threads to use.

Exceptions

if	continuous_revolute_joints has repeated entries, or if any entry is outside the interval [0, ambient_dimension), where ambient_dimension is the ambient dimension of the convex sets in convex_sets.
if	convex_sets is empty.

ComputePairwiseIntersections() [4/4]

std::pair<std::vector<std::pair<int, int> >, std::vector<Eigen::VectorXd> > drake::geometry::optimization::ComputePairwiseIntersections	(	const ConvexSets &	convex_sets,
		const std::vector< int > &	continuous_revolute_joints,
		const std::vector< geometry::optimization::Hyperrectangle > &	bboxes,
		Parallelism	parallelism = Parallelism::Max()
	)

Overload of ComputePairwiseIntersections allowing the user to supply axis- aligned bounding boxes if they're known a priori, to save on computation time.

Parameters

bboxes

is a vector of Hyperrectangles, allowing the user to manually pass in the AABBs of each set in convex_sets to avoid recomputation.

Warning

The function does not check that the entries are indeed the AABBs corresponding to the sets in convex_sets.

Exceptions

if	convex_sets.size() != bboxes.size()
if	not all entries of convex_sets and bboxes have the same ambient dimension.

PartitionConvexSet() [1/2]

geometry::optimization::ConvexSets drake::geometry::optimization::PartitionConvexSet	(	const geometry::optimization::ConvexSet &	convex_set,
		const std::vector< int > &	continuous_revolute_joints,
		const double	epsilon = 1e-5
	)

Partitions a convex set into (smaller) convex sets whose union is the original set and that each respect the convexity radius as in CheckIfSatisfiesConvexityRadius.

In practice, this is implemented as partitioning sets into pieces whose width is less than or equal to π-ϵ. Each entry in continuous_revolute_joints must be non-negative, less than num_positions, and unique.

Parameters

epsilon

is the ϵ value used for the convexity radius inequality. The partitioned sets are made by intersecting convex_set with axis-aligned bounding boxes that respect the convexity radius. These boxes are made to overlap by ϵ radians along each dimension, for numerical purposes.

Returns

the vector of convex sets that each respect convexity radius.

Exceptions

std::exception	if ϵ <= 0 or ϵ >= π.
std::exception	if the input convex set is unbounded along dimensions corresponding to continuous revolute joints.
std::exception	if continuous_revolute_joints has repeated entries, or if any entry is outside the interval [0, convex_set.ambient_dimension()).

PartitionConvexSet() [2/2]

geometry::optimization::ConvexSets drake::geometry::optimization::PartitionConvexSet	(	const geometry::optimization::ConvexSets &	convex_sets,
		const std::vector< int > &	continuous_revolute_joints,
		const double	epsilon = 1e-5
	)

Function overload to take in a list of convex sets, and partition all so as to respect the convexity radius.

Every set must be bounded and have the same ambient dimension. Each entry in continuous_revolute_joints must be non-negative, less than num_positions, and unique.

Exceptions

std::exception	unless every ConvexSet in convex_sets has the same ambient_dimension.
std::exception	if ϵ <= 0 or ϵ >= π.
std::exception	if any input convex set is unbounded along dimensions corresponding to continuous revolute joints.
std::exception	if continuous_revolute_joints has repeated entries, or if any entry is outside the interval [0, ambient_dimension).