Graphs of Geodesically-Convex Sets

Loose Ends, and SO(3) without SDPs

Robot Configuration Spaces as Smooth Manifolds

Robust Combined Adaptive and Variable Structure Adaptive Control of Robot Manipulators, Yu

\mathcal{Q}=\{(q_1,q_2)\,|\,0\le q_1\le 2\pi, 0\le q_2\le 2\pi\}

(No joint limits)

Survey of Graph Embeddings into Compact Surfaces, Potoczak

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

Geodesics: Shortest Paths

For a curve \(\gamma\) connecting two points \(p=\gamma(0),q=\gamma(1)\) on a manifold, its arc length is

\[\int_0^1||\dot\gamma(t)||\,\mathrm{d}t\]

Geodesics on Surfaces by Variational Calculus, Villanueva

The Curvature and Geodesics of the Torus, Irons

This yields 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\}\]

A geodesic is a (locally) length-minimizing curve.

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 strongly geodesically 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

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

Convex Functions

Geodesically Convex Functions

A Graph of Geodesically Convex Sets (GGCS)

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

  • For each vertex \(v\in V\), there is a strongly geodesically convex manifold \(\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 geodesically convex on \(\mathcal{Y}_u\times\mathcal{Y}_v\)

Transforming Back to a GCS

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\)

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\)

Why is Positive Curvature Bad?

Planning on PL Approximations

Path Planning on Manifolds using Randomized Higher-Dimensional Continuation, Porta and Jaillet

Planning on PL Approximations

Black: Shortest path on PL approximation

Red: Shortest path on sphere

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.

Focusing on SO(3)/SE(3)

Recall that \(\operatorname{SE}(3)\) is just \(\operatorname{SO}(3)\times\mathbb R^3\) 

Three representations for \(\operatorname{SO}(3)\):

  • Euler angles (\(S^1\times S^1\times S^1\))
    • 27 sets, 702 edges (fully connected)
  • Axis-angle (\(S^2\times S^1\))
    • 60 sets, 660 edges
  • Quaternions (\(S^3\))
    • 27 sets and 390 edges or 64 sets and 2240 edges

\(S^1\) is flat (no approximation needed)

Numerical Comparison

Randomly sample \(n\) start and goal pairs, plan between them with each approximation.

 

Compute the true distance, and explore the error distribution.

Note: two possible representations in axis-angle and quaternion, so we plan twice. Euler angles are stranger, so we ignore.

SE(3) Maze

Consider a block moving freely in 3D through a 2D maze.

 

Generate collision free regions with IRIS-NP. (Have to use Euler angles.)

More Setup Details

Making the graph:

  1. Create a grid-like roadmap
  2. Cover subset of edges with IRIS regions to connect key poses

200 sets, 4744 edges

  • Good engineering
  • Loose tolerance for relaxation (\(\sim 10^3\))
  • Use L1-norm for convex relaxation (still use L2-norm for the convex restriction)

Tricks to deal with the bigger graph:

Shortest Paths in the Maze

Sample path between a start/goal pair.

 

The large gap in the center gives room for the block to reorient itself.

Statistics across All Pairs

GCS average runtime: 354.98 seconds

GCS* average runtime: 46.71 seconds

GCS* is ~7 times faster, and produces paths that are only 4.4% longer than GCS on average.

Takeaway: For large collision-free kinematic motion planning problems, use GCS*

What's Next?

  • Shruti's talk
  • SO(3) representations are non-unique, but maybe we can get around that with auxiliary vertices and edges.
    • Can interpret this as a configuration-space symmetry
    • I'll be talking more about this next week