Detailed Description

GraphOfConvexSets (GCS) implements the design pattern and optimization problems first introduced in the paper "Shortest Paths in Graphs of Convex Sets".

"Shortest Paths in Graphs of Convex Sets" by Tobia Marcucci, Jack Umenberger, Pablo A. Parrilo, Russ Tedrake. https://arxiv.org/abs/2101.11565

Warning: This feature is considered to be experimental and may change or be removed at any time, without any deprecation notice ahead of time.

Each vertex in the graph is associated with a convex set over continuous variables, edges in the graph contain convex costs and constraints on these continuous variables. We can then formulate optimization problems over this graph, such as the shortest path problem where each visit to a vertex also corresponds to selecting an element from the convex set subject to the costs and constraints. Behind the scenes, we construct efficient mixed-integer convex transcriptions of the graph problem using MathematicalProgram. However, we provide the option to solve an often tight convex relaxation of the problem with GraphOfConvexSetsOptions::convex_relaxation and employ a cheap rounding stage which solves the convex restriction along potential paths to find a feasible solution to the original problem.

Design note: This class avoids providing any direct access to the MathematicalProgram that it constructs nor to the decision variables / constraints. The users should be able to write constraints against "placeholder" decision variables on the vertices and edges, but these get translated in non-trivial ways to the underlying program.

Advanced Usage: Guiding Non-convex Optimization with the GraphOfConvexSets

Solving a GCS problem using convex relaxation involves two components:

Convex Relaxation: The relaxation of the binary variables (edge activations) and perspective operations on the convex cost/constraints leads to a convex problem that considers the graph as a whole.
Rounding: After solving the relaxation, a randomized rounding scheme is applied to obtain a feasible solution for the original problem. We interpret the relaxed flow variables as edge probabilities to guide the maximum likelyhood depth first search from the source to target vertices. Each rounding is calling SolveConvexRestriction.

To handle non-convex constraints, one can provide convex surrogates to the relaxation and the true non-convex constraints to the rounding problem. These surrogates approximate the non-convex constraints, making the relaxation solvable as a convex optimization to guide the non-convex rounding. This can be controlled by the Transcription enum in the AddConstraint method. We encourage users to provide a strong convex surrogate, when possible, to better approximate the original non-convex problem.

Users can also specify a GCS implicitly, which can be important for very large or infinite graphs, by deriving from ImplicitGraphOfConvexSets.

#include <drake/geometry/optimization/graph_of_convex_sets.h>

Classes
class	Edge
	An edge in the graph connects between vertex `u` and vertex `v`. More...

class	Vertex
	Each vertex in the graph has a corresponding ConvexSet, and a std::string name. More...

Public Types
enum	Transcription { kMIP, kRelaxation, kRestriction }
	Specify the transcription of the optimization problem to which a constraint or cost should be added, or from which they should be retrieved. More...

using	VertexId = Identifier< class VertexTag >

using	EdgeId = Identifier< class EdgeTag >

Public Member Functions
	GraphOfConvexSets ()=default
	Constructs an empty graph. More...

virtual	~GraphOfConvexSets ()

std::unique_ptr< GraphOfConvexSets >	Clone () const
	Returns a deep copy of this graph. More...

Vertex *	AddVertex (const ConvexSet &set, std::string name="")
	Adds a vertex to the graph. More...

Vertex *	AddVertexFromTemplate (const Vertex &template_vertex)
	Adds a new vertex to the graph (and assigns a new unique VertexId) by taking the name, costs, and constraints (but not any edges) from `template_vertex`. More...

Edge *	AddEdge (Vertex u, Vertex v, std::string name="")
	Adds an edge to the graph from Vertex `u` to Vertex `v`. More...

Edge *	AddEdgeFromTemplate (Vertex u, Vertex v, const Edge &template_edge)
	Adds an edge to the graph from Vertex `u` to Vertex `v` (and assigns a new unique EdgeId), by taking the name, costs, and constraints from `template_edge`. More...

const Vertex *	GetVertexByName (const std::string &name) const
	Returns the first vertex (by the order added to `this`) with the given name, or nullptr if no such vertex exists. More...

Vertex *	GetMutableVertexByName (const std::string &name)
	Returns the first vertex (by the order added to `this`) with the given name, or nullptr if no such vertex exists. More...

