|
Drake
|
|
Drake
|
A Subgraph is a subset of the larger graph.
It is defined by a set of regions and edges between them based on intersection. From an API standpoint, a Subgraph is useful to define a multi-modal motion planning problem. Further, it allows different constraints and objects to be added to different subgraphs. Note that the the GraphOfConvexSets does not differentiate between subgraphs and can't be mixed with other instances of GcsTrajectoryOptimization.
#include <drake/planning/trajectory_optimization/gcs_trajectory_optimization.h>
Public Member Functions | |
| ~Subgraph () | |
| const std::string & | name () const |
| Returns the name of the subgraph. | |
| int | order () const |
| Returns the order of the Bézier trajectory within the region. | |
| int | size () const |
| Returns the number of vertices in the subgraph. | |
| const std::vector< geometry::optimization::GraphOfConvexSets::Vertex * > & | Vertices () |
| Returns constant reference to a vector of mutable pointers to the vertices stored in the subgraph. | |
| std::vector< const geometry::optimization::GraphOfConvexSets::Vertex * > | Vertices () const |
| Returns pointers to the vertices stored in the subgraph. | |
| const std::vector< geometry::optimization::GraphOfConvexSets::Edge * > & | Edges () |
| Returns constant reference to a vector of mutable pointers to the edges. | |
| std::vector< const geometry::optimization::GraphOfConvexSets::Edge * > | Edges () const |
| Returns pointers to the edges stored in the subgraph. | |
| const geometry::optimization::ConvexSets & | regions () const |
| Returns the regions associated with this subgraph before the CartesianProduct. | |
| void | AddTimeCost (double weight=1.0) |
| Adds a minimum time cost to all regions in the subgraph. | |
| void | AddPathLengthCost (const Eigen::MatrixXd &weight_matrix) |
| Adds multiple L2Norm Costs on the upper bound of the path length. | |
| void | AddPathEnergyCost (const Eigen::MatrixXd &weight_matrix) |
| Similar to AddPathLengthCost in usage, but minimizes ∑ |weight_matrix * (rᵢ₊₁ − rᵢ)|₂². | |
| void | AddPathLengthCost (double weight=1.0) |
| Adds multiple L2Norm Costs on the upper bound of the path length. | |
| void | AddPathEnergyCost (double weight=1.0) |
| Similar to AddPathLengthCost in usage, but minimizes ∑ |(rᵢ₊₁ − rᵢ)|₂² with weight being applied uniformly to all dimensions. | |
| void | AddVelocityBounds (const Eigen::Ref< const Eigen::VectorXd > &lb, const Eigen::Ref< const Eigen::VectorXd > &ub) |
| Adds a linear velocity constraint to the subgraph lb ≤ q̇(t) ≤ ub. | |
| void | AddNonlinearDerivativeBounds (const Eigen::Ref< const Eigen::VectorXd > &lb, const Eigen::Ref< const Eigen::VectorXd > &ub, int derivative_order) |
| Adds a nonlinear derivative constraints to the subgraph lb ≤ dᴺq(t) / dtᴺ ≤ ub. | |
| void | AddPathContinuityConstraints (int continuity_order) |
| Enforces that for any two subsequent path segments within the subgraph, the continuity_orderth path derivative at the end of the first segment equals that of the start of the second segment. | |
| void | AddContinuityConstraints (int continuity_order) |
| Enforces that for any two subsequent path segments within the subgraph, the continuity_orderth time derivative at the end of the first segment equals that of the start of the second segment. | |
| const symbolic::Variable & | vertex_duration () const |
| Returns a placeholder decision variable (not actually declared as a decision variable in the MathematicalProgram) associated with the time scaling of the trajectory in a set within this subgraph. | |
| const solvers::MatrixXDecisionVariable & | vertex_control_points () const |
| Returns a placeholder decision variable (not actually declared as a decision variable in the MathematicalProgram) associated with the control points of the trajectory in a set within this subgraph. | |
| const std::pair< symbolic::Variable, symbolic::Variable > & | edge_constituent_vertex_durations () const |
| Returns a pair of placeholder decision variables (not actually declared as decision variables in the MathematicalProgram) associated with the time scaling of the trajectory in two sets within this subgraph that are connected by an internal edge. | |
| const std::pair< solvers::MatrixXDecisionVariable, solvers::MatrixXDecisionVariable > & | edge_constituent_vertex_control_points () const |
| Returns a pair of placeholder decision variables (not actually declared as decision variables in the MathematicalProgram) associated with the control points of the trajectory in two sets within this subgraph that are connected by an internal edge. | |
| void | AddVertexCost (const symbolic::Expression &e, const std::unordered_set< geometry::optimization::GraphOfConvexSets::Transcription > &use_in_transcription={ geometry::optimization::GraphOfConvexSets::Transcription::kMIP, geometry::optimization::GraphOfConvexSets::Transcription::kRelaxation, geometry::optimization::GraphOfConvexSets::Transcription::kRestriction}) |
| Adds an arbitrary user-defined cost to every vertex in the subgraph. | |
| void | AddVertexCost (const solvers::Binding< solvers::Cost > &binding, const std::unordered_set< geometry::optimization::GraphOfConvexSets::Transcription > &use_in_transcription={ geometry::optimization::GraphOfConvexSets::Transcription::kMIP, geometry::optimization::GraphOfConvexSets::Transcription::kRelaxation, geometry::optimization::GraphOfConvexSets::Transcription::kRestriction}) |
| Convenience overload of AddVertexCost to take in a Binding<Cost>. | |
| void | AddVertexConstraint (const symbolic::Formula &e, const std::unordered_set< geometry::optimization::GraphOfConvexSets::Transcription > &use_in_transcription={ geometry::optimization::GraphOfConvexSets::Transcription::kMIP, geometry::optimization::GraphOfConvexSets::Transcription::kRelaxation, geometry::optimization::GraphOfConvexSets::Transcription::kRestriction}) |
| Adds an arbitrary user-defined constraint to every vertex in the subgraph. | |
| void | AddVertexConstraint (const solvers::Binding< solvers::Constraint > &binding, const std::unordered_set< geometry::optimization::GraphOfConvexSets::Transcription > &use_in_transcription={ geometry::optimization::GraphOfConvexSets::Transcription::kMIP, geometry::optimization::GraphOfConvexSets::Transcription::kRelaxation, geometry::optimization::GraphOfConvexSets::Transcription::kRestriction}) |
| Convenience overload of AddVertexConstraint to take in a Binding<Constraint>. | |
| void | AddEdgeCost (const symbolic::Expression &e, const std::unordered_set< geometry::optimization::GraphOfConvexSets::Transcription > &use_in_transcription={ geometry::optimization::GraphOfConvexSets::Transcription::kMIP, geometry::optimization::GraphOfConvexSets::Transcription::kRelaxation, geometry::optimization::GraphOfConvexSets::Transcription::kRestriction}) |
| Adds an arbitrary user-defined cost to every internal edge within the subgraph. | |
| void | AddEdgeCost (const solvers::Binding< solvers::Cost > &binding, const std::unordered_set< geometry::optimization::GraphOfConvexSets::Transcription > &use_in_transcription={ geometry::optimization::GraphOfConvexSets::Transcription::kMIP, geometry::optimization::GraphOfConvexSets::Transcription::kRelaxation, geometry::optimization::GraphOfConvexSets::Transcription::kRestriction}) |
| Convenience overload of AddEdgeCost to take in a Binding<Cost>. | |
| void | AddEdgeConstraint (const symbolic::Formula &e, const std::unordered_set< geometry::optimization::GraphOfConvexSets::Transcription > &use_in_transcription={ geometry::optimization::GraphOfConvexSets::Transcription::kMIP, geometry::optimization::GraphOfConvexSets::Transcription::kRelaxation, geometry::optimization::GraphOfConvexSets::Transcription::kRestriction}) |
| Adds an arbitrary user-defined constraint (in the form of a Formula or Binding<Constraint>) to every internal edge within the subgraph. | |
| void | AddEdgeConstraint (const solvers::Binding< solvers::Constraint > &binding, const std::unordered_set< geometry::optimization::GraphOfConvexSets::Transcription > &use_in_transcription={ geometry::optimization::GraphOfConvexSets::Transcription::kMIP, geometry::optimization::GraphOfConvexSets::Transcription::kRelaxation, geometry::optimization::GraphOfConvexSets::Transcription::kRestriction}) |
| Convenience overload of AddEdgeConstraint to take in a Binding<Constraint>. | |
Does not allow copy, move, or assignment | |
| Subgraph (const Subgraph &)=delete | |
| Subgraph & | operator= (const Subgraph &)=delete |
| Subgraph (Subgraph &&)=delete | |
| Subgraph & | operator= (Subgraph &&)=delete |
Friends | |
| class | GcsTrajectoryOptimization |
|
delete |
|
delete |
| ~Subgraph | ( | ) |
| void AddContinuityConstraints | ( | int | continuity_order | ) |
Enforces that for any two subsequent path segments within the subgraph, the continuity_orderth time derivative at the end of the first segment equals that of the start of the second segment.
This adds a nonlinear constraint to the restriction and MIP GraphOfConvexSets::Transcription, while adding a convex surrogate to the relaxation. For more details, see Guiding Non-convex Optimization with the GraphOfConvexSets.
The continuity is enforced on the control points of q(t), which appear as nonlinear constraints.
(dᴺrᵤ(s=1) / dsᴺ) / hᵤᴺ == (dᴺrᵥ(s=0) / dsᴺ) / hᵥᴺ
The convex surrogate is simply the path continuity, where hᵤᴺ and hᵥᴺ are replaced by the characteristic times of the respective sets:
(dᴺrᵤ(s=1) / dsᴺ) / hᵤ₀ᴺ == (dᴺrᵥ(s=0) / dsᴺ) / hᵥ₀ᴺ
. For now, these are set to one, but future work may involve scaling them by the size of the sets.
| continuity_order | is the order of the continuity constraint. |
| std::exception | if the continuity order is not equal or less than the order the subgraphs. |
| std::exception | if the continuity order is less than one since path continuity is enforced by default. |
| void AddEdgeConstraint | ( | const solvers::Binding< solvers::Constraint > & | binding, |
| const std::unordered_set< geometry::optimization::GraphOfConvexSets::Transcription > & | use_in_transcription = { geometry::optimization::GraphOfConvexSets::Transcription::kMIP, geometry::optimization::GraphOfConvexSets::Transcription::kRelaxation, geometry::optimization::GraphOfConvexSets::Transcription::kRestriction} ) |
Convenience overload of AddEdgeConstraint to take in a Binding<Constraint>.
| std::exception | if any variables besides those from edge_constituent_vertex_durations and edge_constituent_vertex_control_points are used. |
| void AddEdgeConstraint | ( | const symbolic::Formula & | e, |
| const std::unordered_set< geometry::optimization::GraphOfConvexSets::Transcription > & | use_in_transcription = { geometry::optimization::GraphOfConvexSets::Transcription::kMIP, geometry::optimization::GraphOfConvexSets::Transcription::kRelaxation, geometry::optimization::GraphOfConvexSets::Transcription::kRestriction} ) |
Adds an arbitrary user-defined constraint (in the form of a Formula or Binding<Constraint>) to every internal edge within the subgraph.
The constraint should be defined using the placeholder control point variables (obtained from edge_constituent_vertex_control_points()) and the placeholder time scaling variables (obtained from edge_constituent_vertex_durations()). This enables greater modeling freedom, but we cannot guarantee a feasible solution for all possible constraints.
| std::exception | if any variables besides those from edge_constituent_vertex_durations and edge_constituent_vertex_control_points are used. |
Constraints which do not support the perspective operation cannot be used with Transcription::kMIP or Transcription::kRelaxation. Consider providing an appropriate "convex surrogate" that is supported within GraphOfConvexSets, or exclusively using the SolveConvexRestriction method.
| void AddEdgeCost | ( | const solvers::Binding< solvers::Cost > & | binding, |
| const std::unordered_set< geometry::optimization::GraphOfConvexSets::Transcription > & | use_in_transcription = { geometry::optimization::GraphOfConvexSets::Transcription::kMIP, geometry::optimization::GraphOfConvexSets::Transcription::kRelaxation, geometry::optimization::GraphOfConvexSets::Transcription::kRestriction} ) |
Convenience overload of AddEdgeCost to take in a Binding<Cost>.
| std::exception | if any variables besides those from edge_constituent_vertex_durations and edge_constituent_vertex_control_points are used. |
| void AddEdgeCost | ( | const symbolic::Expression & | e, |
| const std::unordered_set< geometry::optimization::GraphOfConvexSets::Transcription > & | use_in_transcription = { geometry::optimization::GraphOfConvexSets::Transcription::kMIP, geometry::optimization::GraphOfConvexSets::Transcription::kRelaxation, geometry::optimization::GraphOfConvexSets::Transcription::kRestriction} ) |
Adds an arbitrary user-defined cost to every internal edge within the subgraph.
The cost should be defined using the placeholder control point variables (obtained from edge_constituent_vertex_control_points()) and the placeholder time scaling variables (obtained from edge_constituent_vertex_durations()). This enables greater modeling freedom, but we cannot guarantee a feasible solution for all possible costs.
| std::exception | if any variables besides those from edge_constituent_vertex_durations and edge_constituent_vertex_control_points are used. |
Costs which do not support the perspective operation cannot be used with Transcription::kMIP or Transcription::kRelaxation. Consider providing an appropriate "convex surrogate" that is supported within GraphOfConvexSets, or exclusively using the SolveConvexRestriction method.
| void AddNonlinearDerivativeBounds | ( | const Eigen::Ref< const Eigen::VectorXd > & | lb, |
| const Eigen::Ref< const Eigen::VectorXd > & | ub, | ||
| int | derivative_order ) |
Adds a nonlinear derivative constraints to the subgraph lb ≤ dᴺq(t) / dtᴺ ≤ ub.
This adds a nonlinear constraint to the restriction and MIP GraphOfConvexSets::Transcription, while adding a convex surrogate to the relaxation. For more details, see Guiding Non-convex Optimization with the GraphOfConvexSets.
The nonlinear constraint involves the derivative dᴺq(t) / dtᴺ which is decomposed as dᴺr(s) / dsᴺ / hᴺ. The convex surrogate replaces the nonlinear component hᴺ with h₀ᴺ⁻¹h, where h₀ is the characteristic time of the set. For now, h₀ is set to 1.0 for all sets.
| lb | is the lower bound of the derivative. |
| ub | is the upper bound of the derivative. |
| derivative_order | is the order of the derivative to be constrained. |
| std::exception | if subgraph order is less than the derivative order. |
| std::exception | if the derivative order <= 1, since the linear velocity bounds are preferred. |
| std::exception | if lb or ub are not of size num_positions(). |
| void AddPathContinuityConstraints | ( | int | continuity_order | ) |
Enforces that for any two subsequent path segments within the subgraph, the continuity_orderth path derivative at the end of the first segment equals that of the start of the second segment.
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.
| continuity_order | is the order of the continuity constraint. |
| std::exception | if the continuity order is not equal or less than the order the subgraphs. |
| std::exception | if the continuity order is less than one since path continuity is enforced by default. |
| void AddPathEnergyCost | ( | const Eigen::MatrixXd & | weight_matrix | ) |
Similar to AddPathLengthCost in usage, but minimizes ∑ |weight_matrix * (rᵢ₊₁ − rᵢ)|₂².
In comparison to AddPathLength cost, this cost encourages control points to be evenly spaced but may result in greater number of regions and larger path length on the solution. It is recommended to use this cost only with SolveConvexRestriction when it becomes a quadratic cost for which some solvers show a better performance.
| 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 AddPathEnergyCost | ( | double | weight = 1.0 | ) |
Similar to AddPathLengthCost in usage, but minimizes ∑ |(rᵢ₊₁ − rᵢ)|₂² with weight being applied uniformly to all dimensions.
In comparison to AddPathLength cost, this cost encourages control points to be evenly spaced but may result in greater number of regions and larger path length on the solution. It is recommended to use this cost only with SolveConvexRestriction when it becomes a quadratic cost for which some solvers show a better performance.
| weight | is the relative weight of the cost. |
| 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 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.
| 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.
We upper bound the trajectory length by the sum of the 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.
| weight | is the relative weight of the cost. |
| void AddTimeCost | ( | double | weight = 1.0 | ) |
Adds a minimum time cost to all regions in the subgraph.
The cost is the sum of the time scaling variables.
| 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 subgraph lb ≤ q̇(t) ≤ ub.
| lb | is the lower bound of the velocity. |
| ub | is the upper bound of the velocity. |
| std::exception | if subgraph order is zero, since the velocity is defined as the derivative of the Bézier curve. |
| std::exception | if lb or ub are not of size num_positions(). |
| void AddVertexConstraint | ( | const solvers::Binding< solvers::Constraint > & | binding, |
| const std::unordered_set< geometry::optimization::GraphOfConvexSets::Transcription > & | use_in_transcription = { geometry::optimization::GraphOfConvexSets::Transcription::kMIP, geometry::optimization::GraphOfConvexSets::Transcription::kRelaxation, geometry::optimization::GraphOfConvexSets::Transcription::kRestriction} ) |
Convenience overload of AddVertexConstraint to take in a Binding<Constraint>.
| std::exception | if any variables besides those from vertex_duration and vertex_control_points are used. |
| void AddVertexConstraint | ( | const symbolic::Formula & | e, |
| const std::unordered_set< geometry::optimization::GraphOfConvexSets::Transcription > & | use_in_transcription = { geometry::optimization::GraphOfConvexSets::Transcription::kMIP, geometry::optimization::GraphOfConvexSets::Transcription::kRelaxation, geometry::optimization::GraphOfConvexSets::Transcription::kRestriction} ) |
Adds an arbitrary user-defined constraint to every vertex in the subgraph.
The constraint should be defined using the placeholder control point variables (obtained from vertex_control_points()) and the placeholder time scaling variable (obtained from vertex_duration()). This enables greater modeling freedom, but we cannot guarantee a feasible solution for all possible constraints.
| std::exception | if any variables besides those from vertex_duration and vertex_control_points are used. |
Constraints which do not support the perspective operation cannot be used with Transcription::kMIP or Transcription::kRelaxation. Consider providing an appropriate "convex surrogate" that is supported within GraphOfConvexSets, or exclusively using the SolveConvexRestriction method.
| void AddVertexCost | ( | const solvers::Binding< solvers::Cost > & | binding, |
| const std::unordered_set< geometry::optimization::GraphOfConvexSets::Transcription > & | use_in_transcription = { geometry::optimization::GraphOfConvexSets::Transcription::kMIP, geometry::optimization::GraphOfConvexSets::Transcription::kRelaxation, geometry::optimization::GraphOfConvexSets::Transcription::kRestriction} ) |
Convenience overload of AddVertexCost to take in a Binding<Cost>.
| std::exception | if any variables besides those from vertex_duration and vertex_control_points are used. |
| void AddVertexCost | ( | const symbolic::Expression & | e, |
| const std::unordered_set< geometry::optimization::GraphOfConvexSets::Transcription > & | use_in_transcription = { geometry::optimization::GraphOfConvexSets::Transcription::kMIP, geometry::optimization::GraphOfConvexSets::Transcription::kRelaxation, geometry::optimization::GraphOfConvexSets::Transcription::kRestriction} ) |
Adds an arbitrary user-defined cost to every vertex in the subgraph.
The cost should be defined using the placeholder control point variables (obtained from vertex_control_points()) and the placeholder time scaling variable (obtained from vertex_duration()). This enables greater modeling freedom, but we cannot guarantee a feasible solution for all possible costs.
| std::exception | if any variables besides those from vertex_duration and vertex_control_points are used. |
Costs which do not support the perspective operation cannot be used with Transcription::kMIP or Transcription::kRelaxation. Consider providing an appropriate "convex surrogate" that is supported within GraphOfConvexSets, or exclusively using the SolveConvexRestriction method.
| const std::pair< solvers::MatrixXDecisionVariable, solvers::MatrixXDecisionVariable > & edge_constituent_vertex_control_points | ( | ) | const |
Returns a pair of placeholder decision variables (not actually declared as decision variables in the MathematicalProgram) associated with the control points of the trajectory in two sets within this subgraph that are connected by an internal edge.
Each variable will be of shape (num_positions(), order+1), where the ith column is the ith control point. These variables will be substituted for real decision variables in methods like AddEdgeCost and AddEdgeConstraint. Passing this variable directly into objectives/constraints will result in an error.
| const std::pair< symbolic::Variable, symbolic::Variable > & edge_constituent_vertex_durations | ( | ) | const |
Returns a pair of placeholder decision variables (not actually declared as decision variables in the MathematicalProgram) associated with the time scaling of the trajectory in two sets within this subgraph that are connected by an internal edge.
These variables will be substituted for real decision variables in methods like AddEdgeCost and AddEdgeConstraint. Passing this variable directly into objectives/constraints will result in an error.
| const std::vector< geometry::optimization::GraphOfConvexSets::Edge * > & Edges | ( | ) |
Returns constant reference to a vector of mutable pointers to the edges.
| std::vector< const geometry::optimization::GraphOfConvexSets::Edge * > Edges | ( | ) | const |
Returns pointers to the edges stored in the subgraph.
| const std::string & name | ( | ) | const |
Returns the name of the subgraph.
| int order | ( | ) | const |
Returns the order of the Bézier trajectory within the region.
| const geometry::optimization::ConvexSets & regions | ( | ) | const |
Returns the regions associated with this subgraph before the CartesianProduct.
| int size | ( | ) | const |
Returns the number of vertices in the subgraph.
| const solvers::MatrixXDecisionVariable & vertex_control_points | ( | ) | const |
Returns a placeholder decision variable (not actually declared as a decision variable in the MathematicalProgram) associated with the control points of the trajectory in a set within this subgraph.
The variable will be of shape (num_positions(), order+1), where the ith column is the ith control point. This variable will be substituted for real decision variables in methods like AddVertexCost and AddVertexConstraint. Passing this variable directly into objectives/constraints will result in an error.
| const symbolic::Variable & vertex_duration | ( | ) | const |
Returns a placeholder decision variable (not actually declared as a decision variable in the MathematicalProgram) associated with the time scaling of the trajectory in a set within this subgraph.
This variable will be substituted for real decision variables in methods like AddVertexCost and AddVertexConstraint. Passing this variable directly into objectives/constraints will result in an error.
| const std::vector< geometry::optimization::GraphOfConvexSets::Vertex * > & Vertices | ( | ) |
Returns constant reference to a vector of mutable pointers to the vertices stored in the subgraph.
The order of the vertices is the same as the order the regions were added.
| std::vector< const geometry::optimization::GraphOfConvexSets::Vertex * > Vertices | ( | ) | const |
Returns pointers to the vertices stored in the subgraph.
The order of the vertices is the same as the order the regions were added.
|
friend |