|
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. More... | |
| int | order () const |
| Returns the order of the Bézier trajectory within the region. More... | |
| int | size () const |
| Returns the number of vertices in the subgraph. More... | |
| const std::vector< geometry::optimization::GraphOfConvexSets::Vertex * > & | Vertices () |
| Returns constant reference to a vector of mutable pointers to the vertices stored in the subgraph. More... | |
| std::vector< const geometry::optimization::GraphOfConvexSets::Vertex * > | Vertices () const |
| Returns pointers to the vertices stored in the subgraph. More... | |
| const geometry::optimization::ConvexSets & | regions () const |
| Returns the regions associated with this subgraph before the CartesianProduct. More... | |
| void | AddTimeCost (double weight=1.0) |
| Adds a minimum time cost to all regions in the subgraph. 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 subgraph lb ≤ q̇(t) ≤ ub. More... | |
| void | AddPathContinuityConstraints (int continuity_order) |
| Enforces derivative continuity constraints on the subgraph. More... | |
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 |
| ~Subgraph | ( | ) |
| void AddPathContinuityConstraints | ( | int | continuity_order | ) |
Enforces derivative continuity constraints on the subgraph.
| 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 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 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(). |
| 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 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 |