const Edge *	GetEdgeByName (const std::string &name) const
	Returns the first edge (by the order added to `this`) with the given name, or nullptr if no such edge exists. More...

Edge *	GetMutableEdgeByName (const std::string &name)
	Returns the first edge (by the order added to `this`) with the given name, or nullptr if no such edge exists. More...

void	RemoveVertex (Vertex *vertex)
	Removes vertex `vertex` from the graph as well as any edges from or to the vertex. More...

void	RemoveEdge (Edge *edge)
	Removes edge `edge` from the graph. More...

int	num_vertices () const

int	num_edges () const

std::vector< Vertex * >	Vertices ()
	Returns mutable pointers to the vertices stored in the graph. More...

std::vector< const Vertex * >	Vertices () const
	Returns pointers to the vertices stored in the graph. More...

bool	IsValid (const Vertex &v) const
	Returns true iff `v` is registered as a vertex with `this`. More...

std::vector< Edge * >	Edges ()
	Returns mutable pointers to the edges stored in the graph. More...

std::vector< const Edge * >	Edges () const
	Returns pointers to the edges stored in the graph. More...

bool	IsValid (const Edge &e) const
	Returns true iff `e` is registered as an edge with `this`. More...

void	ClearAllPhiConstraints ()
	Removes all constraints added to any edge with AddPhiConstraint. More...

std::string	GetGraphvizString (const solvers::MathematicalProgramResult result=nullptr, const GcsGraphvizOptions &options=GcsGraphvizOptions(), const std::vector< const Edge > *active_path=nullptr) const
	Returns a Graphviz string describing the graph vertices and edges. More...

solvers::MathematicalProgramResult	SolveShortestPath (const Vertex &source, const Vertex &target, const GraphOfConvexSetsOptions &options=GraphOfConvexSetsOptions()) const
	Formulates and solves the mixed-integer convex formulation of the shortest path problem on the graph, as discussed in detail in. More...

std::vector< const Edge * >	GetSolutionPath (const Vertex &source, const Vertex &target, const solvers::MathematicalProgramResult &result, double tolerance=1e-3) const
	Extracts a path from `source` to `target` described by the `result` returned by SolveShortestPath(), via depth-first search following the largest values of the edge binary variables. More...

std::vector< std::vector< const Edge * > >	SamplePaths (const Vertex &source, const Vertex &target, const std::unordered_map< const Edge *, double > &flows, const GraphOfConvexSetsOptions &options) const
	Samples a collection of unique paths from `source` to `target`, where the flow values (the relaxed binary variables associated with each `Edge`) `flows` are interpreted as the probabilities of transitioning an edge. More...

std::vector< std::vector< const Edge * > >	SamplePaths (const Vertex &source, const Vertex &target, const solvers::MathematicalProgramResult &result, const GraphOfConvexSetsOptions &options) const
	Samples a collection of unique paths from `source` to `target`, where the flow values (the relaxed binary variables associated with each `Edge`) in `result` are interpreted as the probabilities of transitioning an edge. More...

solvers::MathematicalProgramResult	SolveConvexRestriction (const std::vector< const Edge * > &active_edges, const GraphOfConvexSetsOptions &options=GraphOfConvexSetsOptions(), const solvers::MathematicalProgramResult *initial_guess=nullptr) const
	The non-convexity in a GCS problem comes from the binary variables (phi) associated with the edges being active or inactive in the solution. More...

Does not allow copy, move, or assignment
	GraphOfConvexSets (const GraphOfConvexSets &)=delete

GraphOfConvexSets &	operator= (const GraphOfConvexSets &)=delete

	GraphOfConvexSets (GraphOfConvexSets &&)=delete

GraphOfConvexSets &	operator= (GraphOfConvexSets &&)=delete

Friends
class	PreprocessShortestPathTest

Member Typedef Documentation

◆ EdgeId

using EdgeId = Identifier<class EdgeTag>

◆ VertexId

using VertexId = Identifier<class VertexTag>

Member Enumeration Documentation

◆ Transcription

enum Transcription

strong

Specify the transcription of the optimization problem to which a constraint or cost should be added, or from which they should be retrieved.

