|
Drake
|
|
Drake
|
GcsTrajectoryOptimization implements a simplified motion planning optimization problem introduced in the paper "Motion Planning around Obstacles with Convex Optimization" by Tobia Marcucci, Mark Petersen, David von Wrangel, Russ Tedrake.
Instead of using the full time-scaling curve, this problem uses a single time-scaling variable for each region. This formulation yields continuous trajectories, which are not differentiable at the transition times between the regions since non-convex continuity constraints are not supported yet. However, it supports continuity on the path r(s) for arbitrary degree. The path r(s) can be reconstructed from the gcs solution q(t) with NormalizeSegmentTimes() and post-processed with e.g. Toppra to enforce acceleration bounds.
The ith piece of the composite trajectory is defined as q(t) = r((t - tᵢ) / hᵢ). r : [0, 1] → ℝⁿ is a the path, parametrized as a Bézier curve with order n. tᵢ and hᵢ are the initial time and duration of the ith sub-trajectory.
This class supports the notion of a Subgraph of regions. This has proven useful to facilitate multi-modal motion planning such as: Subgraph A: find a collision-free trajectory for the robot to a grasping posture, Subgraph B: find a collision-free trajectory for the robot with the object in its hand to a placing posture, etc.
This class also supports robots with continuous revolute joints (revolute joints that don't have any joint limits) and mobile bases. Adding or subtracting 2π to such a joint's angle leaves it unchanged; this logic is implemented behind the scenes. To use it, one should specify the joint indices that don't have limits, and ensure all sets satisfy the "convexity radius" property – their width along a dimension corresponding to a continuous revolute joint must be less than π. This can be enforced when constructing the convex sets, or after the fact with geometry::optimization::PartitionConvexSet. The GcsTrajectoryOptimization methods AddRegions and AddEdges will handle all of the intersection checks behind the scenes, including applying the appropriate logic to connect sets that "wrap around" 2π.
#include <drake/planning/trajectory_optimization/gcs_trajectory_optimization.h>
Classes | |
| class | EdgesBetweenSubgraphs |
| EdgesBetweenSubgraphs are defined as the connecting edges between two given subgraphs. More... | |
| class | Subgraph |
| A Subgraph is a subset of the larger graph. More... | |
Public Member Functions | |
| GcsTrajectoryOptimization (int num_positions, std::vector< int > continuous_revolute_joints=std::vector< int >()) | |
| Constructs the motion planning problem. More... | |
| ~GcsTrajectoryOptimization () | |
| int | num_positions () const |
| Returns the number of position variables. More... | |
| const std::vector< int > & | continuous_revolute_joints () |
| Returns a list of indices corresponding to continuous revolute joints. More... | |
| std::string | GetGraphvizString (const std::optional< solvers::MathematicalProgramResult > &result=std::nullopt, bool show_slack=true, int precision=3, bool scientific=false) const |
| Returns a Graphviz string describing the graph vertices and edges. More... | |
| Subgraph & | AddRegions (const geometry::optimization::ConvexSets ®ions, const std::vector< std::pair< int, int >> &edges_between_regions, int order, double h_min=0, double h_max=20, std::string name="", std::optional< const std::vector< Eigen::VectorXd >> edge_offsets=std::nullopt) |
| Creates a Subgraph with the given regions and indices. More... | |
| Subgraph & | AddRegions (const geometry::optimization::ConvexSets ®ions, int order, double h_min=0, double h_max=20, std::string name="") |
| Creates a Subgraph with the given regions. More... | |
| void | RemoveSubgraph (const Subgraph &subgraph) |
| Remove a subgraph and all associated edges found in the subgraph and to and from other subgraphs. More... | |
| EdgesBetweenSubgraphs & | AddEdges (const Subgraph &from_subgraph, const Subgraph &to_subgraph, const geometry::optimization::ConvexSet *subspace=nullptr) |
| Connects two subgraphs with directed edges. More... | |
| void | AddTimeCost (double weight=1.0) |
| Adds a minimum time cost to all regions in the whole graph. More... | |
| void | AddPathLengthCost (const Eigen::MatrixXd &weight_matrix) |
| Adds multiple L2Norm Costs on the upper bound of the path length. More... | |
| void | AddPathLengthCost (double weight=1.0) |
| Adds multiple L2Norm Costs on the upper bound of the path length. More... | |
| void | AddVelocityBounds (const Eigen::Ref< const Eigen::VectorXd > &lb, const Eigen::Ref< const Eigen::VectorXd > &ub) |
Adds a linear velocity constraint to the entire graph lb ≤ q̇(t) ≤ ub. More... | |
| void | AddPathContinuityConstraints (int continuity_order) |
| Enforces derivative continuity constraints on the entire graph. More... | |
| std::pair< trajectories::CompositeTrajectory< double >, solvers::MathematicalProgramResult > | SolvePath (const Subgraph &source, const Subgraph &target, const geometry::optimization::GraphOfConvexSetsOptions &options={}) |
| Formulates and solves the mixed-integer convex formulation of the shortest path problem on the whole graph. More... | |
| std::pair< trajectories::CompositeTrajectory< double >, solvers::MathematicalProgramResult > | SolveConvexRestriction (const std::vector< const geometry::optimization::GraphOfConvexSets::Vertex * > &active_vertices, const geometry::optimization::GraphOfConvexSetsOptions &options={}) |
| Solves a trajectory optimization problem through specific vertices. More... | |
| double | EstimateComplexity () const |
| Provide a heuristic estimate of the complexity of the underlying GCS mathematical program, for regression testing purposes. More... | |
| std::vector< Subgraph * > | GetSubgraphs () const |
| Returns a vector of all subgraphs. More... | |
| std::vector< EdgesBetweenSubgraphs * > | GetEdgesBetweenSubgraphs () const |
| Returns a vector of all edges between subgraphs. More... | |
| const geometry::optimization::GraphOfConvexSets & | graph_of_convex_sets () const |
| Getter for the underlying GraphOfConvexSets. More... | |
Does not allow copy, move, or assignment | |
| GcsTrajectoryOptimization (const GcsTrajectoryOptimization &)=delete | |
| GcsTrajectoryOptimization & | operator= (const GcsTrajectoryOptimization &)=delete |
| GcsTrajectoryOptimization (GcsTrajectoryOptimization &&)=delete | |
| GcsTrajectoryOptimization & | operator= (GcsTrajectoryOptimization &&)=delete |
Static Public Member Functions | |
| static trajectories::CompositeTrajectory< double > | NormalizeSegmentTimes (const trajectories::CompositeTrajectory< double > &trajectory) |
| Normalizes each trajectory segment to one second in duration. More... | |
| static trajectories::CompositeTrajectory< double > | UnwrapToContinousTrajectory (const trajectories::CompositeTrajectory< double > &gcs_trajectory, std::vector< int > continuous_revolute_joints, std::optional< std::vector< int >> starting_rounds=std::nullopt) |
| Unwraps a trajectory with continuous revolute joints into a continuous trajectory in the Euclidean space. More... | |
|
delete |
|
delete |
|
explicit |
Constructs the motion planning problem.
| num_positions | is the dimension of the configuration space. |
| continuous_revolute_joints | is a list of indices corresponding to continuous revolute joints, i.e., revolute joints which don't have any joint limits, and hence "wrap around" at 2π. Each entry in continuous_revolute_joints must be non-negative, less than num_positions, and unique. This feature is currently only supported within a single subgraph: continuous revolute joints won't be taken into account when constructing edges between subgraphs or checking if sets intersect through a subspace. |
| EdgesBetweenSubgraphs& AddEdges | ( | const Subgraph & | from_subgraph, |
| const Subgraph & | to_subgraph, | ||
| const geometry::optimization::ConvexSet * | subspace = nullptr |
||
| ) |
Connects two subgraphs with directed edges.
| from_subgraph | is the subgraph to connect from. Must have been created from a call to AddRegions() on this object, not some other optimization program. |
| to_subgraph | is the subgraph to connect to. Must have been created from a call to AddRegions() on this object, not some other optimization program. |
| subspace | is the subspace that the connecting control points must be in. Subspace is optional. Only edges that connect through the subspace will be added, and the subspace is added as a constraint on the connecting control points. Subspaces of type point or HPolyhedron are supported since other sets require constraints that are not yet supported by the GraphOfConvexSets::Edge constraint, e.g., set containment of a HyperEllipsoid is formulated via LorentzCone constraints. Workaround: Create a subgraph of zero order with the subspace as the region and connect it between the two subgraphs. This works because GraphOfConvexSet::Vertex , supports arbitrary instances of ConvexSets. |
| void AddPathContinuityConstraints | ( | int | continuity_order | ) |
Enforces derivative continuity constraints on the entire graph.
| continuity_order | is the order of the continuity constraint. |
Note that the constraints are on the control points of the derivatives of r(s) and not q(t). This may result in discontinuities of the trajectory return by SolvePath() since the r(s) will get rescaled by the duration h to yield q(t). NormalizeSegmentTimes() will return r(s) with valid continuity.
| std::exception | if the continuity order is less than one since path continuity is enforced by default. |
| void AddPathLengthCost | ( | const Eigen::MatrixXd & | weight_matrix | ) |
Adds multiple L2Norm Costs on the upper bound of the path length.
Since we cannot directly compute the path length of a Bézier curve, we minimize the upper bound of the path integral by minimizing the sum of (weighted) distances between control points: ∑ |weight_matrix * (rᵢ₊₁ − rᵢ)|₂.
This cost will be added to the entire graph. Since the path length is only defined for Bézier curves that have two or more control points, this cost will only added to all subgraphs with order greater than zero. Note that this cost will be applied even to subgraphs added in the future.
| weight_matrix | is the relative weight of each component for the cost. The diagonal of the matrix is the weight for each dimension. The off-diagonal elements are the weight for the cross terms, which can be used to penalize diagonal movement. |
| void AddPathLengthCost | ( | double | weight = 1.0 | ) |
Adds multiple L2Norm Costs on the upper bound of the path length.
Since we cannot directly compute the path length of a Bézier curve, we minimize the upper bound of the path integral by minimizing the sum of distances between control points. For Bézier curves, this is equivalent to the sum of the L2Norm of the derivative control points of the curve divided by the order.
This cost will be added to the entire graph. Since the path length is only defined for Bézier curves that have two or more control points, this cost will only added to all subgraphs with order greater than zero. Note that this cost will be applied even to subgraphs added in the future.
| weight | is the relative weight of the cost. |
| Subgraph& AddRegions | ( | const geometry::optimization::ConvexSets & | regions, |
| const std::vector< std::pair< int, int >> & | edges_between_regions, | ||
| int | order, | ||
| double | h_min = 0, |
||
| double | h_max = 20, |
||
| std::string | name = "", |
||
| std::optional< const std::vector< Eigen::VectorXd >> | edge_offsets = std::nullopt |
||
| ) |
Creates a Subgraph with the given regions and indices.
| regions | represent the valid set a control point can be in. We retain a copy of the regions since other functions may access them. If any of the positions represent revolute joints without limits, each region has a maximum width of strictly less than π along dimensions corresponding to those joints. |
| edges_between_regions | is a list of pairs of indices into the regions vector. For each pair representing an edge between two regions, an edge is added within the subgraph. Note that the edges are directed so (i,j) will only add an edge from region i to region j. |
| order | is the order of the Bézier curve. |
| h_max | is the maximum duration to spend in a region (seconds). Some solvers struggle numerically with large values. |
| h_min | is the minimum duration to spend in a region (seconds) if that region is visited on the optimal path. Some cost and constraints are only convex for h > 0. For example the perspective quadratic cost of the path energy ||ṙ(s)||² / h becomes non-convex for h = 0. Otherwise h_min can be set to 0. |
| name | is the name of the subgraph. If the passed name is an empty string, a default name will be provided. |
| edge_offsets | is an optional list of vectors. If defined, the list must contain the same number of entries as edges_between_regions. In other words, if defined, there must be one edge offset for each specified edge. For each pair of sets listed in edges_between_regions, the first set is translated (in configuration space) by the corresponding vector in edge_offsets before computing the constraints associated to that edge. This is used to add edges between sets that "wrap around" 2π along some dimension, due to, e.g., a continuous revolute joint. This edge offset corresponds to the translation component of the affine map τ_uv in equation (11) of "Non-Euclidean Motion Planning with Graphs of Geodesically-Convex Sets", and per the discussion in Subsection VI A, τ_uv has no rotation component. |
| Subgraph& AddRegions | ( | const geometry::optimization::ConvexSets & | regions, |
| int | order, | ||
| double | h_min = 0, |
||
| double | h_max = 20, |
||
| std::string | name = "" |
||
| ) |
Creates a Subgraph with the given regions.
This function will compute the edges between the regions based on the set intersections.
| regions | represent the valid set a control point can be in. We retain a copy of the regions since other functions may access them. If any of the positions represent continuous revolute joints, each region must have a maximum width of strictly less than π along dimensions corresponding to those joints. |
| order | is the order of the Bézier curve. |
| h_min | is the minimum duration to spend in a region (seconds) if that region is visited on the optimal path. Some cost and constraints are only convex for h > 0. For example the perspective quadratic cost of the path energy ||ṙ(s)||² / h becomes non-convex for h = 0. Otherwise h_min can be set to 0. |
| h_max | is the maximum duration to spend in a region (seconds). Some solvers struggle numerically with large values. |
| name | is the name of the subgraph. A default name will be provided. |
| std::exception | if any of the regions has a width of π or greater along dimensions corresponding to continuous revolute joints. |
| void AddTimeCost | ( | double | weight = 1.0 | ) |
Adds a minimum time cost to all regions in the whole graph.
The cost is the sum of the time scaling variables.
This cost will be added to the entire graph. Note that this cost will be applied even to subgraphs added in the future.
| weight | is the relative weight of the cost. |
| void AddVelocityBounds | ( | const Eigen::Ref< const Eigen::VectorXd > & | lb, |
| const Eigen::Ref< const Eigen::VectorXd > & | ub | ||
| ) |
Adds a linear velocity constraint to the entire graph lb ≤ q̇(t) ≤ ub.
| lb | is the lower bound of the velocity. |
| ub | is the upper bound of the velocity. |
This constraint will be added to the entire graph. Since the velocity requires forming the derivative of the Bézier curve, this constraint will only added to all subgraphs with order greater than zero. Note that this constraint will be applied even to subgraphs added in the future.
| std::exception | if lb or ub are not of size num_positions(). |
| const std::vector<int>& continuous_revolute_joints | ( | ) |
Returns a list of indices corresponding to continuous revolute joints.
| double EstimateComplexity | ( | ) | const |
Provide a heuristic estimate of the complexity of the underlying GCS mathematical program, for regression testing purposes.
Here we sum the total number of variable appearances in our costs and constraints as a rough approximation of the complexity of the subproblems.
| std::vector<EdgesBetweenSubgraphs*> GetEdgesBetweenSubgraphs | ( | ) | const |
Returns a vector of all edges between subgraphs.
| std::string GetGraphvizString | ( | const std::optional< solvers::MathematicalProgramResult > & | result = std::nullopt, |
| bool | show_slack = true, |
||
| int | precision = 3, |
||
| bool | scientific = false |
||
| ) | const |
Returns a Graphviz string describing the graph vertices and edges.
If results is supplied, then the graph will be annotated with the solution values.
| show_slacks | determines whether the values of the intermediate (slack) variables are also displayed in the graph. |
| precision | sets the floating point precision (how many digits are generated) of the annotations. |
| scientific | sets the floating point formatting to scientific (if true) or fixed (if false). |
| std::vector<Subgraph*> GetSubgraphs | ( | ) | const |
Returns a vector of all subgraphs.
| const geometry::optimization::GraphOfConvexSets& graph_of_convex_sets | ( | ) | const |
Getter for the underlying GraphOfConvexSets.
This is intended primarily for inspecting the resulting programs.
|
static |
Normalizes each trajectory segment to one second in duration.
Reconstructs the path r(s) from the solution trajectory q(t) of SolvePath() s.t. each segment of the resulting trajectory will be one second long. The start time will match the original start time.
| trajectory | The solution trajectory returned by SolvePath(). |
| std::exception | if not all trajectory segments of the CompositeTrajectory are of type BezierCurve<double> |
| int num_positions | ( | ) | const |
Returns the number of position variables.
|
delete |
|
delete |
| void RemoveSubgraph | ( | const Subgraph & | subgraph | ) |
Remove a subgraph and all associated edges found in the subgraph and to and from other subgraphs.
| std::pair<trajectories::CompositeTrajectory<double>, solvers::MathematicalProgramResult> SolveConvexRestriction | ( | const std::vector< const geometry::optimization::GraphOfConvexSets::Vertex * > & | active_vertices, |
| const geometry::optimization::GraphOfConvexSetsOptions & | options = {} |
||
| ) |
Solves a trajectory optimization problem through specific vertices.
This method allows for targeted optimization by considering only selected active vertices, reducing the problem's complexity. See geometry::optimization::GraphOfConvexSets::SolveConvexRestriction(). This API prefers a sequence of vertices over edges, as a user may know which regions the solution should pass through. GcsTrajectoryOptimization::AddRegions() automatically manages edge creation and intersection checks, which makes passing a sequence of edges less convenient.
| active_vertices | A sequence of ordered vertices of subgraphs to be included in the problem. |
| options | include all settings for solving the shortest path problem. |
| std::exception | if the vertices are not connected. |
| std::exception | if two vertices are connected by multiple edges. This may happen if one connects two graphs through multiple subspaces, which is currently not supported with this method. |
| std::exception | if the program cannot be written as a convex optimization consumable by one of the standard solvers. |
| std::pair<trajectories::CompositeTrajectory<double>, solvers::MathematicalProgramResult> SolvePath | ( | const Subgraph & | source, |
| const Subgraph & | target, | ||
| const geometry::optimization::GraphOfConvexSetsOptions & | options = {} |
||
| ) |
Formulates and solves the mixed-integer convex formulation of the shortest path problem on the whole graph.
geometry::optimization::GraphOfConvexSets::SolveShortestPath() for further details.| source | specifies the source subgraph. Must have been created from a call to AddRegions() on this object, not some other optimization program. If the source is a subgraph with more than one region, an empty set will be added and optimizer will choose the best region to start in. To start in a particular point, consider adding a subgraph of order zero with a single region of type Point. |
| target | specifies the target subgraph. Must have been created from a call to AddRegions() on this object, not some other optimization program. If the target is a subgraph with more than one region, an empty set will be added and optimizer will choose the best region to end in. To end in a particular point, consider adding a subgraph of order zero with a single region of type Point. |
| options | include all settings for solving the shortest path problem. The following default options will be used if they are not provided in options:
|
geometry::optimization::GraphOfConvexSetsOptions for further details.
|
static |
Unwraps a trajectory with continuous revolute joints into a continuous trajectory in the Euclidean space.
Trajectories produced by GcsTrajectoryOptimization for robotic systems with continuous revolute joints may include apparent discontinuities, where a multiple of 2π is instantaneously added to a joint value at the boundary between two adjacent segments of the trajectory. This function removes such discontinuities by adding or subtracting the appropriate multiple of 2π, "unwrapping" the trajectory into a continuous representation suitable for downstream tasks that do not take the joint wraparound into account.
| gcs_trajectory | The trajectory to unwrap. |
| continuous_revolute_joints | The indices of the continuous revolute joints. |
| starting_rounds | A vector of integers that sets the starting rounds for each continuous revolute joint. Given integer k for the starting_round of a joint, its initial position will be wrapped into [2πk , 2π(k+1)). If the starting rounds are not provided, the initial position of trajectory will be unchanged. |
| std::exception | if starting_rounds.size()!=continuous_revolute_joints.size(). |
| std::exception | if continuous_revolute_joints contain repeated indices and/or indices outside the range [0, gcs_trajectory.rows()). |
| std::exception | if the gcs_trajectory is not continuous on the manifold defined by the continuous_revolute_joints, i.e., the shift between two consecutive segments is not an integer multiple of 2π (within a tolerance of 1e-10 radians). |
| std::exception | if all the segments are not of type BezierCurve. Other types are not supported yet. Note that currently the output of GcsTrajectoryOptimization::SolvePath() is a CompositeTrajectory of BezierCurves. |