From the free course Motion Planning with Graphs of Convex Sets on Lykke

The GCS Mathematical Framework

Key concepts: Convex Decomposition · Perspective Functions · Mixed-Integer Programming · Convex Hull

Introduction to the core mechanics of Graphs of Convex Sets, including convex decomposition and the shortest path formulation.

The GCS Mathematical Framework

The Graphs of Convex Sets (GCS) framework represents a paradigm shift in robotic motion planning, effectively bridging the historical divide between discrete graph-search algorithms (like A* or Dijkstra) and continuous trajectory optimization (like Sequential Quadratic Programming). Traditionally, motion planning was forced into a trade-off: one could either search a graph of discrete states to find a globally optimal path that ignores complex dynamics, or use local optimization to find a dynamically feasible trajectory that is prone to getting stuck in local minima. GCS provides a unified mathematical structure that allows for global optimization over continuous paths through a series of convex regions.

At its core, GCS formulates the motion planning problem as a Shortest Path Problem on a Graph of Convex Sets. In this formulation, vertices are not single points in space but rather entire convex regions (such as polytopes or ellipsoids) of the configuration space. Edges represent the transitions between these regions. The "cost" of an edge is not a fixed scalar but a convex function of the continuous variables (the path) within those sets. This allows the framework to solve for the discrete sequence of regions and the continuous trajectory simultaneously, often reaching the global optimum via convex relaxation.

Convex Decomposition: Mapping the Free Space

Before a GCS problem can be solved, the robot's configuration space ($C_{free}$) must be represented as a collection of convex sets. This process, known as Convex Decomposition, is the geometric foundation of the framework. Because collision-avoidance constraints are inherently non-convex (obstacles create "holes" in the space), we cannot optimize over the entire space at once. Instead, we decompose the free space into a set of overlapping convex regions where $x \in \mathcal{X}_i$.

The primary tool for this decomposition in the GCS ecosystem is the IRIS (Iterative Regional Inflation by Semidefinite programming) algorithm. IRIS seeks to find the largest possible convex polytopes that do not contain any obstacles. The algorithm alternates between two main steps:

  1. Finding Separating Hyperplanes: Given an ellipsoid, find the planes that best separate the ellipsoid from the obstacles.
  2. Updating the Maximum Inscribed Ellipsoid: Given the hyperplanes, find the largest ellipsoid that fits within the resulting polytope.

Definition: Convex Polytope A convex polytope $\mathcal{P}$ is defined as the intersection of a finite number of half-spaces, represented mathematically as ${x \in \mathbb{R}^n \mid Ax \le b}$, where $A$ is a matrix of normal vectors and $b$ is a vector of offsets.

Feature Polytopes ($Ax \le b$) Ellipsoids ($(x-c)^T Q (x-c) \le 1$) Voxel Grids
Representational Power High (can fit complex walls) Moderate (smooth) Low (blocky)
Constraint Type Linear Inequalities Quadratic Constraints Discrete Lookups
Optimization Fit Excellent for Linear/Quadratic Programs Good for SOCP Poor for Continuous Opt
Overlap Handling Easy to compute intersections Computationally heavier Trivial

The choice of convex sets is critical. If the sets are too small, the graph becomes excessively large, increasing the combinatorial complexity. If the sets are too large or poorly placed, they may fail to capture the connectivity of a narrow passage. The goal is a "sparse but covering" decomposition where the union of the sets $\bigcup \mathcal{X}i$ covers as much of $C{free}$ as possible.

The Shortest Path Problem on GCS

Once the space is decomposed, we define a graph $G = (V, E)$. Each vertex $v \in V$ is associated with a convex set $X_v \subseteq \mathbb{R}^n$. An edge $e = (u, v) \in E$ exists if the robot can transition from set $X_u$ to set $X_v$. Unlike a standard graph where an edge has a weight $w_{uv}$, a GCS edge has a cost function $J_e(x_u, x_v)$ and constraints on the continuous variables $x_u \in X_u$ and $x_v \in X_v$.

