Motion Planning

Configuration-Space Planning

Task Space

Configuration Space

Planning with Roadmaps

Planning with Roadmaps

Trajectory Optimization

Control points of a trajectory are decision variables.

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

Nonconvexity in TrajOpt

Planning through Convex Sets

Obstacle avoidance: nonconvex

Safe-set containment: convex

A Graph of Convex Sets

Configuration Space as a Manifold

Geodesics: Shortest Paths

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

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

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

Some Sets Look Convex

(But Aren't)

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

Geodesics can go "the wrong way around"

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

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

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

GCS

GGCS

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

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
   
• Transition maps are affine functions (in particular, they are an orthogonal transformation plus a translation)
   
• Formal equivalence of GGCS and transformed GCS problem

Why is Positive Curvature Bad?

Planning on PL Approximations

Black: Shortest path on PL approximation

Red: Shortest path on sphere

Rough Bounds on Approximation Error

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

Numerical Comparison

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

Compute the true distance, and explore the error distribution.

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.