Literature
Display
← All papers

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

Thomas Cohn · Mark Petersen · Max Simchowitz · Russ Tedrake

· The International Journal of Robotics Research ↗

Robotics12 min text · CondensedOriginal paper ↗
Contents

At a glance

Graphs of Geodesically-Convex Sets (GGCS) extend convex-set motion planning to configuration spaces with periodic angles, including continuous revolute joints and planar mobile bases. On a flat manifold, local distance-preserving coordinates turn region constraints, path costs, and transitions between charts into an ordinary Graph of Convex Sets (GCS) problem.

The construction supports exact mixed-integer optimization or faster convex relaxation and rounding. Practical guarantees apply to the collision-free region cover that has been constructed, and the demonstrated trajectories use approximate rounding. Simulated examples include a toroidal point robot, a planar arm, a modified KUKA iiwa, and a PR2 mobile manipulator.

I Introduction

Sampling planners can struggle to cover high-dimensional configuration spaces, while nonlinear trajectory optimizers can become trapped in poor local solutions. The GCS formulation and GCS motion planner of Marcucci et al. address obstacle-induced nonconvexity by choosing a sequence of convex collision-free regions and optimizing the continuous path within them.

Periodic joints break a naive Euclidean coordinate model: angles differing by a full turn describe the same configuration. GGCS represents that topology with overlapping manifold charts rather than artificial joint limits or duplicated angle ranges. The scope is kinematic planning with suitable convex costs and constraints; nonlinear dynamics, closed kinematic chains, and ball-joint geometry are not handled by this reduction.

Refer to caption
Fig. 1: The start and goal pose for one of our motion planning experiments, using the PR2 bimanual mobile manipulator.

II Related Work

Probabilistic roadmaps and rapidly exploring trees offer asymptotic sampling guarantees, but their practical coverage can be costly. Trajectory optimization directly searches a parameterized curve but generally lacks global guarantees in obstacle-rich problems. Mixed-integer methods encode discrete route choices, with convex relaxations and rounding sometimes reducing their cost substantially.

Riemannian motion policies use geometric metrics to combine controllers. Here, geometry instead supplies a global path-planning formulation over a finite collection of local regions. The reduction reuses ordinary GCS solvers when its flatness and convexity assumptions hold.

III Preliminaries

The construction needs coordinate charts, a metric for measuring path length, and a notion of convexity that respects the configuration space’s topology. The original mathematical definitions are retained below.

III-A Riemannian Geometry

A chart maps a local manifold neighborhood to Euclidean coordinates; overlapping charts are connected by transition maps. Tangent vectors represent velocities, and a Riemannian metric supplies their inner products. Integrating speed gives coordinate-independent curve length; distance is the infimum over connecting curve lengths.

A local isometry preserves the metric. Geodesics are locally length-minimizing constant-speed curves, and the exponential map follows a geodesic from a point in a chosen tangent direction. Curvature measures departure from Euclidean local geometry, illustrated by Figure 2. Flat manifolds have zero sectional curvature, and products of flat manifolds remain flat. The choice of metric is part of this formulation.

Equations 1–2
τ𝒰,𝒱=ψ𝒱∘ψ𝒰−1|ψ𝒰​(𝒰∩𝒱)\tau_{\mathcal{U},\mathcal{V}}^{\vphantom{1}}=\left.\psi_{\mathcal{V}}^{\vphantom{1}}\circ\psi_{\mathcal{U}}^{-1}\right|_{\psi_{\mathcal{U}}^{\vphantom{1}}(\mathcal{U}\cap\mathcal{V})} (1)
L⁡(γ)=∫ab⟨γ′​(s),γ′​(s)⟩γ⁡(s)(ℳ,g)​𝑑sL(\gamma)=\int_{a}^{b}\sqrt{\left<\gamma^{\prime}(s),\gamma^{\prime}(s)\right>_{\gamma(s)}^{(\mathcal{M},g)}}\,\mathrm{d}s (2)
𝖽ℳ(p,q)=inf{L(γ)|γ∈𝒞pw1([0,1],ℳ),γ(0)=p,γ(1)=q}\mathsf{d}_{\mathcal{M}}(p,q)\!=\!\inf\left\{L(\gamma)\middle|\gamma\in\mathcal{C}^{1}_{\piecewise}([0,1],\mathcal{M}),\gamma(0)\!=\!p,\gamma(1)\!=\!q\right\}
⟨u,v⟩p(ℳ,g)=⟨f∗,p​(u),f∗,p​(v)⟩f⁡(p)(𝒩,h)\left<u,v\right>_{p}^{(\mathcal{M},g)}=\left<f_{*,p}(u),f_{*,p}(v)\right>_{f(p)}^{(\mathcal{N},h)}
Original figure
(a) Positive
Original figure
(b) Zero (flat)
Original figure
(c) Negative
Fig. 2: Examples of geodesic triangles TiT_{i} in manifolds ℳi\mathcal{M}_{i} with various sectional curvatures. In positive curvature spaces, the interior angles sum to more than 180°, and the edges bow outwards. The opposite is true in negative curvature spaces.