The mathematical objective is to find a sequence of edges (a path) and the specific points within each set along that path that minimize the total cost: $$\min \sum_{e \in \text{Path}} J_e(x_u, x_v)$$ subject to:

  • $x_v \in X_v$ (Set membership)
  • $(x_u, x_v) \in \mathcal{C}_e$ (Transition constraints, such as continuity or dynamics)
  • The path must start at $x_{start}$ and end at $x_{goal}$.

This problem is inherently a Mixed-Integer Convex Program (MICP). The "integer" part comes from the discrete decision of which edges to include in the path (binary variables $z_e \in {0, 1}$), and the "convex" part comes from the optimization of $x$ within the chosen sets.

Perspective Functions: The Key to Convex Relaxation

The most significant mathematical innovation in GCS is the use of Perspective Functions to create a "tight" convex relaxation of the MICP. In a standard Mixed-Integer problem, if we simply relax the binary variables $z_e \in {0, 1}$ to continuous variables $z_e \in [0, 1]$, the resulting optimization often yields "weak" bounds, meaning the solution to the relaxed problem is far from the true integer solution.

To solve this, GCS employs the Perspective of the convex constraints. For a convex function $f(x)$, the perspective function is defined as: $$g(x, z) = z f(x/z)$$ where $z$ is the relaxation of the binary indicator variable.

Why the Perspective Matters

Consider a constraint $x \in \mathcal{X}$ where $\mathcal{X}$ is a convex set. If the edge is "off" ($z=0$), we want $x$ to be forced to zero (or some null value) so it doesn't contribute to the cost. If the edge is "on" ($z=1$), we want the original constraint $x \in \mathcal{X}$ to apply. The perspective transformation $z (A(x/z)) \le z b$, which simplifies to $Ax \le zb$, achieves exactly this. When $z=1$, we get $Ax \le b$. When $z=0$, we get $Ax \le 0$, which (for many sets) forces $x=0$.

Theorem: Convex Hull of the Union of Sets The perspective formulation is mathematically equivalent to finding the Convex Hull of the union of the individual convex constraints. This is the "tightest" possible convex representation of a discrete choice between convex sets.

Mixed-Integer Programming vs. Convex Relaxation

The GCS framework allows the user to choose between solving the exact MICP (which is NP-hard but yields the absolute global optimum) or solving the convex relaxation (which is polynomial time). Remarkably, for many motion planning problems, the convex relaxation is exact or "tight," meaning the $z_e$ variables naturally come out as 0 or 1, or can be easily rounded to a valid path.

Approach Complexity Optimality Use Case
Pure Graph Search (A)* $O(V \log V + E)$ Global (Discrete) High-level routing, no dynamics
Trajectory Opt (SQP) $O(Iteration \times N^3)$ Local Refining a known path
GCS (MICP) Exponential (Worst case) Global (Continuous) Complex environments, high stakes
GCS (Relaxation) Polynomial Often Global Real-time planning, large graphs

The "tightness" of the GCS relaxation stems from the fact that we are not just relaxing the binary variables, but we are lifting the continuous variables into a higher-dimensional space where the union of the convex sets is represented by its convex hull. This is a much stronger formulation than the "Big-M" method often used in older mixed-integer formulations.

Implementation: The GCS Pipeline

A senior engineer implementing GCS for a robotic system (such as a manipulator or a humanoid) follows a specific mathematical pipeline. This pipeline ensures that the resulting trajectory is not only collision-free but also satisfies the dynamic limits of the robot.

  1. Decomposition: Run IRIS to generate a set of polytopes $\mathcal{P}_i$ that cover the free configuration space.
  2. Graph Construction: Create vertices for each $\mathcal{P}_i$. Create edges between vertices if $\mathcal{P}_i \cap \mathcal{P}_j \neq \emptyset$.
  3. Constraint Specification: For each edge, define the continuity constraints. For example, if $x$ represents a spline, ensure that the position and velocity at the end of $x_u$ match the start of $x_v$.
  4. Cost Definition: Define the objective function, typically a quadratic cost like $\int |\ddot{x}(t)|^2 dt$ to minimize control effort (jerk/acceleration).
  5. Solve Relaxation: Solve the resulting large-scale convex program (usually a Second-Order Cone Program or SOCP).
  6. Rounding (if necessary): If the relaxation returns fractional $z_e$ values, use a k-shortest path algorithm on the graph weights to extract the best discrete path, then solve the resulting fixed-path convex optimization.

