Non-Euclidean Motion Planning with Graphs of Geodesically-Convex Sets

Robot Locomotion Group

Thomas Cohn, Mark Petersen, Max Simchowitz, Russ Tedrake

ICCOPT 2025

Outline

  1. Motivation
    1. Robot Motion Planning
  2. Context
    1. Mixed-Integer Optimization
    2. Optimization on Manifolds
  3. Our Contributions
    1. Methodology
    2. Experimental Results
    3. Beyond Flat Manifolds
  4. Conclusion

Non-Euclidean Motion Planning with Graphs of Geodesically-Convex Sets

Thomas Cohn, Mark Petersen, Max Simchowitz, and Russ Tedrake

Part 1

Motivation

Non-Euclidean Motion Planning with Graphs of Geodesically-Convex Sets

Thomas Cohn, Mark Petersen, Max Simchowitz, and Russ Tedrake

Motion Planning

https://mapsplatform.google.com/maps-products/routes/

Broadly speaking

  • How can I get from Point A to Point B?
  • Objectives?
    • Distance
    • Time
    • Gas
  • Constraints?
    • Road closures
    • Avoid tolls
    • Avoid ferries

Non-Euclidean Motion Planning with Graphs of Geodesically-Convex Sets

Thomas Cohn, Mark Petersen, Max Simchowitz, and Russ Tedrake

Robot Motion Planning

What has changed?

  • No more roads to follow
    • We can move freely
    • Have to avoid obstacles
  • More complex motions
    • Not just worrying about position in 2D
    • Robot arms move in strange ways

Non-Euclidean Motion Planning with Graphs of Geodesically-Convex Sets

Thomas Cohn, Mark Petersen, Max Simchowitz, and Russ Tedrake

Configuration-Space Planning

https://github.com/ethz-asl/amr_visualisations

Task Space

Configuration Space

Non-Euclidean Motion Planning with Graphs of Geodesically-Convex Sets

Thomas Cohn, Mark Petersen, Max Simchowitz, and Russ Tedrake

Planning with Roadmaps

Michigan Robotics 320, Lecture 15

Standard algorithms include PRM (Kavraki 1996), RRT (LaValle 1998), EST (Hsu 1999), and PRM* (Karaman 2011).

Non-Euclidean Motion Planning with Graphs of Geodesically-Convex Sets

Thomas Cohn, Mark Petersen, Max Simchowitz, and Russ Tedrake

Trajectory Optimization

Underactuated Robotics, Russ Tedrake

Control points of a trajectory are decision variables.

 

Use costs and constraints to enforce objectives, dynamics, and more.

Trajectory optimization approaches include STOMP (Kalakrishnan 2011), CHOMP (Zucker 2013), KOMO (Toussaint 2014), ALTRO (Howell 2019), cuRobo (Sundaralingam 2023).

Non-Euclidean Motion Planning with Graphs of Geodesically-Convex Sets

Thomas Cohn, Mark Petersen, Max Simchowitz, and Russ Tedrake

Motion Planning Today

  • Sampling-based planning 
    • +
       Global (probabilistic) completeness
    • -
       "Curse of Dimensionality"
    • -
       Non-smooth paths
  • Trajectory optimization 
    • -
       Nonconvex
    • +
       Scales with dimension
    • +
       Smooth paths, dynamics

Motion Planning, Wikipedia

Non-Euclidean Motion Planning with Graphs of Geodesically-Convex Sets

Thomas Cohn, Mark Petersen, Max Simchowitz, and Russ Tedrake

Part 2

Context

Non-Euclidean Motion Planning with Graphs of Geodesically-Convex Sets

Thomas Cohn, Mark Petersen, Max Simchowitz, and Russ Tedrake

Nonconvexity in TrajOpt

Reproduced from Slides for the Technion Robotics Seminar, Russ Tedrake

Non-Euclidean Motion Planning with Graphs of Geodesically-Convex Sets

Thomas Cohn, Mark Petersen, Max Simchowitz, and Russ Tedrake

Planning through Convex Sets

Motion Planning around Convex Obstacles with Convex Optimization, Marcucci et. al.

  • Want a path from \(q_0\) to \(q_T\)
  • Red region: obstacle
  • Blue regions (\(\mathcal Q_i\)): safe sets

Convex obstacle avoidance guarantees!

  • Piecewise-linear trajectories: just check endpoints
  • Bezier curves: check control points

Non-Euclidean Motion Planning with Graphs of Geodesically-Convex Sets

Thomas Cohn, Mark Petersen, Max Simchowitz, and Russ Tedrake

A Graph of Convex Sets

A directed graph \(G=(V,E)\)

  • For each vertex \(v\in V\), there is a convex set \(\mathcal{X}_v\)
  • For each edge \(e=(u,v)\in E\), there is a convex cost function \[\ell_e:\mathcal{X}_u\times\mathcal{X}_v\to\mathbb{R}\]
  • Formulate and solve as a MICP
    • Builds off network flow formulation of graph shortest path
    • Each edge gets a binary variable