III-B Convex Analysis on Manifolds

A strongly geodesically convex region contains the unique globally shortest geodesic between any two of its points. This stronger definition ensures that intersections stay geodesically convex and that each segment has an unambiguous shortest realization. Small enough neighborhoods satisfy the condition, with the convexity radius bounding their size.

A geodesically convex function is convex along each geodesic in its domain. The retained inequality is the manifold counterpart of ordinary convexity. These definitions alone do not provide a general-purpose mixed-integer solver on arbitrary curved spaces.

Equations 3–4
Br(p)={expp(q)|q∈𝒯pℳ,||q||<r}B_{r}(p)=\left\{\exp_{p}(q)\;\middle|\;q\in\mathcal{T}_{p}\mathcal{M},\left|\left|q\right|\right|<r\right\} (3)
f⁡(γ⁡(t))≤(1−t)​f​(γ⁡(0))+t​f​(γ⁡(1))f(\gamma(t))\leq(1-t)f(\gamma(0))+tf(\gamma(1)) (4)

IV Problem Statement

The formal problem minimizes Riemannian arc length between two configurations in the closure of a bounded, connected collision-free region. The ambient configuration manifold is assumed connected and complete. The closure permits boundary configurations in the mathematical model; practical collision guarantees depend on the chosen certified regions and collision geometry.

Equation 5
argminL⁡(γ)s.t.γ∈𝒞pw1​([0,1],ℳ¯)γ⁡(0)=pγ⁡(1)=q\begin{array}[]{rl}\argmin&L(\gamma)\\ \textrm{{s.t.}}&\gamma\in\mathcal{C}^{1}_{\piecewise}([0,1],\overline{\mathcal{M}})\\ &\gamma(0)=p\\ &\gamma(1)=q\end{array} (5)

V Graphs of Geodesically-Convex Sets

Each graph vertex carries a geodesically convex region, and each directed edge has a nonnegative geodesically convex cost on the product of its endpoint regions. A discrete route and one continuous choice per visited vertex jointly determine the total cost.

Applying a chart to each region produces a candidate Euclidean GCS problem. That conversion is useful only if the charted regions and transformed costs remain convex. Flat configuration spaces built from one-degree-of-freedom joints and a planar mobile base meet the later assumptions with suitable charts. Universal joints can be modeled as two one-degree-of-freedom joints; the same geometric argument does not extend to ball joints.

Equations 6–7
ℓπ𝒴​(yπ)=∑k=1Kℓ(vk−1,vk)𝒴​(yk−1,yk)\ell^{\mathcal{Y}}_{\pi}(y_{\pi})=\sum_{k=1}^{K}\ell^{\mathcal{Y}}_{(v_{k-1},v_{k})}(y_{k-1},y_{k}) (6)
minπ∈Π​minyπ∈𝒴π​ℓπ𝒴​(yπ)\underset{\pi\in\Pi}{\min}\,\underset{y_{\pi}\in\mathcal{Y}_{\pi}}{\min}\ell_{\pi}^{\mathcal{Y}}(y_{\pi}) (7)
Original figure
Fig. 3: Moving edges and sets from Riemannian manifolds to Euclidean spaces with coordinate charts. In this diagram, 𝒴u\mathcal{Y}_{u} and 𝒴v\mathcal{Y}_{v} are visualized as part of the same Riemannian manifold, although this need not be true in general.
Equation 8
ℓe​(xu,xv)=ℓe𝒴​(ψu−1​(xu),ψv−1​(xv)).\ell_{e}(x_{u},x_{v})=\ell_{e}^{\mathcal{Y}}(\psi_{u}^{-1}(x_{u}),\psi_{v}^{-1}(x_{v})). (8)