Enumerator
kMIP	The mixed integer formulation of the GCS problem.
kRelaxation	The relaxation of the GCS problem.
kRestriction	The restrction of the GCS problem where the path is fixed.

Constructor & Destructor Documentation

◆ GraphOfConvexSets() [1/3]

GraphOfConvexSets ( const GraphOfConvexSets & )

delete

◆ GraphOfConvexSets() [2/3]

GraphOfConvexSets ( GraphOfConvexSets && )

delete

◆ GraphOfConvexSets() [3/3]

GraphOfConvexSets ( )

default

Constructs an empty graph.

◆ ~GraphOfConvexSets()

virtual ~GraphOfConvexSets ( )

virtual

Member Function Documentation

◆ AddEdge()

Edge* AddEdge	(	Vertex *	u,
		Vertex *	v,
		std::string	name = `""`
	)

Adds an edge to the graph from Vertex u to Vertex v.

The vertex references must refer to valid vertices in this graph. If name is empty then a default name will be provided.

Exceptions

std::exception if u or v are not valid vertices in this graph.

◆ AddEdgeFromTemplate()

Edge* AddEdgeFromTemplate	(	Vertex *	u,
		Vertex *	v,
		const Edge &	template_edge
	)

Adds an edge to the graph from Vertex u to Vertex v (and assigns a new unique EdgeId), by taking the name, costs, and constraints from template_edge.

template_edge does not need to be registered with this GCS instance; this method can be used to effectively copy an Edge from another GCS instance into this.

Exceptions

std::exception	if `u` or `v` are not valid vertices in this graph.
std::exception	if `u` or `v` do not match the sizes of the `template_edge.u()` and `template_edge.v()` vertices.
std::exception	if edges have slack variables. We can add this support once it's needed.

◆ AddVertex()

Vertex* AddVertex	(	const ConvexSet &	set,
		std::string	name = `""`
	)

Adds a vertex to the graph.

A copy of set is cloned and stored inside the graph. If name is empty then a default name will be provided.

◆ AddVertexFromTemplate()

Vertex* AddVertexFromTemplate ( const Vertex & template_vertex )

Adds a new vertex to the graph (and assigns a new unique VertexId) by taking the name, costs, and constraints (but not any edges) from template_vertex.

template_vertex does not need to be registered with this GCS instance; this method can be used to effectively copy a Vertex from another GCS instance into this.

◆ ClearAllPhiConstraints()

void ClearAllPhiConstraints ( )

Removes all constraints added to any edge with AddPhiConstraint.

◆ Clone()

std::unique_ptr<GraphOfConvexSets> Clone ( ) const

Returns a deep copy of this graph.

Exceptions

std::exception if edges have slack variables. We can add this support once it's needed.

◆ Edges() [1/2]

std::vector<Edge*> Edges ( )

Returns mutable pointers to the edges stored in the graph.

◆ Edges() [2/2]

std::vector<const Edge*> Edges ( ) const

Returns pointers to the edges stored in the graph.

◆ GetEdgeByName()

const Edge* GetEdgeByName ( const std::string & name ) const

Returns the first edge (by the order added to this) with the given name, or nullptr if no such edge exists.

◆ GetGraphvizString()

std::string GetGraphvizString	(	const solvers::MathematicalProgramResult *	result = `nullptr`,
		const GcsGraphvizOptions &	options = `GcsGraphvizOptions()`,
		const std::vector< const Edge * > *	active_path = `nullptr`
	)		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, according to options.

Parameters

result	the optional result from a solver.
options	the struct containing various options for visualization.
active_path	optionally highlights a given path in the graph. The path is displayed as dashed edges in red, displayed in addition to the original graph edges.

◆ GetMutableEdgeByName()

Edge* GetMutableEdgeByName ( const std::string & name )

Returns the first edge (by the order added to this) with the given name, or nullptr if no such edge exists.

◆ GetMutableVertexByName()

Vertex* GetMutableVertexByName ( const std::string & name )

Returns the first vertex (by the order added to this) with the given name, or nullptr if no such vertex exists.

◆ GetSolutionPath()

std::vector<const Edge*> GetSolutionPath	(	const Vertex &	source,
		const Vertex &	target,
		const solvers::MathematicalProgramResult &	result,
		double	tolerance = `1e-3`
	)		const

