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.

Planning through Convex Sets

Obstacle avoidance: nonconvex

Safe-set containment: convex

A Graph of Convex Sets

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$

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$