VI Motion Planning with GGCS

The paper first states existence of a shortest continuous curve, then assumes a finite chart cover whose region closures are strongly geodesically convex and cover the relevant free-space closure. These are substantive conditions, not automatic properties of arbitrary obstacle spaces. The original theorem and assumption statements are retained below.

Theorem 2 replaces the portion between the first and last visit to each region by its unique minimizing geodesic. Under the stated setup, this yields a piecewise-geodesic optimum that visits each region at most once, matching the graph’s simple-path representation. Equation 9 preserves the replacement construction. The appendix’s general existence argument has a completeness gap discussed in Appendix A.1.

Theorem 1.

(Existence of Optimal Trajectories) For any p,q∈ℳ¯p,q\in\overline{\mathcal{M}}, there exists a continuous curve γ\gamma connecting them such that L⁡(γ)=𝖽⁡(p,q)L(\gamma)=\mathsf{d}(p,q).

Assumption 1.

We are given a finite atlas 𝒜={(𝒴v,ψv)}v∈V\mathcal{A}=\left\{(\mathcal{Y}_{v},\psi_{v})\right\}_{v\in V} of ℳ\mathcal{M}. For each vv, the closure 𝒴¯v\overline{\mathcal{Y}}_{v} is g-convex as a subset of 𝒬\mathcal{Q}. Furthermore, the union of the closures 𝒴¯v\overline{\mathcal{Y}}_{v} covers ℳ¯\overline{\mathcal{M}}.

Theorem 2.

(Piecewise Geodesic Optimal Paths) Let p,q∈ℳ¯p,q\in\overline{\mathcal{M}}, and suppose the sets 𝒴¯v\overline{\mathcal{Y}}_{v} satisfy Assumption 1. Then there is a curve γ∗∈𝒞pw1​([a,b],ℳ¯)\gamma^{*}\in\mathcal{C}^{1}_{\piecewise}([a,b],\overline{\mathcal{M}}) connecting them, such that the following are true:

  • •

    L⁡(γ∗)=𝖽⁡(p,q)L(\gamma^{*})=\mathsf{d}(p,q)

  • •

    γ∗\gamma^{*} is a piecewise geodesic of 𝒬\mathcal{Q}

  • •

    Each geodesic segment is contained in some 𝒴¯v\overline{\mathcal{Y}}_{v}

  • •

    γ∗\gamma^{*} passes through each 𝒴¯v\overline{\mathcal{Y}}_{v} at most once.