Extracts a path from source to target described by the result returned by SolveShortestPath(), via depth-first search following the largest values of the edge binary variables.

Parameters

tolerance defines the threshold for checking the integrality conditions of the binary variables for each edge. tolerance = 0 would demand that the binary variables are exactly 1 for the edges on the path. tolerance = 1 would allow the binary variables to be any value in [0, 1]. The default value is 1e-3.

Exceptions

std::exception if !result.is_success() or no path from source to target can be found in the solution.

◆ GetVertexByName()

const Vertex* GetVertexByName ( const std::string & name ) const

Returns the first vertex (by the order added to this) with the given name, or nullptr if no such vertex exists.

◆ IsValid() [1/2]

bool IsValid ( const Vertex & v ) const

Returns true iff v is registered as a vertex with this.

◆ IsValid() [2/2]

bool IsValid ( const Edge & e ) const

Returns true iff e is registered as an edge with this.

◆ num_edges()

int num_edges ( ) const

◆ num_vertices()

int num_vertices ( ) const

◆ operator=() [1/2]

GraphOfConvexSets& operator= ( GraphOfConvexSets && )

delete

◆ operator=() [2/2]

GraphOfConvexSets& operator= ( const GraphOfConvexSets & )

delete

◆ RemoveEdge()

void RemoveEdge ( Edge * edge )

Removes edge edge from the graph.

Precondition: The edge must be part of the graph.

◆ RemoveVertex()

void RemoveVertex ( Vertex * vertex )

Removes vertex vertex from the graph as well as any edges from or to the vertex.

Runtime is O(nₑ) where nₑ is the number of edges in the graph.

Precondition: The vertex must be part of the graph.

◆ SamplePaths() [1/2]

std::vector<std::vector<const Edge*> > SamplePaths	(	const Vertex &	source,
		const Vertex &	target,
		const std::unordered_map< const Edge *, double > &	flows,
		const GraphOfConvexSetsOptions &	options
	)		const

Samples a collection of unique paths from source to target, where the flow values (the relaxed binary variables associated with each Edge) flows are interpreted as the probabilities of transitioning an edge.

The returned paths are guaranteed to be unique, and the number of returned paths can be 0 if no paths are found. This function implements the first part of the rounding scheme put forth in Section 4.2 of "Motion Planning around Obstacles with Convex Optimization": https://arxiv.org/abs/2205.04422

Parameters

source	specifies the source vertex.
target	specifies the target vertex.
flows	specifies the edge flows, which are interprested as the probability of transition an edge. Edge flows that are not specified are taken to be zero.
options	include all settings for sampling the paths. Specifically, the behavior of this function is determined through `options.rounding_seed`, `options.max_rounded_paths`, `options.max_rounding_trials`, and `options.flow_tolerance`, as described in `GraphOfConvexSetsOptions`.

Returns: A vector of paths, where each path is a vector of Edges.

Exceptions

std::exception if options.max_rounded_path < 1.

◆ SamplePaths() [2/2]

std::vector<std::vector<const Edge*> > SamplePaths	(	const Vertex &	source,
		const Vertex &	target,
		const solvers::MathematicalProgramResult &	result,
		const GraphOfConvexSetsOptions &	options
	)		const

Samples a collection of unique paths from source to target, where the flow values (the relaxed binary variables associated with each Edge) in result are interpreted as the probabilities of transitioning an edge.

The returned paths are guaranteed to be unique, and the number of returned paths can be 0 if no paths are found. This function implements the first part of the rounding scheme put forth in Section 4.2 of "Motion Planning around Obstacles with Convex Optimization": https://arxiv.org/abs/2205.04422

Parameters

source	specifies the source vertex.
target	specifies the target vertex.
options	include all settings for sampling the paths. Specifically, the behavior of this function is determined through `options.rounding_seed`, `options.max_rounded_paths`, `options.max_rounding_trials`, and `options.flow_tolerance`, as described in `GraphOfConvexSetsOptions`.

Returns: A vector of paths, where each path is a vector of Edges.

Exceptions

std::exception if options.max_rounded_path < 1.

◆ SolveConvexRestriction()