Main Reference: Shortest Paths in Graphs of Convex Sets, Marcucci et. al.

Non-Euclidean Motion Planning with Graphs of Geodesically-Convex Sets

Thomas Cohn, Mark Petersen, Max Simchowitz, and Russ Tedrake

A Graph of Convex Sets

\(\mathcal X_u\)

\(\mathcal X_v\)

\(\ell_{(u,v)}\)

Non-Euclidean Motion Planning with Graphs of Geodesically-Convex Sets

Thomas Cohn, Mark Petersen, Max Simchowitz, and Russ Tedrake

Configuration Space as a Manifold

Trajectory Optimization on Manifolds: A Theoretically-Guaranteed Embedded Sequential Convex Programming Approach, Bonalli et. al.

Non-Euclidean Motion Planning with Graphs of Geodesically-Convex Sets

Thomas Cohn, Mark Petersen, Max Simchowitz, and Russ Tedrake

Riemannian Metric

A Riemannian Metric assigns an inner product \(\left<\cdot,\cdot\right>_g\) to each tangent space of a manifold \(\mathcal M\).

The "speed" of a curve \(\gamma:(a,b)\to\mathcal M\) at time \(t\) is \(\sqrt{\left<\dot\gamma(t),\dot\gamma(t)\right>_g}\)

 

The arc length of \(\gamma\) is \[ L(\gamma)=\int_a^b \sqrt{\left<\dot\gamma(t),\dot\gamma(t)\right>_{g}}\,\mathrm dt\]

 

Shortest path planning: want to minimize \(L(\gamma)\).

Non-Euclidean Motion Planning with Graphs of Geodesically-Convex Sets

Thomas Cohn, Mark Petersen, Max Simchowitz, and Russ Tedrake

Geodesics: Shortest Paths

The arc length equation gives us a distance metric

\[d(p,q)=\inf\{L(\gamma)\,|\,\gamma\in\mathcal{C}^{1}_{\operatorname{pw}}([0,1],\mathcal{M}),\\\gamma(0)=p,\gamma(1)=q\}\]

Curves which minimize this function are called geodesics.

 

Without any obstacles, the solution to the shortest-path problem is a geodesic.

Non-Euclidean Motion Planning with Graphs of Geodesically-Convex Sets

Thomas Cohn, Mark Petersen, Max Simchowitz, and Russ Tedrake

Convex Sets

A set \(X\subseteq\mathbb{R}^n\) is convex if \(\forall p,q\in X\), the line connecting \(p\) and \(q\) is contained in \(X\)

Geodesically Convex Sets

A set \(X\subseteq\mathcal{M}\) is geodesically convex (or g-convex) if \(\forall p,q\in X\), there is a unique length-minimizing geodesic (w.r.t. \(\mathcal{M}\)) connecting \(p\) and \(q\), which is completely contained in \(X\).

Convex Set, Wikipedia

Textbook references: Convex Optimization (Boyd 2004) and An Introduction to Optimization on Manifolds (Boumal 2023).

Non-Euclidean Motion Planning with Graphs of Geodesically-Convex Sets

Thomas Cohn, Mark Petersen, Max Simchowitz, and Russ Tedrake

Some Sets Look Convex