Equation 9
γ~​(t)={γi−1​(t)t∉[ai′,bi′]αi​(t)t∈[ai′,bi′]\tilde{\gamma}(t)=\begin{cases}\gamma_{i-1}(t)&t\not\in[a_{i}^{\prime},b_{i}^{\prime}]\\ \alpha_{i}(t)&t\in[a_{i}^{\prime},b_{i}^{\prime}]\end{cases} (9)

VI-A Formulation as a GCS Problem

With a flat ambient manifold and locally isometric charts, charted regions are Euclidean convex sets, within-region distances become Euclidean norms, and chart transitions are affine isometries. For the product coordinates used here, transitions can be aligned as translations.

Each graph vertex represents a segment’s two endpoints. Its cost is the segment length; adjacent endpoints agree after applying the transition map. These are convex constraints and costs, so ordinary GCS machinery applies. Figure 4 shows how chart-space segments lift back to the cylinder.

Theorem 3 compares the two constructions in both directions: every feasible GCS path lifts to a feasible piecewise geodesic of equal length, and an optimal curve with Theorem 2’s structure induces a feasible graph solution. Exact global optimality is conditional on this represented region cover and an exact solve; relaxation and randomized rounding are approximate.

Assumption 2.

𝒬\mathcal{Q} is flat. Also, each ψv\psi_{v} is a local isometry into Euclidean space, viewed as a Riemannian manifold with the canonical Euclidean metric.

Theorem 3.

(Proof of Problem Equivalence) If the path xπx_{\pi} given in Eq. 12 is optimal for the GCS problem defined by Eqs. 10 and 11, then the piecewise geodesic γπ\gamma_{\pi} defined by Eq. 13 is optimal for Problem (5).

Equations 10–13
ℓe​(xu,xv)=𝖽⁡(ψu−1​(xu,0),ψu−1​(xu,1))=||xu,0−xu,1||2\ell_{e}(x_{u},x_{v})=\mathsf{d}(\psi_{u}^{-1}(x_{u,0}),\psi_{u}^{-1}(x_{u,1}))=\left|\left|x_{u,0}-x_{u,1}\right|\right|_{2} (10)
ψu−1(xu,1)=ψv−1(xv,0)⇔τu,v(xu,1)=xv,0\psi_{u}^{-1}(x_{u,1})=\psi_{v}^{-1}(x_{v,0})\quad\Leftrightarrow\quad\tau_{u,v}(x_{u,1})=x_{v,0} (11)
xπ=(x0,0,x0,1,x1,0,x1,1,…,xK,0,xK,1)x_{\pi}=(x_{0,0},x_{0,1},x_{1,0},x_{1,1},\ldots,x_{K,0},x_{K,1}) (12)
yπ\displaystyle y_{\pi} =(y0=p,y1,y2,…,yK,yK+1=q)\displaystyle=(y_{0}=p,y_{1},y_{2},\ldots,y_{K},y_{K+1}=q) (13)
=(ψ0−1​(x0,0),ψ1−1​(x1,0),…,ψK−1​(xK,0),ψK−1​(xK,1))\displaystyle=(\psi_{0}^{-1}(x_{0,0}),\psi_{1}^{-1}(x_{1,0}),\ldots,\psi_{K}^{-1}(x_{K,0}),\psi_{K}^{-1}(x_{K,1}))
Refer to caption
Fig. 4: The process of transforming a GGCS problem into a GCS problem for a simple cylinder manifold. Each of the three charts maps to a Euclidean space, with transition maps encoding the equality constraints across chart domains. The line segments then lift to a piecewise geodesic on the manifold.
Equations
L⁡(γπ)=∑i=0K𝖽⁡(yi,yi+1)=∑i=0K||xi,0−xi,1||2=ℓπ​(xπ)L(\gamma_{\pi})=\sum_{i=0}^{K}\mathsf{d}(y_{i},y_{i+1})=\sum_{i=0}^{K}\left|\left|x_{i,0}-x_{i,1}\right|\right|_{2}=\ell_{\pi}(x_{\pi})
(xi,0,xi,1)=(ψi​(γi​(0)),ψi​(γi​(1)))(x_{i,0},x_{i,1})=\left(\psi_{i}(\gamma_{i}(0)),\psi_{i}(\gamma_{i}(1))\right)

VI-B Construction of the Atlas

IRIS grows collision-free convex regions around chosen seeds in local Euclidean coordinates. The implementation limits each joint’s region extent using its convexity radius, with a small numerical margin, so periodic coordinates do not admit a shorter route around the other side of the manifold. Theorem 4 states the sufficient product-space condition.

The generated regions provide an inner approximation of free space. Configurations outside that cover are effectively treated as obstacles, so an exact solve optimizes within the covered space rather than necessarily finding the shortest path in the robot’s entire physical free space. Seed choice and connectivity therefore matter.

Theorem 4.

Suppose 𝒬=𝒬1×⋯×𝒬m\mathcal{Q}=\mathcal{Q}_{1}\times\cdots\times\mathcal{Q}_{m}, where each 𝒬i\mathcal{Q}_{i} has a convexity radius rir_{i}. Let (𝒴,ψ)(\mathcal{Y},\psi) be a coordinate chart, with ψ\psi a local isometry and ψ⁡(𝒴)\psi(\mathcal{Y}) convex in Euclidean space. If 𝒬\mathcal{Q} is flat and the diameter of proj𝒬i⁡(𝒴)\proj_{\mathcal{Q}_{i}}(\mathcal{Y}) is less than 2​ri2r_{i}, then 𝒴\mathcal{Y} is g-convex.

VI-C More General Motion Planning

Piecewise Bézier curves permit duration and energy costs, velocity bounds, and continuity requirements beyond position. On flat regions, ordinary segment-local expressions carry over. Matching derivatives across charts uses the transition map’s linear pushforward; aligned product charts make that map the identity. Equation 14 is retained in its original notation.

Equation 14
(τi,j)∗,ψi−1​(w)​(νi(k))=νj(k)∀l∈[η](\tau_{i,j})_{*,\psi_{i}^{-1}(w)}\left(\nu_{i}^{(k)}\right)=\nu_{j}^{(k)}\qquad\forall l\in[\eta] (14)

VI-D Positive Curvature Induces Nonconvexity

Positive sectional curvature obstructs this particular convex-distance reduction. Theorem 5 states that distance on the product manifold is nonconvex in every neighborhood of a point with a positive-curvature tangent plane. This explains the limitation for SO(3), ball joints, and unconstrained spatial rigid-body orientation. It does not say that every planning formulation on those spaces is impossible.

Theorem 5.

Let ℳ\mathcal{M} be a Riemannian manifold, let A1∈ℳA_{1}\in\mathcal{M} and u,v∈TA1​ℳu,v\in T_{A_{1}}\mathcal{M} such that 𝒦⁡(u,v)>0\mathcal{K}(u,v)>0. Then for any neighborhood 𝒰\mathcal{U} containing A1A_{1}, 𝖽:ℳ2→ℝ\mathsf{d}:\mathcal{M}^{2}\to\mathbb{R} is nonconvex on 𝒰2\mathcal{U}^{2}.

VII Experiments

All demonstrations are implemented in Drake. They exercise periodic coordinates, region connectivity, and obstacle-rich high-dimensional planning. The original figure and table links retain access to interactive trajectory recordings.

VII-A Point Robot

A point robot moves on a two-dimensional torus represented by a unit square with opposite edges identified. Figure 5 shows obstacles, overlapping regions, and a path that wraps around the boundary. Planning took 0.79 seconds in this example.

Refer to caption
Fig. 5: Results for a point robot in a toroidal world, realized as a unit square with opposite edges identified. Obstacles are shown in red, and each IRIS region is given a distinct pastel color. Note that one of the regions “wraps around” along the horizontal dimension, connecting opposite sides of the world. Grey dashed lines indicate which regions overlap. The optimal path between the start and end points is shown in black.

VII-B Planar Arm

The planar arm has five continuous revolute joints, with self-collision excluded from the model. Figure 6 retains both trajectories and swept collision volumes. They took 5.36 and 4.63 seconds to plan. The second uses more than a full turn of its middle joint, illustrating why a fixed one-turn coordinate interval can exclude a useful solution.

Refer to caption
Refer to caption
Fig. 6: Two plans produced by GGCS-Planner for a planar arm around task-space obstacles (shown in red). We display both the swept collision volume and individual poses in the trajectory (colored by time, as indicated by the colorbar).

VII-C Modified KUKA iiwa Arm

In simulation, the first joint of a seven-joint KUKA iiwa is made continuous. A specified sequence of fourteen motions sorts mugs across shelves; the planner solves each geometric motion rather than choosing the task sequence. Eighteen seed-based regions are refined as mug locations change, including collision checks for a held mug.

Figures 7 and 8 show seed configurations and initial/final states. Mean planning time per motion is 25.75 seconds, ranging from 4.63 to 50.30 seconds. The continuous base joint avoids unnecessary unwinding. Appendix B gives the trajectory and region-generation details.

Refer to caption
Fig. 7: Key configurations (overlaid) used for a mug reorganization demo.
Refer to caption
Refer to caption
Fig. 8: Initial (left) and final (right) states for the mug reorganization demo.

VII-D PR2 Bimanual Mobile Manipulator

The PR2 experiment plans in a fifteen-dimensional configuration space after fixing wrist/gripper joints. Shelf-reaching and crossed-arm poses seed the regions; additional intermediate seeds are chosen manually to maintain connectivity. Figure 9 shows a resulting trajectory.

Tables I and II preserve every path length, online planning time, collision timestamp, and recording link. GGCS takes about 25.5–66.2 seconds per query, while the prebuilt roadmap answers in roughly half a second but produces much longer paths. Roadmap construction takes 124.39 seconds; parallel generation of the IRIS regions takes 156.63 seconds. These preparation costs are separate from the online timings.

The PRM comparison is initialized from the overlap skeleton of the GGCS regions before adding 100,000 random samples. Nonlinear trajectory optimization sometimes requires extra waypoints, and several apparently shorter outputs collide with the environment, as marked in Table I and Figure 10. Postprocessing a roadmap path does not always remove this issue.

Two Euclidean workarounds either impose artificial angular limits or treat angles as unbounded real coordinates. Their best choices require prior knowledge of the desired winding; worst choices can produce much longer paths. Some best-case results are slightly shorter because their regions are allowed to grow larger. With the same regions and favorable angle choices, their results match GGCS.

Experiment GGCS-Planner Drake-Trajopt PRM PRM + Drake-Trajopt GCS-Planner (Joint Limits) GCS-Planner (No Joint Limits)
1T to 1B  1.829\,1.829\;  1.803\,1.803\;  4.359\,4.359\;  1.808\,1.808\;  1.826\,1.826\;  1.839\,1.839\;
1CL to 1CR  2.255\,2.255\;  2.204\,2.204\;  9.219\,9.219\;  2.182\,2.182\;  2.239\,2.239\;  2.247\,2.247\;
1M to 4M  3.875\,3.875\; 6.938 t=0.275,5.272t=0.275,5.272  14.554\,14.554\; 5.874 t=0.714,4.381t=0.714,4.381  6.482\,6.482\; /  10.478\,10.478\;  3.990\,3.990\; /  12.589\,12.589\;
1CL to 2CR  4.473\,4.473\; 5.409 t=2.155t=2.155  12.110\,12.110\; 4.108 t=0.49t=0.49  4.441\,4.441\; /  13.815\,13.815\;  4.640\,4.640\; /  13.233\,13.233\;
1CL to 3CR  8.182\,8.182\;  10.263\,10.263\;  15.250\,15.250\; 7.166 t=0.7,1.87,2.02,2.77t=0.7,1.87,2.02,2.77  7.820\,7.820\; /  12.125\,12.125\;  8.501\,8.501\; /  12.125\,12.125\;
1CL to 4CR  4.382\,4.382\;  7.583\,7.583\;  17.459\,17.459\; 6.088 t=0.27,0.555,4.39t=0.27,0.555,4.39  4.728\,4.728\; /  9.961\,9.961\;  4.559\,4.559\; /  12.418\,12.418\;
1T to 4B  4.538\,4.538\;  8.781\,8.781\;  12.351\,12.351\; 5.949 t=0.34,0.68t=0.34,0.68  5.320\,5.320\; /  14.928\,14.928\;  5.473\,5.473\; /  14.160\,14.160\;
TABLE I: A comparison of trajectory lengths (in configuration space) for each PR2 experiment across different methods. Experiments are titled by the start and goal configurations. The configuration names indicate the shelf positions on the table (1 through 4), followed by the position of the grippers. T, M, B, CL, and CR stand for top, middle, bottom, cross left over right, and cross right over left (respectively). Table cells that are struck through indicate that the trajectory is not collision-free, and the time stamps below the trajectory length indicate when the collisions occurred. For both GCS-Planner workarounds, we include the best- and worst-case results (in general, achieving the best-case results requires a priori knowledge of the optimal plan). Interactive recordings of each trajectory are available online at our results website. Each cell is linked to its corresponding recording.
Experiment GGCS-Planner Drake-Trajopt PRM PRM + Drake-Trajopt
1T to 1B 25.51 12.69 0.49 11.61
1CL to 1CR 39.42 15.23 0.49 16.11
1M to 4M 46.61 2.26 0.53 25.51
1CL to 2CR 62.87 9.74 0.54 21.48
1CL to 3CR 58.60 7.82 0.52 27.30
1CL to 4CR 66.15 4.32 0.54 40.10
1T to 4B 29.89 10.92 0.54 15.36
TABLE II: A comparison of online planning times (in seconds) for each PR2 experiment across different methods. (We omit the GCS workaround comparisons, as they are indistinguishable from the corresponding GGCS-Planner runtimes.) Experiment names match Table I.
Refer to caption(1)(2)(3)(4)(5)
Fig. 9: Individual poses along a trajectory produced by GGCS-Planner for the PR2 robot, labeled with their order in the plan.
Refer to caption
Fig. 10: An example of the slight collisions typical of the trajectories produced by Drake-Trajopt. (The blue circle highlights the point of collision.)

VIII Discussion

The main contribution is a topology-aware reduction that preserves convex optimization structure on flat configuration manifolds. It combines region selection with continuous trajectory optimization and handles wraparound directly.

The practical tradeoffs are region-construction effort, incomplete free-space coverage, solve time, and the gap between exact mixed-integer solutions and rounded approximations. Demonstrated collision guarantees rely on certified regions and the modeled geometry. Closed-chain constraints, contact, nonlinear dynamics, and positive-curvature orientation spaces remain outside the demonstrated method.

Acknowledgments

The authors thank Tobia Marcucci, Seiji Shaw, and David von Wrangel. Support came from Amazon and the Frederick and Barbara Cronin Fellowship.

Appendix A Proofs

The appendix supplies the existence, chart-convexity, and positive-curvature arguments. Original lemma statements are retained, with the proof steps condensed and the published limitations of the argument identified.

A.1 Proof of Theorem 1

Lemma 1 seeks to construct a piecewise-smooth connecting path by covering a continuous path with finitely many charts, building local smooth pieces, lifting them, and concatenating them. Theorem 1 then invokes a shortest-path existence result for complete, locally compact length spaces.

There is a gap in the argument as written: the appendix identifies completeness with finiteness of pairwise distances, whereas metric completeness requires convergence of every Cauchy sequence. Establishing a finite path length alone does not verify that hypothesis. The original theorem and lemma are preserved as published claims; this condensed edition does not supply a repaired general existence proof.

Lemma 1.

For any p,q∈ℳ¯p,q\in\overline{\mathcal{M}}, there is a piecewise-smooth path connecting pp and qq.

A.2 Proof of Theorem 4

Lemma 2 uses connected chart overlap and local isometry to identify the transition with a Euclidean isometry. Lemma 3 observes that a Euclidean convex subset within an already geodesically convex isometric chart lifts to a geodesically convex subset.

Theorem 4’s proof encloses each component projection in a ball below its convexity radius, takes their product, and uses flat normal coordinates. The chart transition preserves Euclidean convexity, allowing Lemma 3 to conclude geodesic convexity. The implementation’s explicit per-joint ball bounds provide the concrete construction used for the robot examples. Figure 11 is retained with the original caption.

Lemma 2.

Let (𝒴1,ψ1)(\mathcal{Y}_{1},\psi_{1}) and (𝒴2,ψ2)(\mathcal{Y}_{2},\psi_{2}) be coordinate charts of ℳ\mathcal{M}, with ψ1\psi_{1} and ψ2\psi_{2} local isometries, and 𝒴1\mathcal{Y}_{1} and 𝒴2\mathcal{Y}_{2} g-convex. Then there is a Euclidean isometry ξ\xi such that ∀p∈𝒴1∩𝒴2\forall p\in\mathcal{Y}_{1}\cap\mathcal{Y}_{2}, ψ1​(p)=(ξ∘ψ2)​(p)\psi_{1}(p)=(\xi\circ\psi_{2})(p).

Lemma 3.

Consider 𝒴⊆𝒵⊆ℳ\mathcal{Y}\subseteq\mathcal{Z}\subseteq\mathcal{M}, where 𝒵\mathcal{Z} is g-convex, and we have a coordinate chart (𝒵,ψ)(\mathcal{Z},\psi) such that ψ\psi is a local isometry. If ψ⁡(𝒴)\psi(\mathcal{Y}) is convex, then 𝒴\mathcal{Y} is g-convex.

Original figure
Fig. 11: The construction of two Levi-Civita parallelogramoids used in the proof of Theorem 5. 𝖽⁡(A0,B0)<𝖽⁡(A1,B1)\mathsf{d}(A_{0},B_{0})<\mathsf{d}(A_{1},B_{1}) and 𝖽⁡(A2,B2)<𝖽⁡(A1,B1)\mathsf{d}(A_{2},B_{2})<\mathsf{d}(A_{1},B_{1}), which demonstrates the nonconvexity of 𝖽\mathsf{d} around A1A_{1}. ∠​A0​B0​B1\angle A_{0}B_{0}B_{1} and ∠​A2​B2​B1\angle A_{2}B_{2}B_{1} are obtuse.

A.3 Proof of Theorem 5

The positive-curvature proof constructs paired short geodesics using parallel transport, forming two Levi-Civita parallelogramoids. The retained curvature expansion makes each endpoint pair closer than the central pair for sufficiently small displacement, violating convexity of distance along the product geodesic.

The source’s final sentence calls this a “local minimum,” but the inequalities establish a larger midpoint than both endpoints. The summary follows those inequalities; the published equations and figure remain unchanged.

Equations
𝖽​(A2,B2)2=𝖽​(A1,B1)2+83​⟨ℛ⁡(ϵ​u,ϵ​v)​ϵ​u,ϵ​v⟩+O⁡(ϵ5)\mathsf{d}(A_{2},B_{2})^{2}=\mathsf{d}(A_{1},B_{1})^{2}+\tfrac{8}{3}\left<\mathcal{R}(\epsilon u,\epsilon v)\epsilon u,\epsilon v\right>+O(\epsilon^{5})
⟨ℛ⁡(ϵ​u,ϵ​v)​ϵ​u,ϵ​v⟩=−ϵ4​⟨ℛ⁡(u,v)​v,u⟩=−ϵ4​K​(u,v)<0\left<\mathcal{R}(\epsilon u,\epsilon v)\epsilon u,\epsilon v\right>=-\epsilon^{4}\left<\mathcal{R}(u,v)v,u\right>=-\epsilon^{4}K(u,v)<0

Appendix B Experiment Implementation Details

The implementation details clarify how manually selected regions, approximate optimization, and collision-checking choices affect the reported comparisons.

B.1 Planar Arm

The planar-arm graph contains nineteen regions. Start/goal and manually chosen narrow-passage seeds are supplemented with random collision-free seeds. The objective combines path length and duration.

The shown relaxed-and-rounded trajectories have lengths 7.749 and 8.448; branch-and-bound solutions have lengths 7.274 and 8.008. Thus the displayed plans are not the exact shortest results, although the exact second plan still needs more than a full turn of the middle joint.

B.2 KUKA iiwa Arm

The iiwa seeds cover reachable middle/top shelves and intermediate positions; bottom shelves are unreachable. Regions are prepared for empty and mug-holding hands and simplified by removing redundant inequalities. The objective combines time and path length, with continuous position, velocity, and acceleration plus hardware velocity limits. All shown trajectories use relaxation and rounding.

B.3 PR2 Bimanual Mobile Manipulator

The PR2 uses Drake’s robot description with convex hulls of link meshes for planning; reported collision annotations use the true mesh geometry. Shelf-target regions and manually selected intermediate regions are simplified, and irrelevant shelf-reaching regions are omitted for each query. GGCS again minimizes length plus duration using relaxation and rounding.

Drake-Trajopt uses B-splines and SNOPT. It first solves without collision constraints, then adds pointwise minimum-distance constraints at fifty trajectory samples, with a 1 mm target clearance and penalties beginning at 1 cm. Additional base-retreat waypoints are sometimes needed.

When initialized from a PRM trajectory, the optimizer uses one hundred collision samples and begins penalizing proximity at 1 m. These finite sampling choices explain why continuous-time collision avoidance differs from the region-based method even when an optimized curve looks shorter.

Concept demonstration

Open image ↗