Drake
Drake C++ Documentation

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)
 Computes the pairwise intersections between two lists of convex sets, returning a list of edges, and a list of their corresponding offsets. More...
 
std::vector< std::tuple< int, int, Eigen::VectorXd > > CalcPairwiseIntersections (const ConvexSets &convex_sets_A, const ConvexSets &convex_sets_B, const std::vector< int > &continuous_revolute_joints, bool preprocess_bbox=true)
 Computes the pairwise intersections between two lists of convex sets, returning a list of edges. 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)
 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::vector< std::tuple< int, int, Eigen::VectorXd > > CalcPairwiseIntersections (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)
 Overload of CalcPairwiseIntersections 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)
 Convenience overload to compute pairwise intersections within a list of convex sets. More...
 
std::vector< std::tuple< int, int, Eigen::VectorXd > > CalcPairwiseIntersections (const ConvexSets &convex_sets, const std::vector< int > &continuous_revolute_joints, bool preprocess_bbox=true)
 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)
 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::vector< std::tuple< int, int, Eigen::VectorXd > > CalcPairwiseIntersections (const ConvexSets &convex_sets, const std::vector< int > &continuous_revolute_joints, const std::vector< geometry::optimization::Hyperrectangle > &bboxes)
 Overload of CalcPairwiseIntersections allowing the user to supply axis- aligned bounding boxes if they're known a priori, to save on computation time. More...
 

Function Documentation

◆ CalcPairwiseIntersections() [1/4]

std::vector<std::tuple<int, int, Eigen::VectorXd> > drake::geometry::optimization::CalcPairwiseIntersections ( const ConvexSets convex_sets_A,
const ConvexSets convex_sets_B,
const std::vector< int > &  continuous_revolute_joints,
bool  preprocess_bbox = true 
)

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

Each edge is a tuple in the form [index_A, index_B, offset_A_to_B], where index_A is the index of the list in convex_sets_A, index_B is the index of the list in convex_sets_B, and offset_A_to_B is is the translation to 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_Ais a vector of convex sets. Pairwise intersections will be computed between convex_sets_A and convex_sets_B.
convex_sets_Bis the other vector of convex sets.
continuous_revolute_jointsis a list of joint indices corresponding to continuous revolute joints.
preprocess_bboxis 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.
Exceptions
ifcontinuous_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.
ifconvex_sets_A or convex_sets_B are empty. (Deprecated.)
Deprecated:
Instead use ComputePairwiseIntersections, with return type std::pair<std::vector<std::pair<int, int>>, std::vector<Eigen::VectorXd>>.
This will be removed from Drake on or after 2024-12-01.

◆ CalcPairwiseIntersections() [2/4]

std::vector<std::tuple<int, int, Eigen::VectorXd> > drake::geometry::optimization::CalcPairwiseIntersections ( 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 
)

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

Parameters
bboxes_Ais a vector of Hyperrectangles, allowing the user to manually pass in the AABBs of each set in convex_sets_A to avoid recomputation.
bboxes_Bserves 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
ifconvex_sets_A.size() != bboxes_A.size()
ifconvex_sets_B.size() != bboxes_B.size()
ifnot all entries of convex_sets_A, convex_sets_B, bboxes_A, and bboxes_B have the same ambient dimension. (Deprecated.)
Deprecated:
Instead use ComputePairwiseIntersections, with return type std::pair<std::vector<std::pair<int, int>>, std::vector<Eigen::VectorXd>>.
This will be removed from Drake on or after 2024-12-01.

◆ CalcPairwiseIntersections() [3/4]

std::vector<std::tuple<int, int, Eigen::VectorXd> > drake::geometry::optimization::CalcPairwiseIntersections ( const ConvexSets convex_sets,
const std::vector< int > &  continuous_revolute_joints,
bool  preprocess_bbox = true 
)

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

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

Parameters
convex_setsis a vector of convex sets. Pairwise intersections will be computed within convex_sets.
continuous_revolute_jointsis a list of joint indices corresponding to continuous revolute joints.
preprocess_bboxis 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.
Exceptions
ifcontinuous_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.
ifconvex_sets is empty. (Deprecated.)
Deprecated:
Instead use ComputePairwiseIntersections, with return type std::pair<std::vector<std::pair<int, int>>, std::vector<Eigen::VectorXd>>.
This will be removed from Drake on or after 2024-12-01.

◆ CalcPairwiseIntersections() [4/4]

std::vector<std::tuple<int, int, Eigen::VectorXd> > drake::geometry::optimization::CalcPairwiseIntersections ( const ConvexSets convex_sets,
const std::vector< int > &  continuous_revolute_joints,
const std::vector< geometry::optimization::Hyperrectangle > &  bboxes 
)

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

Parameters
bboxesis 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
ifconvex_sets.size() != bboxes.size()
ifnot all entries of convex_sets and bboxes have the same ambient dimension. (Deprecated.)
Deprecated:
Instead use ComputePairwiseIntersections, with return type std::pair<std::vector<std::pair<int, int>>, std::vector<Eigen::VectorXd>>.
This will be removed from Drake on or after 2024-12-01.

◆ 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::exceptionif 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 
)

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_Ais a vector of convex sets. Pairwise intersections will be computed between convex_sets_A and convex_sets_B.
convex_sets_Bis the other vector of convex sets.
continuous_revolute_jointsis a list of joint indices corresponding to continuous revolute joints.
preprocess_bboxis 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.
Exceptions
ifcontinuous_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.
ifconvex_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 
)

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_Ais a vector of Hyperrectangles, allowing the user to manually pass in the AABBs of each set in convex_sets_A to avoid recomputation.
bboxes_Bserves 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
ifconvex_sets_A.size() != bboxes_A.size()
ifconvex_sets_B.size() != bboxes_B.size()
ifnot 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 
)

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_setsis a vector of convex sets. Pairwise intersections will be computed within convex_sets.
continuous_revolute_jointsis a list of joint indices corresponding to continuous revolute joints.
preprocess_bboxis 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.
Exceptions
ifcontinuous_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.
ifconvex_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 
)

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

Parameters
bboxesis 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
ifconvex_sets.size() != bboxes.size()
ifnot 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
epsilonis 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::exceptionif ϵ <= 0 or ϵ >= π.
std::exceptionif the input convex set is unbounded along dimensions corresponding to continuous revolute joints.
std::exceptionif 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::exceptionunless every ConvexSet in convex_sets has the same ambient_dimension.
std::exceptionif ϵ <= 0 or ϵ >= π.
std::exceptionif any input convex set is unbounded along dimensions corresponding to continuous revolute joints.
std::exceptionif continuous_revolute_joints has repeated entries, or if any entry is outside the interval [0, ambient_dimension).