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... | |
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
.
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. |
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. (Deprecated.) |
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.
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 . |
convex_sets_A
(and likewise for bboxes_B).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. (Deprecated.) |
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).
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. |
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. (Deprecated.) |
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.
bboxes | is a vector of Hyperrectangles, allowing the user to manually pass in the AABBs of each set in convex_sets to avoid recomputation. |
convex_sets
.if | convex_sets.size() != bboxes.size() |
if | not all entries of convex_sets and bboxes have the same ambient dimension. (Deprecated.) |
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.
std::exception | if continuous_revolute_joints has repeated entries, or if any entry is outside the interval [0, convex_set.ambient_dimension()). |
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
.
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. |
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. |
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.
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 . |
convex_sets_A
(and likewise for bboxes_B).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. |
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).
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. |
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. |
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.
bboxes | is a vector of Hyperrectangles, allowing the user to manually pass in the AABBs of each set in convex_sets to avoid recomputation. |
convex_sets
.if | convex_sets.size() != bboxes.size() |
if | not all entries of convex_sets and bboxes have the same ambient dimension. |
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.
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. |
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()). |
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.
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). |