Worked Example: 2D Point Robot

Imagine a point robot moving from $(0,0)$ to $(10,10)$ with a large obstacle in the middle.

  • Sets: We decompose the space into four overlapping rectangles: $S_1$ (bottom-left), $S_2$ (top-left), $S_3$ (bottom-right), $S_4$ (top-right).
  • Edges: $E = {(S_1, S_2), (S_1, S_3), (S_2, S_4), (S_3, S_4)}$.
  • Decision: The solver must choose between the path $S_1 \to S_2 \to S_4$ or $S_1 \to S_3 \to S_4$.
  • Perspective: The solver evaluates the "flow" $z_e$ through these edges. Because the perspective formulation is used, the solver "sees" the cost of the optimal continuous trajectory within each path during the discrete search.

Variations and Extensions

The GCS framework is highly extensible. Beyond simple point-to-point planning, it has been adapted for several complex robotic tasks:

  • Dynamic Constraints: By representing the trajectory as a B-spline, the derivatives (velocity, acceleration) are also linear functions of the control points. This allows GCS to enforce limits on speed and torque while remaining a convex problem.
  • Task and Motion Planning (TAMP): GCS can handle discrete changes in the environment, such as picking up an object. The "sets" in this case represent different modes of the system (e.g., "Robot moving," "Robot carrying object").
  • Time-Optimal Planning: While minimizing time is non-convex, GCS can be used with a "time-scaling" approach where the path is fixed and the timing is optimized, or by performing a line search over the total duration.

Common Pitfalls and Misconceptions

While GCS is powerful, it is not a "silver bullet." Engineers often encounter specific hurdles during implementation:

  • The "Over-Segmentation" Problem: If the convex decomposition creates too many tiny sets, the number of binary variables in the MICP grows, and even the relaxation can become slow to solve. Efficient decomposition is as important as the solver itself.
  • Numerical Conditioning: The perspective function $z f(x/z)$ involves division by $z$. As $z \to 0$, this can lead to numerical instability in solvers. Practical implementations use a small epsilon ($z + \epsilon$) or specialized conic solver formulations to handle this.
  • Completeness: GCS is "resolution-complete" with respect to the convex decomposition. If a path exists through the union of the sets, GCS will find it. However, if the IRIS decomposition misses a narrow gap, GCS will not find a path through that gap.

Key Insight: GCS is not just a new algorithm; it is a new way of writing down the motion planning problem so that modern, high-performance convex solvers (like Mosek or Gurobi) can do the heavy lifting. It moves the complexity from the "search logic" to the "mathematical formulation."

The GCS Mathematical Framework - Motion Planning with Graphs of Convex Sets - diagram 1
The GCS Mathematical Framework - Motion Planning with Graphs of Convex Sets - diagram 1
The GCS Mathematical Framework - Motion Planning with Graphs of Convex Sets - diagram 2
The GCS Mathematical Framework - Motion Planning with Graphs of Convex Sets - diagram 2

Source Materials

Study Motion Planning with Graphs of Convex Sets on Lykke

This course page is free to open: signed out, you can read its first two sections, and signing in, on any plan, opens the rest. The free Lykke Chrome extension syncs your own Canvas courses, so ChatGPT, Claude and Muse can answer from your classes and deadlines. It only reads Canvas.

Add the free extensionSign in to open every section

All sections of Motion Planning with Graphs of Convex Sets · Browse more course pages

The GCS Mathematical Framework — Motion Planning with Graphs of Convex Sets | Lykke