(But Aren't)

Sets can be convex in isometric local coordinates, but not geodesically-convex on the manifold

 

Radius of convexity: how large can a set be and still be convex?

Which Continent Is Situated In All Four Hemispheres?, Junior

Non-Euclidean Motion Planning with Graphs of Geodesically-Convex Sets

Thomas Cohn, Mark Petersen, Max Simchowitz, and Russ Tedrake

Convex Functions

A function \(f:X\to\mathbb{R}\) is convex if for any \(x,y\in X\) and \(\lambda\in[0,1]\), we have

 

\[f(\lambda x+(1-\lambda)y)\le\]

\[\lambda f(x)+(1-\lambda) f(y)\]

Geodesically Convex Functions

A function \(f:\mathcal{M}\to\mathbb{R}\) is geodesically convex if for any \(x,y\in \mathcal{M}\) and any geodesic \(\gamma:[0,1]\to\mathcal{M}\) such that \(\gamma(0)=x\) and \(\gamma(1)=y\), \(f\circ\gamma\) is convex

Non-Euclidean Motion Planning with Graphs of Geodesically-Convex Sets

Thomas Cohn, Mark Petersen, Max Simchowitz, and Russ Tedrake

Challenge: Geodesic Loops

If \(\mathcal M\) has a geodesic loop, then every g-convex function is constant. Most robot configuration spaces have geodesic loops.

Non-Euclidean Motion Planning with Graphs of Geodesically-Convex Sets

Thomas Cohn, Mark Petersen, Max Simchowitz, and Russ Tedrake

Solution: In some cases, we can restrict the domain of the function to a domain on which it is g-convex. This is not always possible.

Example: \(\mathrm d:S^1\times S^1\to\mathbb R\), the Riemannian distance between two points on the unit circle, is g-convex if both points lie in the interior of a common semicircle.

Part 3a

Methodology

Non-Euclidean Motion Planning with Graphs of Geodesically-Convex Sets

Thomas Cohn, Mark Petersen, Max Simchowitz, and Russ Tedrake

Problem Statement

Configuration space \(\mathcal{Q}\) as a Riemannian manifold

\(\mathcal{M}\subseteq\mathcal{Q}\) is the set of collision free configurations, \(\bar{\mathcal{M}}\) its closure

The shortest path between \(p,q\in\bar{\mathcal{M}}\) is the solution to the optimization problem

 

 

\[\begin{array}{rl}    \argmin & L(\gamma)\\    \textrm{subject to} & \gamma\in\mathcal{C}^{1}_{\operatorname{pw}}([0,1],\bar{\mathcal{M}})\\    & \gamma(0)=p\\    & \gamma(1)=q\end{array}\]

Non-Euclidean Motion Planning with Graphs of Geodesically-Convex Sets

Thomas Cohn, Mark Petersen, Max Simchowitz, and Russ Tedrake

A Graph of Geodesically Convex Sets (GGCS)

A directed graph \(G=(V,E)\)

  • For each vertex \(v\in V\), there is a g-convex set \(\mathcal{Y}_v\)
  • For each edge \(e=(u,v)\in E\), there is a cost function \(\ell_e^\mathcal{Y}:\mathcal{Y}_u\times\mathcal{Y}_v\to\mathbb{R}\)
  • \(\ell_e^\mathcal{Y}\) must be g-convex on \(\mathcal{Y}_u\times\mathcal{Y}_v\)

Non-Euclidean Motion Planning with Graphs of Geodesically-Convex Sets

Thomas Cohn, Mark Petersen, Max Simchowitz, and Russ Tedrake

GCS

Non-Euclidean Motion Planning with Graphs of Geodesically-Convex Sets

Thomas Cohn, Mark Petersen, Max Simchowitz, and Russ Tedrake

GGCS

Non-Euclidean Motion Planning with Graphs of Geodesically-Convex Sets

Thomas Cohn, Mark Petersen, Max Simchowitz, and Russ Tedrake

Shortest Paths in GGCS

For each \(v\in V\), let \(\psi_v:\mathcal{Y}_v\to\mathbb{R}^n\) be a coordinate chart

Define \(X_v=\psi_v(\mathcal{Y}_v)\)

For each \(e=(u,v)\in E\), new edge cost:

\[\ell_e(x_u,x_v)=\ell_e^\mathcal{Y}(\psi_u^{-1}(x_u),\psi_v^{-1}(x_v))\]

If all \(X_v\) and \(\ell_e\) are convex, we have a valid GCS problem

Mixed-Integer Riemannian Optimization? No existing literature.

Better option: transform into a GCS problem

Non-Euclidean Motion Planning with Graphs of Geodesically-Convex Sets

Thomas Cohn, Mark Petersen, Max Simchowitz, and Russ Tedrake

Transforming Back to a GCS

Non-Euclidean Motion Planning with Graphs of Geodesically-Convex Sets

Thomas Cohn, Mark Petersen, Max Simchowitz, and Russ Tedrake

Nice Theoretical Results?

  • There is a shortest path \(\gamma\) connecting \(p\) to \(q\) through \(\bar{\mathcal{M}}\) such that
    • \(\gamma\) is piecewise geodesic
    • Each geodesic segment is contained in some \(\bar{X}_u\)
    • \(\gamma\) passes through each \(\bar{X}_u\) at most once
  • Ensures that a shortest path is feasible for GGCS
  •  
  • Formal equivalence of GGCS and transformed GCS problem

Non-Euclidean Motion Planning with Graphs of Geodesically-Convex Sets

Thomas Cohn, Mark Petersen, Max Simchowitz, and Russ Tedrake

When is it Convex?

Have to consider the curvature of the manifold.

Positive curvature

E.g. \(S^n,\operatorname{SO}(3),\operatorname{SE}(3)\)

Negative curvature

E.g. Saddle, PSD Cone \(\vphantom{S^n}\)

Zero curvature

E.g. \(\mathbb R^n, \mathbb T^n\)

?

Non-Euclidean Motion Planning with Graphs of Geodesically-Convex Sets

Thomas Cohn, Mark Petersen, Max Simchowitz, and Russ Tedrake

Zero-Curvature is Still Useful

  • All 1-dimensional manifolds
  • Continuous revolute joints
  • \(\operatorname{SO}(2)\), \(\operatorname{SE}(2)\)
  • Products of flat manifolds

 

 

 

Fetch Robot Configuration

Space: \(\operatorname{SE}(2)\times \mathbb{T}^3\times \mathbb{R}^7\)

Trajectory Control for Differential Drive Mobile Manipulators, Karunakaran and Subbaraj

Non-Euclidean Motion Planning with Graphs of Geodesically-Convex Sets

Thomas Cohn, Mark Petersen, Max Simchowitz, and Russ Tedrake

Part 3b

Experimental Results

Non-Euclidean Motion Planning with Graphs of Geodesically-Convex Sets

Thomas Cohn, Mark Petersen, Max Simchowitz, and Russ Tedrake

Planar Arm

A robot arm with 5 continuous revolute joints in the plane

 

Configuration Space: \(\mathbb{T}^5\)

 

(No self collisions)

Non-Euclidean Motion Planning with Graphs of Geodesically-Convex Sets

Thomas Cohn, Mark Petersen, Max Simchowitz, and Russ Tedrake

Part 3c

Beyond Flat Manifolds

Non-Euclidean Motion Planning with Graphs of Geodesically-Convex Sets

Thomas Cohn, Mark Petersen, Max Simchowitz, and Russ Tedrake

Why is Positive Curvature Bad?

Non-Euclidean Motion Planning with Graphs of Geodesically-Convex Sets

Thomas Cohn, Mark Petersen, Max Simchowitz, and Russ Tedrake

Planning on PL Approximations

Black: Shortest path on PL approximation

Red: Shortest path on sphere

Non-Euclidean Motion Planning with Graphs of Geodesically-Convex Sets

Thomas Cohn, Mark Petersen, Max Simchowitz, and Russ Tedrake

Rough Bounds on Approximation Error

Intuition: How much does the Jacobian of the chart vary?

Intuition: The amount we can "slip" along the boundary also depends on the Hausdorff distance \(\epsilon_H\) between the manifold and its triangulation, as well as the angle \(\alpha_{\operatorname{max}}\) between adjacent charts.

Intuition: The error grows additively with the number of sets traversed in the optimal path.

Non-Euclidean Motion Planning with Graphs of Geodesically-Convex Sets

Thomas Cohn, Mark Petersen, Max Simchowitz, and Russ Tedrake

Assessment

The piecewise-linear approximation strategy is probably

  • computationally intractable for real-world problems
  • only an approximation, whose error is difficult to bound

Non-Euclidean Motion Planning with Graphs of Geodesically-Convex Sets

Thomas Cohn, Mark Petersen, Max Simchowitz, and Russ Tedrake

A More Pragmatic Solution?

Flatten the manifold, and pick a convex surrogate objective.

  • We can construct charts for a variety of robotic systems
  • Post-process with the original nonconvex objective

Kinematic Constraints

 Learning the Metric of Task Constraint Manifolds for Constrained Motion Planning, Zha et. al.

Non-Euclidean Motion Planning with Graphs of Geodesically-Convex Sets

Thomas Cohn, Mark Petersen, Max Simchowitz, and Russ Tedrake

Parametrizing the Constraint Manifold

Constrained Bimanual Planning with Analytic Inverse Kinematics, Thomas Cohn, Seiji Shaw, Max Simchowitz, Russ Tedrake

Non-Euclidean Motion Planning with Graphs of Geodesically-Convex Sets

Thomas Cohn, Mark Petersen, Max Simchowitz, and Russ Tedrake

Nonconvex Objectives

Planning Shorter Paths in Graphs of Convex Sets by Undistorting Parametrized Configuration Spaces, Shruti Garg, Thomas Cohn, and Russ Tedrake

Non-Euclidean Motion Planning with Graphs of Geodesically-Convex Sets

Thomas Cohn, Mark Petersen, Max Simchowitz, and Russ Tedrake

Part 4

Conclusion

Non-Euclidean Motion Planning with Graphs of Geodesically-Convex Sets

Thomas Cohn, Mark Petersen, Max Simchowitz, and Russ Tedrake

Relevant Under-Studied Problems

  • Mixed-integer optimization on manifolds
    • Special cutting planes induced by manifold structure?
  • Optimization on manifolds with additional constraints
    • Either from the intrinsic or extrinsic perspective
  • "Locally" g-convex optimization
    • Or other tricks for dealing with compact manifolds and closed geodesics?

Non-Euclidean Motion Planning with Graphs of Geodesically-Convex Sets

Thomas Cohn, Mark Petersen, Max Simchowitz, and Russ Tedrake

Non-Euclidean Motion Planning with Graphs of Geodesically-Convex Sets

Robot Locomotion Group

Thomas Cohn, Mark Petersen, Max Simchowitz, Russ Tedrake

ICCOPT 2025