solvers::MathematicalProgramResult SolveConvexRestriction	(	const std::vector< const Edge * > &	active_edges,
		const GraphOfConvexSetsOptions &	options = `GraphOfConvexSetsOptions()`,
		const solvers::MathematicalProgramResult *	initial_guess = `nullptr`
	)		const

The non-convexity in a GCS problem comes from the binary variables (phi) associated with the edges being active or inactive in the solution.

If those binary variables are fixed, then the problem is convex – this is a so-called "convex restriction" of the original problem.

The convex restriction can often be solved much more efficiently than solving the full GCS problem with additional constraints to fix the binaries; it can be written using less decision variables, and needs only to include the vertices associated with at least one of the active edges. Decision variables for all other convex sets will be set to NaN.

Note that one can specify additional non-convex constraints, which may be not supported by all solvers. In this case, the provided solver will throw an exception.

If an initial_guess is provided, the solution inside this result will be used to set the initial guess for the convex restriction. Typically, this will be the result obtained by solving the convex relaxation.

Exceptions

std::exception if the initial_guess does not contain solutions for the decision variables required in this convex restriction.

◆ SolveShortestPath()

solvers::MathematicalProgramResult SolveShortestPath	(	const Vertex &	source,
		const Vertex &	target,
		const GraphOfConvexSetsOptions &	options = `GraphOfConvexSetsOptions()`
	)		const

Formulates and solves the mixed-integer convex formulation of the shortest path problem on the graph, as discussed in detail in.

"Shortest Paths in Graphs of Convex Sets" by Tobia Marcucci, Jack Umenberger, Pablo A. Parrilo, Russ Tedrake. https://arxiv.org/abs/2101.11565

Parameters

source	specifies the source set. The solver will choose any point in that set; to start at a particular continuous state consider adding a Point set to the graph and using that as the source.
target	specifies the target set. The solver will choose any point in that set.
options	include all settings for solving the shortest path problem. See `GraphOfConvexSetsOptions` for further details. The following default options will be used if they are not provided in `options`: `options.convex_relaxation = false`, `options.max_rounded_paths = 0`, `options.preprocessing = false`.

Exceptions

std::exception if any of the costs or constraints in the graph are incompatible with the shortest path formulation or otherwise unsupported. All costs must be non-negative for all values of the continuous variables.

◆ Vertices() [1/2]

std::vector<Vertex*> Vertices ( )

Returns mutable pointers to the vertices stored in the graph.

◆ Vertices() [2/2]

std::vector<const Vertex*> Vertices ( ) const

Returns pointers to the vertices stored in the graph.

Friends And Related Function Documentation

◆ PreprocessShortestPathTest

friend class PreprocessShortestPathTest

friend

The documentation for this class was generated from the following file:

drake/geometry/optimization/graph_of_convex_sets.h

Detailed Description

Classes

Public Types

Public Member Functions

Friends

Member Typedef Documentation

◆ EdgeId

◆ VertexId

Member Enumeration Documentation

◆ Transcription

Constructor & Destructor Documentation

◆ GraphOfConvexSets() [1/3]

◆ GraphOfConvexSets() [2/3]

◆ GraphOfConvexSets() [3/3]

◆ ~GraphOfConvexSets()

Member Function Documentation

◆ AddEdge()

◆ AddEdgeFromTemplate()

◆ AddVertex()

◆ AddVertexFromTemplate()

◆ ClearAllPhiConstraints()

◆ Clone()

◆ Edges() [1/2]

◆ Edges() [2/2]

◆ GetEdgeByName()

◆ GetGraphvizString()

◆ GetMutableEdgeByName()

◆ GetMutableVertexByName()

◆ GetSolutionPath()

◆ GetVertexByName()

◆ IsValid() [1/2]

◆ IsValid() [2/2]

◆ num_edges()

◆ num_vertices()

◆ operator=() [1/2]

◆ operator=() [2/2]

◆ RemoveEdge()

◆ RemoveVertex()

◆ SamplePaths() [1/2]

◆ SamplePaths() [2/2]

◆ SolveConvexRestriction()

◆ SolveShortestPath()

◆ Vertices() [1/2]

◆ Vertices() [2/2]

Friends And Related Function Documentation

◆ PreprocessShortestPathTest