Approximating Configuration Space with Few Convex Sets Using Clique Cover

With Peter Werner, Tobia Marcucci, and Russ Tedrake

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

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

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

Naive Strategy

Sample Randomly

Grow An Iris Region

Iterate, Treating Previous Regions as Obstacles

DownSides

Highly biased toward "open" space

Inherently Sequential

Each iteration takes longer than the last

Fragments the space

Visibility Clique Cover

Intuition

Visibility Clique Definition:

\text{If } q_{1}, q_{2} \in \mathcal{K}, t \in [0,1], \\ \text{then } tq_{1} + (1-t)q_{2} \subseteq \mathcal{C}^{free}

Convex Set Definition:

\text{If } q_{1},~q_{2} \in \mathcal{R},~t \in [0,1], \\ \text{ then } tq_{1} + (1-t)q_{2} \in \mathcal{R}

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

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

Limitations

The rest of this talk is work in progress

And (Maybe) How to Fix them

Are ClIques Convex Sets?

Not Even With Infinite Samples

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

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

Addresses the Problem

At the cost that Clique Cover now takes 20 times longer

Are All PartS of the Free Configuration Space Equal?

Big C-Space Regions Aren't Always Useful

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

Task-Space Rapidly Exploring Random Tree

Voronoi Bias In Task Space

Recap

Want To Cover Configuration Space with Convex Regions

Have an (expensive) Region Inflation Algorithm

Cliques in the Visibility Graph Can Give Local Information About Convex Subsets

Clique Covers Can Give Global Information about Convex Cover

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?