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 timescaling curve, this problem uses a single timescaling variable for each region. This formulation yields continuous trajectories, which are not differentiable at the transition times between the regions since nonconvex 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 postprocessed 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 subtrajectory.
This class supports the notion of a Subgraph of regions. This has proven useful to facilitate multimodal motion planning such as: Subgraph A: find a collisionfree trajectory for the robot to a grasping posture, Subgraph B: find a collisionfree 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 solvers::MathematicalProgramResult *result=nullptr, const geometry::optimization::GcsGraphvizOptions &options={}) 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="", const std::vector< Eigen::VectorXd > *edge_offsets=nullptr) 
Creates a Subgraph with the given regions and indices. More...  
Subgraph &  AddRegions (const geometry::optimization::ConvexSets ®ions, int order, double h_min=1e6, 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, const std::vector< std::pair< int, int >> *edges_between_regions=nullptr, const std::vector< Eigen::VectorXd > *edge_offsets=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  AddPathEnergyCost (const Eigen::MatrixXd &weight_matrix) 
Similar to AddPathLengthCost in usage, but minimizes ∑ weight_matrix * (rᵢ₊₁ − rᵢ)₂². More...  
void  AddPathLengthCost (double weight=1.0) 
Adds multiple L2Norm Costs on the upper bound of the path length. More...  
void  AddPathEnergyCost (double weight=1.0) 
Similar to AddPathLengthCost in usage, but minimizes ∑ (rᵢ₊₁ − rᵢ)₂² with weight being applied uniformly to all dimensions. 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  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 entire graph lb ≤ dᴺq(t) / dtᴺ ≤ ub . More...  
void  AddPathContinuityConstraints (int continuity_order) 
Enforces derivative continuity constraints on the entire graph. More...  
void  AddContinuityConstraints (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 mixedinteger 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, double tol=1e8) 
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 nonnegative, 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. 
void AddContinuityConstraints  (  int  continuity_order  ) 
Enforces derivative continuity constraints on the entire graph.
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 Nonconvex 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 less than one since path continuity is enforced by default. 
EdgesBetweenSubgraphs& AddEdges  (  const Subgraph &  from_subgraph, 
const Subgraph &  to_subgraph,  
const geometry::optimization::ConvexSet *  subspace = nullptr , 

const std::vector< std::pair< int, int >> *  edges_between_regions = nullptr , 

const std::vector< Eigen::VectorXd > *  edge_offsets = 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. 
edges_between_regions  can be used to manually specify which edges should be added, avoiding the intersection checks. It should be a list of tuples (i,j) , where an edge will be added from the i th index region in from_subgraph to the j th index region in to_subgraph . 
edge_offsets  is an optional list of vectors. If defined, the list must contain the same number of entries as edges_between_regions , and the order must match. In other words, if defined, there must be one edge offset for each specified edge, and they must be at the same index. 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 "NonEuclidean Motion
Planning with Graphs of GeodesicallyConvex Sets", and per the discussion in Subsection VI A, τ_uv has no rotation component. If edge_offsets is nullptr, it will instead be computed automatically. 
std::exception  if edge_offsets is provided, but edge_offsets.size() != edges_between_regions.size() . 
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 entire graph 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 Nonconvex 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 lb or ub are not of size num_positions(). 
std::exception  if the derivative order <= 1, since the linear velocity bounds are preferred. 
void AddPathContinuityConstraints  (  int  continuity_order  ) 
Enforces derivative continuity constraints on the entire graph.
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 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.
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 offdiagonal 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.
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. 
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 offdiagonal 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 = "" , 

const std::vector< Eigen::VectorXd > *  edge_offsets = nullptr 

) 
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 nonconvex 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 . 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 "NonEuclidean Motion
Planning with Graphs of GeodesicallyConvex Sets", and per the discussion in Subsection VI A, τ_uv has no rotation component. If edge_offsets is nullptr, it will instead be computed automatically. 
std::exception  if any index referenced in edges_between_regions is outside the range [0, ssize(regions)). 
Subgraph& AddRegions  (  const geometry::optimization::ConvexSets &  regions, 
int  order,  
double  h_min = 1e6 , 

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 nonconvex 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 solvers::MathematicalProgramResult *  result = nullptr , 
const geometry::optimization::GcsGraphvizOptions &  options = {} 

)  const 
Returns a Graphviz string describing the graph vertices and edges.
If result
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 mixedinteger 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. 
tol  The numerical tolerance used to determine if two subsequent segments start and end at the same value modulo 2π for 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 tol 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. 