Approximating Configuration Space with Few Convex Sets

Tobia Marcucci

Russ Tedrake

Peter Werner

Daniela Rus

Alexandre Amice

Motion Planning IS Hard Because Configuration Space is Complicated

Describing Configuration Space As a Collection of Simple Sets makes motion planning tractable

Globally optimal motion plans using only convex optimization

Locally Optimal Motion Plans in Real Time

Simpler Sampling Based Motion Planning

Easier to Solve Inverse Kinematics

Automatically Decomposing Configuration Space

How do I build a single, collision-free set?

How do I arrange the sets to cover as much space as possible?

How Do I Build a Single, Collision Free Set?

Computing Large Convex Regions of Obstacle-Free Space Through Semidefinite Programming (Deits et al. 2015)
Certified Polyhedral Decompositions of Collision-Free Configuration Space (Dai, Amice, et. al 2023)
Growing Convex Collision-Free Regions in Configuration Space using Nonlinear Programming (Petersen, et. al. 2023)

Approximating Robot Configuration Spaces with few Convex Sets using Clique Covers of Visibility Graphs (Werner, et. al. 2023)

How do I arrange the sets to cover as much space as possible?

Iterative Regional Inflation by Semidefinite programming (IRIS)

Credits: Robin Deits

Iterative Regional Inflation by Semidefinite programming (IRIS)

Credits: Robin Deits

Iterative Regional Inflation by Semidefinite programming (IRIS)

Credits: Robin Deits

Iterative Regional Inflation by Semidefinite programming (IRIS)

Credits: Robin Deits

Iterative Regional Inflation by Semidefinite programming (IRIS)

Credits: Robin Deits

Convex In Task Space Is Not Convex in Configuration Space

IRIS-NP

Direct Generalization Of IRIS to Based On Non-Convex Optimization

Generates Regions Very Quickly in Arbitrary Configuration Spaces

Not Guaranteed To Generate Fully Collision Free Regions, But Does so Frequently

Configuration Space Generalizations

C-IRIS

Conceptually Different Method Based on Sums-Of-Squares

Generates Regions in a Reparametrization of Configuration Space

Guaranteed Collision-Free Regions, But Much Slower

IRIS Steps

Choose a Seed Point

Choose an Initial Ellipse

Expand the Polytope

Iterate Steps 2-3

IRIS Steps

Choose a Seed Point

Choose an Initial Ellipse

Expand the Polytope

Iterate Steps 2-3

IRIS Steps

Choose a Seed Point

Choose an Initial Ellipse

Expand the Polytope

Iterate Steps 2-3

IRIS Features

Converged Region is Sensitive to Initial Ellipse

IRIS Features

Biased Towards Open Space

Credit: Tommy Cohn

From a Single Region To A Cover

Problem Statement

How Hard Is This?

Exact Cover of 2D polygon:

\exists \mathbb{R}-\text{complete}

Approximate Cover:

As hard as checking if a system of polynomial equations has a root.

\text{APX}-\text{Hard}

No polynomial time algorithm can achieve an approximation ratio bounded by a constant factor.

There is an algorithm which achieves a logarithmic performance in the number of vertices with runtime

\mathcal{O}(n^{29} \log(n))

Naive Strategy

Sample Randomly

Grow An Iris Region

Iterate, Treating Previous Regions as Obstacles

DownSides

Inherently Sequential

Each iteration takes longer than the last

Fragments the space

Visibility Clique Cover Algorithm

Visibility Clique Cover

Recall:

Visibility Clique Cover

Intuition

Convex Set	Clique	Clique on V-Graph

What is a ...

Intuition

Convex Set	Clique	Clique on V-Graph

What is a ...

\scriptsize{\mathcal{S} \text{ is convex iff } \forall a,b \in \mathcal{S} \Rightarrow \overline{ab} \subseteq \mathcal{S}}

Intuition

Convex Set	Clique	Clique on V-Graph

What is a ...

\forall a,b \in \mathcal{S} \Rightarrow \overline{ab} \subseteq \mathcal{S}

\overline{ab}

Intuition

Convex Set	Clique	Clique on V-Graph

What is a ...

\forall i,j \in \mathcal{K} \Rightarrow \{i,j\} \in \mathcal{E}

\mathcal{G} = (\mathcal{V}, \mathcal{E}),

Given a graph

\mathcal{G}

Intuition

Convex Set	Clique	Clique on V-Graph

What is a ...

\forall i,j \in \mathcal{K} \Rightarrow \{i,j\} \in \mathcal{E}

\scriptsize{\mathcal{K_1} = \{2,3,4\}}

\scriptsize{\rightarrow \{1,3\} \notin \mathcal{E}}

Intuition

Convex Set	Clique	Clique on V-Graph

What is a ...

Visibility Graph

Add edge if two points have line of sight

Intuition

Convex Set	Clique	Clique on V-Graph

What is a ...

Visibility Graph

Add edge if two points have line of sight
Edges are collision-free line segments

Intuition

Convex Set	Clique	Clique on V-Graph

What is a ...

Collision-free convex sets

\scriptsize{a,b \in \mathcal{S} \Rightarrow \overline{ab} \subseteq \mathcal{S}\subseteq\mathcal{C}^\text{free}}

Intuition

Convex Set	Clique	Clique on V-Graph

What is a ...

Points inside a convex set form a clique on the visibility graph

Intuition

Convex Set	Clique	Clique on V-Graph

What is a ...

Points inside a convex set form a clique on the visibility graph

Cliques approximate convex sets

Intuition

Convex Set	Clique	Clique on V-Graph

What is a ...

Converse not true in general

\scriptsize{\mathcal{K} = \{1, 2, 3\} \text{ is a clique} }\\ \scriptsize{ \text{but }\textbf{conv}\{a_1, a_2, a_3\} \not \subseteq\mathcal{C}^\text{free} }

Intuition

Convex Set	Clique	Clique on V-Graph

What is a ...

\scriptsize{\text{If } a_1,\dots, a_N \in \mathcal{S} \text{ then }}\\ \scriptsize{\mathcal{K} = \{1, \dots, N\} \text{ is a clique}}

Intuition

Convex Set	Clique	Clique on V-Graph

What is a ...

Proves to be a powerful heuristic!

Cover continuous space in small number of continuous regions

Cover discrete graph in small number of cliques

Cover graph in small number of cliques

use approximations to minimum clique cover

use repeated Max Clique solves

Advantages

Clique Cover Gives Global Information

Each Clique Gives Local Information

Can parallelize IRIS region growth

Village	# of Regions	Time To Construct
Ours	94	28 min
Naive	198	45 min

https://wernerpe.github.io/files/Village.html

IIWA	# of Regions	Time To Construct
Ours	46	95 min
Naive	483	1483 min

https://wernerpe.github.io/files/7DOF_IIWA_arxiv.html

Extension

The rest of this talk is work in progress

Are ClIques Convex Sets?

Not Even With Infinite Samples

Greedy Clique Covering:

compute largest clique
remove it
repeat

The minimum Clique Cover isn't The Minimum ConveX Cover

Obviously a problem

Solution

Ask for a clique cover that is more like a convex set

\max \sum_{i} x_{i} \textbf{ subject to } \\ x_{i} + x_{j} \leq 1 \text{ if } (i,j) \notin \mathcal{E} \\ x_{i} \in \{0,1\}

x_{i} \in \textbf{conv}\left(\mathcal{K}\right) \implies x_{i} \in \mathcal{K}

If the convex hull condition is violated, then your clique must contain a collision

Clique Cover MIP

\min \sum_{1 \leq i \leq M} c_{i} \textbf{ subject to } \\ \sum_{i = 1}^{M} x_{v i} = 1 ~ \forall v \in \mathcal{V} \\ x_{ui} + x_{vi} \leq c_{i} ~ \forall (u,v) \notin \mathcal{E} \\ x_{vi}, c_{i} \in \{0,1\}

Max Clique Mip

\max \sum_{i} x_{i} \textbf{ subject to } \\ x_{i} + x_{j} \leq 1 \text{ if } (i,j) \notin \mathcal{E} \\ x_{i} \in \{0,1\}

Solution 1

\max_{x, c} \sum_{i} x_{i} \textbf{ subject to } \\ x_{i} + x_{j} \leq 1 \text{ if } (i,j) \notin \mathcal{E} \\ x_{i} \in \{0,1\}

c_{i}^{T}(q_{i} - q_{j}) \geq (1- x_{i}) - M_{ij}(1-x_{j})

If the th point is in the clique, but the th point is not, then find a separating plane between them.

Solution 2

\max_{x, E} \sum_{i} x_{i} \textbf{ subject to } \\ x_{i} + x_{j} \leq 1 \text{ if } (i,j) \notin \mathcal{E} \\ x_{i} \in \{0,1\}

\begin{bmatrix} q_{i} \\ 1 \end{bmatrix}^{T} E \begin{bmatrix} q_{i} \\ 1 \end{bmatrix} \leq 1 + M_{i}(1-x_{i})

\begin{bmatrix} q_{i} \\ 1 \end{bmatrix}^{T} E \begin{bmatrix} q_{i} \\ 1 \end{bmatrix} \geq (1-x_{i})

Find a classifier for the clique whose decision boundary is a convex set

Solution 2

Find a classifier for the clique whose decision boundary is a convex set

Addresses the Problem

At the cost that Clique Cover now takes 20 times longer

Are All PartS of the Free Configuration Space Equal?

Big regions are not always useful

We Might only care about a Small portion of CSPACE

Reaching Into Shelves

T-SNE + Visibility

Reaching Into Shelves

T-SNE + Visibility

IK solutions form clusters
Want coverage of very particular regions
Dense sampling challenging for 7dof

Our Target C-Space is 14+ Dimensional

No hope of densely sampling

More samples means harder clique cover

Uniform sampling biased away from collisions

Proposal: Sample in Task Space

Task Space is Always 3D

Intuitive to Weight "Important" Areas

Task Space Coverage is the Real Goal

The Mode problem

unsupervised learning to clip long edges

results in better control for region growth

Task-Space Rapidly Exploring Random Tree

Voronoi Bias In Task Space

Promising

Recap

Want To Cover Configuration Space with Convex Regions

Have an Region Inflation Algorithm

Cliques in the Visibility Graph Can Give Local Information About What Convex Subset To Grow

Clique Covers Can Give Global Information about Where To Grow Convex Cover

Extension Recap

Question 1: How can we put more geometric information in the clique cover to better approximate convex cover?

Question 2: How can we leverage task space to generate a better sampling distribution?