Certified Polyhedral Decompositions of Collision-Free Configuration Space

TL;DR

This paper introduces C-IRIS, a novel method that rigorously decomposes the collision-free configuration space into certified convex polyhedral regions using convex optimization, enabling scalable motion planning in complex robotic systems.

Contribution

It presents the first practical, certified polyhedral decomposition method for C-free spaces using rational parametrization and semidefinite programming, applicable in arbitrary dimensions.

Findings

Successfully decomposes C-free space in 2-DOF examples

Scales to high-dimensional manipulators like 7-DOF KUKA iiwa

Provides open-source implementation and interactive examples

Abstract

Understanding the geometry of collision-free configuration space (C-free) in the presence of task-space obstacles is an essential ingredient for collision-free motion planning. While it is possible to check for collisions at a point using standard algorithms, to date no practical method exists for computing C-free regions with rigorous certificates due to the complexity of mapping task-space obstacles through the kinematics. In this work, we present the first to our knowledge rigorous method for approximately decomposing a rational parametrization of C-free into certified polyhedral regions. Our method, called C-IRIS (C-space Iterative Regional Inflation by Semidefinite programming), generates large, convex polytopes in a rational parameterization of the configuration space which are rigorously certified to be collision-free. Such regions have been shown to be useful for both optimization-based and randomized motion planning. Based on convex optimization, our method works in arbitrary dimensions, only makes assumptions about the convexity of the obstacles in the task space, and is fast enough to scale to realistic problems in manipulation. We demonstrate our algorithm's ability to fill a non-trivial amount of collision-free C-space in several 2-DOF examples where the C-space can be visualized, as well as the scalability of our algorithm on a 7-DOF KUKA iiwa, a 6-DOF UR3e and 12-DOF bimanual manipulators. An implementation of our algorithm is open-sourced in Drake. We furthermore provide examples of our algorithm in interactive Python notebooks.

Tables

Table 1. Table 1 : Parameterizations of conditions ( 3a ) and ( 4a ) respectively for particular convex bodies. 5 5 5 Strictly speaking, the formulation ( 3a ) given for the sphere, capsule, and cylinder only enforce non-strict separation i.e. a T ​ x + b ≥ 0 superscript 𝑎 𝑇 𝑥 𝑏 0 a^{T}x+b\geq 0 . This can be remedied by replacing r 𝑟 r with r + ε 𝑟 𝜀 r+\varepsilon for any ε > 0 𝜀 0 \varepsilon>0 .

Body	$a^{T}x+b>0$, $\forall x\in 𝒜$	$x\in 𝒜$
V-rep Polytope with vertices {v_1, …, v_m}.	$a^T{v}_i+b\ge 1,\forall i\in \{1,\dots ,m\}$	$x=\sum _{i=1}^m{\mu }_i{v}_i,\sum _{i=1}{\mu }_i=1,$ ${\mu }_i\ge 0$
Sphere with center $o$ and radius $r$.	$a^{T}o+b\ge r\Vert a\Vert$ $a^{T}o+b\ge 1$	$\Vert x-o\Vert$^2 ≤r^2
Capsule, the convex hull of two spheres with centers $o_1$ and $o_2$ and radii $r_1$ and $r_2$.	$a^T{o}_1+b\ge r_1\Vert a\Vert$ $a^T{o}_2+b\ge r_2\Vert a\Vert$ $a^T{o}_1+b\ge 1$	$o_{\mu }=\mu o_1+\left(1-\mu \right)o_2$ $\Vert x-o_{\mu }\Vert \le \mu r_1+\left(1-\mu \right)r_2$ $0\le \mu \le 1$
Cylinder, the convex hull of two circles with centers $o_1$ and $o_2$, and with radii $r_1$ and $r_2$.	$a^T\left(\frac{o_1+o_2}{2}\right)+b\ge 1$	$o_{\mu }=\mu o_1+\left(1-\mu \right)o_2$ $v^T\left(o_1-o_2\right)=0$ $x=o_{\mu }+v$ $\Vert v\Vert \le \mu r_1+\left(1-\mu \right)r_2$ $0\le \mu \le 1$

Table 2. Table 2 : SOS conditions for the constraint ( 23b ) and ( 23c ) depending on the geometry of bodies 𝒜 𝒜 \mathcal{A} and ℬ ℬ \mathcal{B} .

Body	Psatz Condition for (23b)
V-rep Polytope with $m$ vertices $v_i$ at position ${\text{leftidx}}^F{p}^{v_i}\left(s\right)=\frac{{\text{leftidx}}^F{f}^{v_i}\left(s\right)}{{\text{leftidx}}^F{g}^{v_i}\left(s\right)}$	Enforce (19) for each vertex $v_i$.
Sphere with center $o$ at position ${\text{leftidx}}^F{p}^o\left(s\right)=\frac{{\text{leftidx}}^F{f}^o\left(s\right)}{{\text{leftidx}}^F{g}^o\left(s\right)}$ and radius $r$	Enforce (22) for the center $o$ with radius $r$. Also enforce (19) for the center $o$.
Capsule, the convex hull of two spheres with centers $o_1$ and $o_2$ at positions ${\text{leftidx}}^F{p}^{o_i}\left(s\right)=\frac{{\text{leftidx}}^F{f}^{o_i}\left(s\right)}{{\text{leftidx}}^F{g}^{o_i}\left(s\right)}$ and radii $r_1$, $r_2$	For $i\in \{1,2\}$ enforce (22) for center $o_i$ with radius $r_i$. Also enforce (19) for $o_i$.
Cylinder, the convex hull of two circles with centers $o_1$ and $o_2$, at position ${\text{leftidx}}^F{p}^{o_i}\left(s\right)=\frac{{\text{leftidx}}^F{f}^{o_i}\left(s\right)}{{\text{leftidx}}^F{g}^{o_i}\left(s\right)}$, lying in the plane normal to ${\text{leftidx}}^F{p}^{o_1}\left(s\right)-{\text{leftidx}}^F{p}^{o_2}\left(s\right)$, and with radii $r_1$ and $r_2$.	See Appendix D.

Table 3. Table 3 : parameterization of algebraic joints in terms of the matrix given in ( 38 ).

Joint	Restriction	Definition of $q_i$
R	$x_i=y_i=z_i=0$	$q_i=\left\{{\theta }_i\right\}$
P	${\theta }_i=x_i=y_i=0$	$q_i=\left\{z_i\right\}$
C	$x_i=y_i=0$	$q_i=\{{\theta }_i,z_i\}$
E	$z_i=0$	$q_i=\{{\theta }_i,x_i,y_i\}$
S	see equation (41)	$q_i=\{{\varphi }_{i,x},{\varphi }_{i,y},{\varphi }_{i,z}\}$

Table 4. Table 4: Parameterizations of the condition that x 𝑥 x lies in a convex body that moves rigidly as a function of s 𝑠 s .

Body	Variables	Description of $𝒜\left(s\right)$ as a semi-algebraic set
V-rep Polytope with $m$ vertices $v_i$ at position ${\text{leftidx}}^F{p}^{v_i}\left(s\right)=\frac{{\text{leftidx}}^F{f}^{v_i}\left(s\right)}{{\text{leftidx}}^F{g}^{v_i}\left(s\right)}$	$\{s,x,\mu \}$	$h_1(s,x,\mu )=\left(\prod _i\text{leftidx}{}_{}{}^{F}g_{}^{v_i}\right)\left(x-\sum _{i=1}^m{\mu }_i\left(\frac{{\text{leftidx}}^F{f}^{v_i}\left(s\right)}{{\text{leftidx}}^F{g}^{v_i}\left(s\right)}\right)\right)$ $h_2\left(\mu \right)=1-\sum _{i=1}{\mu }_i$ ${\gamma }_i\left({\mu }_i\right)={\mu }_i,i\in \left[m\right]$
Sphere with center $o$ at position ${\text{leftidx}}^F{p}^o\left(s\right)=\frac{{\text{leftidx}}^F{f}^o\left(s\right)}{{\text{leftidx}}^F{g}^o\left(s\right)}$ and radius $r$	$\{s,x\}$	$\begin{array}{c}{\gamma }_1(s,x)={\left({\text{leftidx}}^F{g}^o\left(s\right)\right)}^2\left(r^2-{\Vert x-\frac{{\text{leftidx}}^F{f}^o\left(s\right)}{{\text{leftidx}}^F{g}^o\left(s\right)}\Vert }^2\right)\end{array}$
Capsule, the convex hull of two spheres with centers $c_1$ and $c_2$ at positions ${\text{leftidx}}^F{p}^{o_i}\left(s\right)=\frac{{\text{leftidx}}^F{f}^{o_i}\left(s\right)}{{\text{leftidx}}^F{g}^{o_i}\left(s\right)}$ and radii $r_1$, $r_2$	$\{s,x,\mu \}$	$\frac{{\text{leftidx}}^F{f}^{o_{\mu }}}{{\text{leftidx}}^F{g}^{o_{\mu }}}=\mu \frac{{\text{leftidx}}^F{f}^{o_1}\left(s\right)}{{\text{leftidx}}^F{g}^{o_1}\left(s\right)}+\left(1-\mu \right)\frac{{\text{leftidx}}^F{f}^{o_2}\left(s\right)}{{\text{leftidx}}^F{g}^{o_2}\left(s\right)}$ $r_{\mu }=\mu r_1+\left(1-\mu \right)r_2$ ${\gamma }_1(s,x,\mu )={\left({\text{leftidx}}^F{g}^{o_{\mu }}\left(s\right)\right)}^2\left(r_{\mu }^2-{\Vert x-\frac{{\text{leftidx}}^F{f}^{o_{\mu }}}{{\text{leftidx}}^F{g}^{o_{\mu }}}\Vert }^2\right)$ ${\gamma }_2\left(\mu \right)=\mu$ ${\gamma }_3\left(\mu \right)=1-\mu$
Cylinder, the convex hull of two circles with centers $o_1$ and $o_2$, at position ${\text{leftidx}}^F{p}^{o_i}\left(s\right)=\frac{{\text{leftidx}}^F{f}^{o_i}\left(s\right)}{{\text{leftidx}}^F{g}^{o_i}\left(s\right)}$, lying in the plane normal to ${\text{leftidx}}^F{p}^{o_1}\left(s\right)-{\text{leftidx}}^F{p}^{o_2}\left(s\right)$, and with radii $r_1$ and $r_2$.	$\{s,x,v,\mu \}$	$\frac{{\text{leftidx}}^F{f}^{o_{\mu }}\left(s\right)}{{\text{leftidx}}^F{g}^{o_{\mu }}\left(s\right)}=\mu \frac{{\text{leftidx}}^F{f}^{o_1}\left(s\right)}{{\text{leftidx}}^F{g}^{o_1}\left(s\right)}+\left(1-\mu \right)\frac{{\text{leftidx}}^F{f}^{o_2}\left(s\right)}{{\text{leftidx}}^F{g}^{o_2}\left(s\right)}$ $r_{\mu }=\mu r_1+\left(1-\mu \right)r_2$ $h_1(v,s)=v^T\left(\frac{{\text{leftidx}}^F{f}^{o_1}\left(s\right)}{{\text{leftidx}}^F{g}^{o_1}\left(s\right)}-\frac{{\text{leftidx}}^F{f}^{o_2}\left(s\right)}{{\text{leftidx}}^F{g}^{o_2}\left(s\right)}\right)$ $h_2(s,x,\mu ,v)=x-\frac{{\text{leftidx}}^F{f}^{o_{\mu }}\left(s\right)}{{\text{leftidx}}^F{g}^{o_{\mu }}\left(s\right)}-v$ ${\gamma }_1(v,\mu )=r_{\mu }^2-v^{T}v$ ${\gamma }_2\left(\mu \right)=\mu$ ${\gamma }_3\left(\mu \right)=1-\mu$

Equations

$$-\pi <{\theta }_l\le \theta \le {\theta }_u<\pi ,$$

$$z_l\le z\le z_u.$$

$$\text{Find }a,b$$

$$a^{T}x+b>0,\forall x\in \mathcal{A}$$

$$a^{T}y+b<0,\forall y\in \mathcal{B}$$

$$\text{Find }x,y\text{subject to}$$

$$x\in \mathcal{A},y\in \mathcal{B}$$

$$x=y$$

$${\mathcal{S}}_{g,h}=\{x\mid g_i(x)\ge 0,h_j(x)=0,i\in [n],j\in [m]\},$$

$$x\in {\mathcal{S}}_{g,h}⟹p(x)\ge 0$$

$${\mathcal{S}}_{g,h}=\varnothing .$$

$$p(x)={\lambda }_0(x)+\sum _{i=1}^n{\lambda }_i(x)g_i(x)+\sum _{j=1}^m{\varphi }_j(x)h_j(x).$$

$$x\in {\mathcal{S}}_{g,h}⟹p(x)\ge 0$$

$$-1={\lambda }_0(x)+\sum _i{\lambda }_i(x)g_i(x)+\sum _j{\varphi }_j(x)h_j(x).$$

$$\text{leftidx}{}^F{X}^A=\left[\begin{array}{cc}\text{leftidx}{}^F{R}^A(q)& \text{leftidx}{}^F{p}^A(q)\\ 0_{1\times 3}& 1\end{array}\right]=\prod _{i\in {\mathcal{I}}_{F,A}}\text{leftidx}{}^{P_i}{X}^{C_i}(q_i)\text{leftidx}{}^{C_i}{X}^{P_{i+1}}$$

$$\text{leftidx}{}^{P_i}{X}^{C_i}(q_i)$$

$$\text{leftidx}{}^F{p}_w^A(q)=\sum _j{c}_{jw}\prod _{i\in {\mathcal{I}}_{F,A}}{\xi }_{ij,w}(q_i)$$

$$q=\bigcup _i\{{\theta }_i,z_i\}.$$

$$t_i:=\tan \left(\frac{{\theta }_i}{2}\right),$$

$$\cos ({\theta }_i)$$

$$s=\bigcup _i\{t_i,z_i\}.$$

$$\text{leftidx}{}^F{p}_w^A(s)=\sum _j{c}_{jw}\prod _{i\in {\mathcal{I}}_{F,A}}\frac{\text{leftidx}{}^F{f}_{ij,w}^A(s_i)}{\text{leftidx}{}^F{g}_{ij,w}^A(s_i)}=\frac{\text{leftidx}{}^F{f}_w^A(s)}{\text{leftidx}{}^F{g}_w^A(s)},w\in \{x,y,z\},$$

$$\frac{\text{leftidx}{}^F{f}_{ij,w}^A(s_i)}{\text{leftidx}{}^F{g}_{ij,w}^A(s_i)}\in \left\{\frac{1-t_i^2}{1+t_i^2},\frac{2t_i}{1+t_i^2},\frac{z_i}{1}\right\}.$$

$$\text{leftidx}{}^F{p}^A(s)=\frac{\text{leftidx}{}^F{f}^A(s)}{\text{leftidx}{}^F{g}^A(s)}$$

$$-\pi <{\theta }_{l,i}\le {\theta }_i\le {\theta }_{u,i}<\pi ,$$

$$z_{l,i}\le z_i\le z_{u,i}.$$

$$\left[\begin{array}[]{ c | c}R(\theta)&\begin{array}[]{c}p_{x}(\theta)\\ p_{y}(\theta)\end{array}\\ \hline\cr 0&1\end{array}\right]=\begin{bmatrix}\cos(\theta_{1})&\sin(\theta_{1})&0\\ \sin(\theta_{1})&-\cos(\theta_{1})&0\\ 0&0&1\end{bmatrix}\begin{bmatrix}1&0&0\\ 0&1&l_{1}\\ 0&0&1\end{bmatrix}*\\ \begin{bmatrix}\cos(\theta_{2})&\sin(\theta_{2})&0\\ \sin(\theta_{2})&-\cos(\theta_{2})&0\\ 0&0&1\end{bmatrix}\begin{bmatrix}1&0&0\\ 0&1&l_{2}\\ 0&0&1\end{bmatrix}$$

$$p_x({\theta }_1,{\theta }_2)$$

$$p_y({\theta }_1,{\theta }_2)$$

$$p_x(t_1,t_2)$$

$$p_y(t_1,t_2)$$

$$s\in \mathcal{P}⟹a^T(s)\text{leftidx}{}^F{p}^v(s)+b(s)\ge 1,$$

$$s\in \mathcal{P}⟹a^T(s)\text{leftidx}{}^F{p}^o(s)+b(s)\ge r\Vert a(s)\Vert ,$$

$$s\in \mathcal{P}⟹a^T(s)\text{leftidx}{}^F{f}^v(s)+(b(s)-1)\text{leftidx}{}^F{g}^v(s)\ge 0$$

Full text

1 Introduction

The notion of configuration space (C-space) has played a foundational role in robot motion planning since its proposal in the seminal work lozano1983spatial. In the presence of obstacles in the Cartesian task space, a fundamental challenge is describing the collision-free C-space (C-free): the full range of configurations for which a robot is not in collision. Prior work has taken two complementary approaches to this problem.

The first approach attempts to find an explicit description of the C-space obstacles from their task-space description and the inverse kinematics (IK). We refer to this approach as the negative approach, as C-free is described as the complement of the set of C-space obstacles. In its full generality, the problem of describing C-space obstacles is intractable canny1988complexity, and so limiting assumptions on the robot are often made. For example, kavraki1995computation develops a method for computing C-space obstacles based on the Fast Fourier Transform under the assumption that the robot can only translate in the workspace. In branicky1990computing, explicit descriptions of C-space obstacle due to the presence of point, line, and planar task-space obstacles are presented for two and three degree of freedom (DOF) robots. A thorough review of describing C-space obstacles can be found in (latombe2012robot, Chapter 3). There it is shown that if all the task-space obstacles are described as semi-algebraic sets (i.e. as the intersection and union of polynomial inequalities) then C-space obstacles are also semi-algebraic. This is an important result from a complexity-theoretic standpoint as it shows that describing the C-space obstacles is at least decidable, though still very hard.

We refer to the second approach as the positive approach, as it seeks to directly describe C-free as a union of simpler sets. This description is attractive as a variety of optimization-based motion planning methods can efficiently leverage such descriptions, particularly when the simpler sets are convex deits2015efficient; schouwenaars2001mixed; marcucci2021shortest; marcucci2022motion.

Rapidly-exploring Random Trees (RRT) lavalle1998rapidly, Probabilistic Roadmaps (PRM) kavraki1996probabilistic, and their variants can all be considered examples of this approach, describing C-free using piecewise-linear paths. Frequently, these methods provide probabilistic guarantees that the paths contain no collisions via sampling along the paths. To avoid false positive claims of non-collision, rigorous certification procedures such as schwarzer2004exact can be used. Works such as verghese2022configuration; han2019configuration; wong2014adaptive all seek to describe non-zero volume subsets of C-free. Similar to RRTs and PRMs, these methods have the advantage of working in arbitrary configuration spaces, make no assumptions on the C-space obstacles, and proceed via sampling. Therefore, they are typically relatively simple to implement and quite fast in low dimensions. Unfortunately, these methods only provide probabilistic guarantees of non-collision.

When the C-space obstacles are assumed to be convex, rigorous descriptions of C-free may be possible, though hardness results exist. For example, in two and three dimensions with polyhedral C-space obstacles, it is known that finding a minimal decomposition is NP-hard lingas1982power to solve exactly and even APX-hard eidenbenz2003approximation to approximate111A problem is said to be APX-hard if no polynomial time algorithm can achieve an approximation ratio of $1+\delta$ for some $\delta>0$ unless $P=NP$.. Works such as lien2007approximate and ghosh2013fast overcome these hardness results by finding decompositions that are unions of approximately convex sets.

In arbitrary dimensions and under the assumption of known, convex C-space obstacles, C-free can be decomposed into convex polyhedra by using the Iris algorithm of deits2015computing. As it is based on convex programming, Iris is relatively fast, and is also able to generate rigorous certificates of non-collision. Unfortunately, it is often the case that obstacles are naturally described as convex sets in task space, which are rarely convex in C-space.

In this work, we similarly provide a method for describing C-free using convex polyhedra in a bijective, rational parametrization of C-space known as the tangent configuration space (TC-space). Our primary technical contributions are two convex (specifically Sums-of-Squares (SOS)) programs which can certify that a polyhedron in TC-space contains no collision when the obstacles are specified as convex sets in task space. Similar to deits2015computing, we then construct certified, collision-free polytopic regions by alternating between a pair of convex programs. Our method works in arbitrary dimensions and is the first to our knowledge to provide rigorous certificates for non-zero volume sets in this setting. Moreover, we provide a fast, mature implementation technique in the open-source robotics toolbox Drake222https://drake.mit.edu/.

A conference version of this paper is published in amice2022finding, which assumes a robotic manipulator composed of revolute joints operating in a scene where all task-space obstacles are decomposed as a union of vertex representation (V-rep) polytopes. This journal version extends these results in many ways.

First, we demonstrate how our approach can be extended to handle other common, non-polytopic geometries such as spheres, capsules, and cylinders. Moreover, we describe how to extend our approach to handle a robot composed of any of the algebraic joints: revolute, prismatic, spherical, planar, and cylindrical. Our second technical contribution introduces a second method for certifying non-collision inspired by the dual of the separating hyperplane approach used in the conference paper. This approach takes the form of certifying the emptiness of a set of polynomial equations and inequalities which can also be written as an optimization program. The third technical contribution of this work is to show that feasibility of the optimization programs we use for certification is not only sufficient, but also necessary for a TC-space region to be collision free provided the degree of certain polynomials are chosen sufficiently large. Finally, we provide new examples of our algorithm deployed on various robots including 2-DOF robots to visualize the TC-space, a robot containing a prismatic joint, and a UR3e robot with collision geometries approximate by cylinders.

We begin in Section 2 by formally introducing our problem and our assumptions. We proceed in Section 3 by introducing necessary mathematical background for describing our technical approach. In Section 4, we present our most technical results: two convex programs which can certify whether a region of TC-space is collision-free. We also state the conditions under which feasibility of these programs are guaranteed when a proposed region is collision-free. We describe how to leverage the certification programs to generate convex decompositions of TC-free in Section 5. We conclude in Section 6 with examples of our algorithm deployed on various robots. We will first illustrate the algorithm on two simple 2-DOF systems where both the task and configuration spaces can be visualized and the entire configuration space can be quickly covered. We next demonstrate the ability of our algorithm to certify a wide range of postures for two realistic, 7-DOF manipulators interacting with a shelf. We conclude by showing our algorithm’s ability to scale by exploring two 12-DOF, bimanual manipulators.

Notation: Throughout the paper, we will use calligraphic letters ($\mathcal{S}$) to denote sets, Roman capitals ($X$) to denote matrices, and Roman lower case ($x$) to denote vectors. We use $[N]=\{1,\dots,N\}$, denote the set of all multivariate polynomials in the vector of variables $x$ as $\mathbb{R}[x]$, and denote the cone of Sums-of-Squares (SOS) polynomials as $\bm{\Sigma}$. Additionally, we will adopt the monogram notation of tedrakeManip for rigid transforms.

2 Problem Statement

We consider a known, task-space environment where our robot and all obstacles have been decomposed as a union of compact, convex bodies333For technical reasons, we formally assume that the bodies are compact, convex sets expressible as a Archimedean, basic semi-algebraic sets. See Appendix B for the definition of Archimedean for example cylinders, capsule, spheres, or vertex representation (V-rep) polytopes. Such collision geometries of our task space are readily available through standard tools such as V-HACD mamou2009simple and are often a required step for simulating any given environment.

Our robot is a mechanism composed of $N+1$ links connect via either revolute or prismatic joints wampler2011numerical:

• •

Revolute (R): a 1-DOF joint permitting revolution about an axis of symmetry. An example is a door handle.

• •

Prismatic (P): a 1-DOF joint permitting translation along an axis. An example is a linear rail.

We will assume that all revolute joints are constrained from undergoing complete rotations and all prismatic joints have bounded translation. Formally, if $\theta$ is the configuration-space variable associated to revolute joint, then:

[TABLE]

and if $z$ is the configuration-space variable associated to a displacement then:

[TABLE]

where the bounds $\theta_{l}$, $\theta_{u}$, $z_{l}$, and $z_{u}$ are fixed constants.

Our objective is to find large, convex regions of TC-free regardless of the dimension of the configuration space. This objective is beyond the scope of current decomposition for non-convex spaces/objects such as V-HACD due to the dimensionality of the problem for interesting robots and the complexity of the non-linear kinematics.

Remark 1**.**

Our approach can handle a robot composed of any of the five algebraic joints: revolute, prismatic, planar, cylindrical, planar, and spherical wampler2011numerical. We restrict ourselves to R and P joints as the other joints can be seen as a composition of these two (see appendix A for details).

3 Background

This section introduces key notions from convex analysis and algebraic geometry that will be essential for our approach presented in Section 4. We begin by recalling some classic theorems pertaining to the separation of convex bodies. We next review the Positivstellensatz, a central theorem from algebraic geometry that forms the basis for many applications of the Sums-of-Squares method that we will leverage. We conclude by recalling a parameterization of a robot’s forward kinematics using rational functions.

3.1 Separating Convex Bodies

In this section, we review two dual ways to check whether two compact, convex sets $\mathcal{A}$ and $\mathcal{B}$ intersect by using convex optimization. Our certification programs in Section 4 will rely on generalizations of the programs introduced in this section.

A well-known result from convex optimization theory is the Separating Hyperplane Theorem (boyd2004convex, Section 2.5) which states that $\mathcal{A}$ and $\mathcal{B}$ do not intersect, if and only if there exists a hyperplane $\mathcal{H}(a,b)=\{x\mid a^{T}x+b=0,(a,b)\neq(0,0)\}$ which strictly separates the two bodies. The hyperplane $\mathcal{H}(a,b)$ serves as a certificate of non-intersection. Such a hyperplane is visualized in Figure 1 and is described by the solution to program (3). Many previous works brossette2017collision; lin2022reduce have applied the Separating Hyperplane Theorem to find a single collision-free posture; in this paper we apply the theorem to find a convex set of collision-free postures.

Conversely, if $\mathcal{A}$ and $\mathcal{B}$ do intersect, then it is possible to certify this by finding a point in $\mathcal{A}\cap\mathcal{B}$. Such a point can be found by solving the convex optimization program (4). A certificate of the infeasibility of (4) proves that $\mathcal{A}$ and $\mathcal{B}$ do not intersect. Finding a certificate of infeasibility can be obtained by considering the dual of (4) and is a standard notion in convex optimization (boyd2004convex, Section 5.8).

A solution to program (3) has the advantage of being able to quantify the magnitude of separation between the two bodies. Therefore, in Section 5 we will prefer to base our algorithm on a generalization of (3). However, we will see that certain results will be easier to show by considering the infeasibility of program (4).

We conclude by noting that programs (3) and (4) are strong alternatives; exactly one of the two programs is feasible. The key to solving either program is to find a finite parameterization of conditions (3a), (3b), and (4a). In Table 5, we provide a convenient reference for some common geometries.

Remark 2**.**

Problem (3) is frequently written with non-strict inequalities (3a) and (3b) to make it compatible with modern solvers. Such a formulation requires excluding the trivial solution $(a,b)=(0,0)$ via extra constraints as well as planes which are not strictly separating. The conditions given in Table 5 accomplish both with the constraint $a^{T}x+b\geq 1$ with $x=v_{i}$ for polytopic geometries and $x=o$ for sphere, cylinder, and capsules.

3.2 Certificates of Positivity and Infeasibility

In Section 4, we will show how to generalize programs (3) and (4) to be able to certify non-collision for a range of robot configurations. Both generalizations will reduce to well-studied polynomial problems. Specifically, given the set

[TABLE]

where $g_{i}(x)$ and $h_{j}(x)$ are all given polynomial functions of $x$, then certifying the separating hyperplane conditions (3a) and (3b) will be akin to a certifying a polynomial implication of the form

[TABLE]

where $p(x)$ is again a polynomial.

Moreover, certifying the infeasibility of (4) will be akin to certifying that

[TABLE]

Both of these polynomial problems are tractable. In particular, a class of results known as Positivstellensatz Theorems (Psatz) can be used to reduce both problems to a convex optimization program parrilo2000structured; blekherman2012semidefinite. In this section, we review the Psatz results that we will use.

Our assumption (1) and (2) that our robot has joint limits implies that the subsets of TC-free we wish to certify will be Archimedean sets, a property slightly stronger than compactness formally defined in Appendix B. This will enable us to use a very strong Psatz Theorem for proving implications of the form (5) known as Putinar’s Positivstellensatz.

Theorem 1** (Positivstellensatz putinar1993positive).**

Suppose $\mathcal{S}_{g,h}$ is Archimedean and suppose that $p(x)>0$ for all $x\in\mathcal{S}_{g,h}$. Then there exists polynomials $\phi_{j}(x),~{}j=0,\dots,m$ and SOS polynomials $\lambda_{i}(x),~{}i=0,\dots,n$ such that:

[TABLE]

Moreover, if $p(x)$ is any polynomial that can be expressed as in (7), then

[TABLE]

As an immediate corollary, the previous theorem can be used to prove that $\mathcal{S}_{g,h}$ is empty.

Theorem 2** (parrilo2004sum).**

Suppose $\mathcal{S}_{g,h}$ is Archimedean. Then $\mathcal{S}_{g,h}=\emptyset$ if and only if there exists polynomials $\phi_{j}(x)$ and SOS polynomials $\lambda_{i}(x)$ such that

[TABLE]

In both cases, the multiplier polynomials $\lambda$ and $\phi$ serve as certificates that the conditions (5) or (6) hold. These certificates can be searched for using a convex optimization technique known as Sums-of-Squares (SOS) programming, a subset of semidefinite programming (SDP) parrilo2000structured. The SOS technique has been widely used in robotics, for example in stability verification tedrake2010lqr; majumdar2017funnel; shen2020sampling, reachability analysis jarvis2003some; yin2021backward and geometric modeling ahmadi2016geometry. In this paper, we will use SOS programming to generate certificates that subsets of TC-space are contained in TC-free.

3.3 Rational Forward Kinematics

Our method in Section 4 will rely critically on parameterizing the forward kinematics of our robot using polynomials. Many robots contain rotational joints and so their forward kinematics are naturally specified as trigonometric functions. In this section, we review a standard change of variables of our robot kinematics which will enable us to parameterize the forward kinematics as a rational function.

The forward kinematics of a rigid-body robot with $N$ joints can be written by composing rigid transforms craig2005introduction; tedrakeManip. Written in homogeneous coordinates, and using the monogram notation tedrakeManip666In monogram notation, the pose of a frame $A$ expressed in a frame $F$ is denoted as $\leftidx{{}^{F}}X^{A}$., the pose of a frame $A$, expressed in the reference frame $F$, as a function of the robot configuration $q$ assumes the form:

[TABLE]

In equation (10), $\mathcal{I}_{F,A}=\{i_{1},\ldots,i_{n}\}\subseteq[N]$ is the set of joints lying on the kinematic chain between $F$ and $A$. We attach two frames to each joint, with $P_{i}$ rigidly fixed to the parent link of the $i$th joint, and $C_{i}$ rigidly fixed to the child link of the same joint. The two frames $P_{i}$ and $C_{i}$ coincide when the joint configuration $q_{i}=0$. The subset of configuration variables $q_{i}$ defines the degrees of freedom at the $i$th joint, $\leftidx{{}^{P_{i}}}X^{C_{i}}(q_{i})$ is the relative transform of the joint after the joint moves by $q_{i}$. The rigid transform $\leftidx{{}^{C_{i}}}X^{P_{i+1}}$ describes the physical properties of the $i$th link such as its length. We assume that the reference frame $F$ is the $P_{i_{1}}$, the parent frame of the first joint $i_{1}$; while the frame $A$ is $C_{i_{n}}$, the child frame of the last joint $i_{n}$. 777Since joint $i_{n}$ is the last joint on this chain $\mathcal{I}_{F,A}$, we assume $\leftidx{{}^{C_{i_{n}}}}X^{P_{i_{n+1}}}=I$. We choose to be explicit about the reference frame $F$ at the risk of being pedantic, as the choice of reference frame $F$ will have important consequences for the scalability of the approach described in Section 5 (see Appendix F.1 for a detailed discussion).

The matrices $\leftidx{{}^{P_{i}}}X^{C_{i}}(q_{i})$ assume the following forms wampler2011numerical

[TABLE]

Expression (10) expresses the position of our robot as an multilinear trigonometric polynomial function. Concretely, the $w$th component (where $w\in\{x,y,z\}$) of the position of $A$ relative to $F$ and expressed in $F$ is an expression of the form:

[TABLE]

with $\xi_{ij,w}(q_{i})\in\{\cos(\theta_{i}),\sin(\theta_{i}),z_{i}\}$. The scalar constants $c_{jw}$ are determined by the robot kinematic parameters (link length, joint axis, etc). Therefore, our configuration-space variables are

[TABLE]

Multilinear trigonometric functions have many fortunate algebraic properties which we exploit throughout this paper, the first of which will be a change of variables enabling us to write (12) as a rational function.

Specifically, we will introduce the substitution:

[TABLE]

which allows us to write

[TABLE]

This substitution is known as the stereographic projection spivakCalc and is bijective if $\theta_{i}\in(-\pi,\pi)$ which we have assumed is the case for our robotic system888An alternative approach is to write the forward kinematics $p_{w}(q)$ as a multilinear polynomial of indeterminates $c_{i}=\cos(\theta_{i})$ and $s_{i}=\sin(\theta_{i})$, with the additional constraints $c_{i}^{2}+s_{i}^{2}=1$. We don’t choose this parameterization as it is hard to integrate the volume on the quotient ring $c_{i}^{2}+s_{i}^{2}=1,\;\forall i$. Also this parameterization requires introducing two variables $c_{i},s_{i}$ for each revolute joint, rather than one variable $t_{i}$.. After performing this change of variables, our forward kinematics variables are

[TABLE]

We refer to the configuration-space variable $s$ as the tangent-configuration-space (TC-space) variable.

In the TC-space variable, our forward kinematics are a rational function with a polynomial numerator and positive, polynomial denominator. This is an expression of the form

[TABLE]

where

[TABLE]

We will abbreviate the vector quantity:

[TABLE]

where $\leftidx{{}^{F}}f^{A}(s)$ is a vector of polynomials and $\leftidx{{}^{F}}g^{A}(s)$ is a single, positive polynomial. Notice that $\leftidx{{}^{F}}g^{A}(s)>0$ since each denominator $\leftidx{{}^{F}}g_{ij,w}^{A}(s_{i})=1+t_{i}^{2}\text{ or }1$, which is strictly positive.

We emphasize again that we have assumed:

[TABLE]

and therefore generically $s_{l}\leq s\leq s_{u}$ component-wise.

Therefore, our substitution between $q$ and $s$ is bijective and so trajectories in TC-space correspond unambiguously to trajectories in C-space. Moreover, this assumption on boundedness of our configuration space allows us to seek collision-free regions $\mathcal{P}$ that are contained within $\mathcal{P}_{lim}$, a polytope encoding our joint limit: $\mathcal{P}\subseteq\mathcal{P}_{lim}=\{s\mid s_{l}\leq s\leq s_{u}\}$.

Example 1**.**

As an example, we consider the double pendulum underactuated.

The pose of the tip of the second pendulum can be written as:

[TABLE]

The difference in the sign of the trigonometric part ensures that the $y$-axis is pointing down. Expanding out this product enables us to write the $x$ coordinate of the tip of the system as:

[TABLE]

Notice that these are multilinear trigonometric polynomials, i.e. no term contains $\cos(\theta_{i})\sin(\theta_{i})$. We can perform the substitution given in (13) to express the position as a rational function:

[TABLE]

.

4 Certification of Set-Membership in TC-Free

In this section, we will consider the problem of certifying the non-collision of two convex bodies $\mathcal{A}$ and $\mathcal{B}$ whose poses in task space are a function of the configuration of our robot. While programs (3) and (4) can be used to certify non-collision between $\mathcal{A}$ and $\mathcal{B}$ for any fixed configuration, they are insufficient to certify $\mathcal{A}$ and $\mathcal{B}$ do not intersect for all configurations in an entire region $\mathcal{P}$ of the configuration space. Therefore, in Sections 4.1 and 4.2, we will show how to combine the ingredients of Section 3 to generalize programs (3) and (4).

The presence of trigonometric functions when the forward kinematics are expressed in the variable $q$ precludes using SOS programming, our tool of choice. Therefore, we will assume that $\mathcal{A}(s)$ and $\mathcal{B}(s)$ are convex sets in task space with their poses expressed as rational functions in the TC-space variable $s$. This can be achieved using the developments in Section 3.3. Our objective will be to certify that $\mathcal{A}(s)$ and $\mathcal{B}(s)$ do not intersect for all $s\in\mathcal{P}=\{s\mid Cs\leq d\}\subseteq\mathcal{P}_{lim}=\{s\mid s_{l}\leq s\leq s_{u}\}$.

Under these assumptions, the generalizations of (3) and (4) will respectively take the form of certifying a polynomial implication and certifying the emptiness of a basic-semialgebraic set. We give a formulation of each as a SOS program. We will conclude in Section 4.3 by proving that feasibility of our convex optimization programs is both necessary and sufficient for $\mathcal{P}$ to be collision-free.

4.1 Parametrized Hyperplane Certificates of Non-Collision

In this section, we generalize (3) and use SOS to search for a polynomial family of hyperplanes parametrized by the TC-space variable $s$ which will certify the non-collision of $\mathcal{A}(s)$ and $\mathcal{B}(s)$ for all $s\in\mathcal{P}=\{s\mid Cs\leq d\}$.

We begin by remarking that even if $\mathcal{A}(s)$ and $\mathcal{B}(s)$ do not collide for all $s\in\mathcal{P}$, there may not be a single, static hyperplane $\mathcal{H}=(a,b)$ which certifies this fact. An example of this can be seen in Figure 3.

We therefore will look for a polynomial family of hyperplanes $\mathcal{H}(s)=\{x\mid a(s)^{T}x+b(s)=0\}$ parametrized by our TC-space variable $s$. Inspection of Table 5 shows that we must generalize

[TABLE]

for particular points $v$ specific to each of the geometries, and

[TABLE]

for center $o$ if $\mathcal{A}(s)$ is either a sphere or capsule. The generalization of the conditions for the cylinder are similar to those of the sphere and capsule, and so we defer its complete derivation to Appendix D.

To generalize (16) and (17), we recall that the position of any point $A\in\mathcal{A}(s)$ (and similarly $\mathcal{B}(s)$) can be expressed as a rational function $\leftidx{{}^{F}}p^{A}(s)=\frac{\leftidx{{}^{F}}f^{A}(s)}{\leftidx{{}^{F}}g^{A}(s)}$ where $\leftidx{{}^{F}}g^{A}(s)>0$.

Therefore, we can express (16) as:

[TABLE]

This is an polynomial implication of the form (8). As $\mathcal{P}\subseteq\mathcal{P}_{lim}$ is compact polytope, $\mathcal{P}$ is Archimedean (marshall2008positive, Theorem 7.1.3) and so we can use Theorem 1 to express condition (16) as:

[TABLE]

where $\lambda_{j1},j=0,\ldots,m$ are all SOS polynomials.

The condition (17), can be expressed as a polynomial, matrix inequality using the Schur complement999 We have that $\gamma\geq r\left\|a\right\|$ if and only if the Schur complement $\begin{bmatrix}\gamma I_{3}&ra\\ ra^{T}&\gamma\end{bmatrix}\succeq 0$.

boyd2004convex

[TABLE]

This is known as a matrix SOS condition which can be represented as a set of semidefinite constraints nie2011polynomial. Specifically, by introducing a vector auxillary variable $u$, we can write (20) as:

[TABLE]

which can be expressed as the SOS condition:

[TABLE]

where $\lambda_{j2}$ are all SOS polynomials, and $\phi\in\mathbb{R}[u,s]$. We introduce the additional equality $u^{T}u=1$ to make the set $\{(u,s)|s\in\mathcal{P},u^{T}u=1\}$ an Archimedean set.

We are now ready to describe our convex program certifying that $\mathcal{P}$ is a region of TC-space containing no collision. For each pair of bodies $\mathcal{A}(s)$ and $\mathcal{B}(s)$ which can collide in the scene, we search for a polynomial hyperplane via the optimization program:

[TABLE]

where $(a_{\mathcal{A},\mathcal{B}}(s),b_{\mathcal{A},\mathcal{B}}(s))$ are the parameters of the polynomial hyperplane separating $\mathcal{A}$ and $\mathcal{B}$, the polynomials $\lambda_{ij}^{\mathcal{A},\mathcal{B}}(s)$ and $\phi^{\mathcal{A},\mathcal{B}}(s)$ collect all the multiplier polynomials for enforcing (23b), and $\mu_{ij}^{\mathcal{A},\mathcal{B}}(s)$ and $\chi^{\mathcal{A},\mathcal{B}}(s)$ collect all the multiplier polynomials for enforcing (23c) by using (19) and (22) depending on the geometry of $\mathcal{A}$ and $\mathcal{B}$. We stress in the above program that the decision variables are the coefficients of the polynomials $a_{\mathcal{A},\mathcal{B}}$, $b_{\mathcal{A},\mathcal{B}}$, and the multiplier polynomials. The symbols $u$ and $s$ are known as indeterminates and are not explicitly searched over.

In Table 2, we summarize the conditions for enforcing (23b) and (23c) for common families of sets. We call a feasible solution to (23) a certificate for the polytope $\mathcal{P}$ which we denote:

[TABLE]

4.2 Polynomial Infeasibility Certificates

As we remarked in section 3.1, non-collision of two convex shapes $\mathcal{A}$ and $\mathcal{B}$ can be checked by certifying the infeasibility of (4). The infeasibility of (4) can be extended to the case when the locations of $\mathcal{A}(s)$ and $\mathcal{B}(s)$ are a function of $s$.

[TABLE]

An equivalent, and perhaps more instructive, way of expressing (25) is to consider the set

[TABLE]

and to consider the problem

[TABLE]

In (31), $\gamma_{i}^{\mathcal{A}}(s,x,u_{\mathcal{A}})$ and $h_{j}^{\mathcal{A}}(s,x,u_{\mathcal{A}})$ are the polynomials encoding the condition that $x\in\mathcal{A}(s)$ and $u_{\mathcal{A}}$ collects any extra variables needed to write this condition. Similarly, $u_{\mathcal{B}}$, $\gamma_{k}^{\mathcal{B}}(s,x,u_{\mathcal{B}})$, and $h_{l}^{\mathcal{B}}(s,x,u_{\mathcal{B}})$ encode that $x\in\mathcal{B}(s)$. We provide explicit expressions for $\gamma_{i}^{\mathcal{A}},\gamma_{k}^{\mathcal{B}}$ and $h_{j}^{\mathcal{A}},h_{l}^{\mathcal{B}}$ in Table 4 (given in Appendix C) for a few common geometries.

Example 2**.**

If $\mathcal{A}$ is a polytope with $n_{\mathcal{A}}$ vertices given by $v_{\mathcal{A}_{i}}$, and $\mathcal{B}$ is a sphere with center $o_{\mathcal{B}}$ and radius $r_{\mathcal{B}}$, then we can write

[TABLE]

Now, we note that $\mathcal{S}_{\mathcal{P},\mathcal{A},\mathcal{B}}$ is an Archimedean set. This implies that we can use Theorem 2 to write (32) as an optimization problem. Denoting $u=\{u_{\mathcal{A}},u_{\mathcal{B}}\}$, this can be written explicitly as

[TABLE]

We again emphasize that in program (33) the decision variables are the coefficients of $\lambda_{0},~{}\lambda_{j}^{\mathcal{P}},~{}\lambda_{i}^{\mathcal{A}},~{}\lambda_{l}^{\mathcal{B}},~{}\phi_{j}^{\mathcal{A}},~{}\text{and }\phi_{k}^{\mathcal{B}}$, while the symbols $\{x,s,u\}$ are not decision variables but rather polynomial indeterminates. Similar to the program in (23), a certificate of non-collision can be obtained by solving (33) for each pair $(\mathcal{A},\mathcal{B})$ with the multipliers acting as the certificate.

4.3 Power of the Certification Programs

In this section, we consider the power of both certification programs. Specifically, in Sections 4.1 and 4.2 we argued that feasibility of (23) and (33) are sufficient to prove that $\mathcal{P}$ is collision-free. In this section, we present two theorems showing that the feasibility of these programs is also necessary.

Such a result is important given the fact that as stated, (23) and (33) are infinite dimensional and therefore in practice must be solved by selecting a basis of finite degree for the polynomials. Other subtleties about the power of our formulation are discussed in Appendix E. Fortunately, we can prove that there do exist finite degrees such that both programs become feasible when $\mathcal{P}$ is truly collision-free.

Theorem 3**.**

Let all multiplier polynomials from (23) have degree at least $\rho$ and let all of the polynomials in the parameterization of the hyperplane have degree at least $\kappa$. Suppose $\mathcal{P}\subseteq\mathcal{P}_{lim}$ is a subset of TC-free.

Then there exists finite $\kappa$ and $\rho$ sufficiently large such that (23) is feasible.

A similar theorem can be stated for the program in (33).

Theorem 4**.**

Let $\mathcal{P}\subseteq\mathcal{P}_{lim}$ be a compact, polytopic subset of TC-free and let all multiplier polynomials from (33) have degree at least $\rho$. There exists a finite $\rho$ sufficiently large such that (33) is feasible.

We delay the proofs and further discussion of these results to Appendix E. For now, we simply remark that Theorems 3 and 4 assert that the certification programs presented in this section are both complete in the sense that any collision-free polytope $\mathcal{P}$ can be certified with our technique.

5 Polyhedral Decomposition of TC-free

In this section, we describe our algorithm for rapidly generating certified, polyhedral decomposition of TC-free. Our algorithm can be seen as a generalization of the Iris algorithm of deits2015computing to non-convex TC-space obstacles and so we name it C-Iris (Configuration-Space, Iterative Regional Inflation by Semidefinite programming). The key idea is to iteratively grow certified convex polytopes of increasing size around various important configurations in the TC-space. This is achieved by solving a series of convex optimization programs. The complete algorithm is summarized in Algorithm 1.

We begin by discussing how we will measure the size of our polytope $\mathcal{P}=\{s\mid Cs\leq d\}$. While it may be attractive to measure the size of a polytope by its volume, it is known that computing the volume of a half-space representation (H-Rep) polytope is #P-hard101010#P-hard problems are at least as hard as NP-complete problems provan1983complexity. dyer1988complexity and therefore intractable as an objective. A useful surrogate for the volume of $\mathcal{P}$ used in deits2015computing is the volume of the maximum volume inscribed ellipse of $\mathcal{P}$: the set $\mathcal{E}_{\mathcal{P}}=\{Qs+s_{0}\mid\left\|s\right\|_{2}\leq 1\}$ where $Q$ is a positive-semidefinite matrix describing the shape of the ellipsoid and $s_{0}$ its center. The problem of finding the maximum volume inscribed ellipsoid in a polytope is a semidefinite program described in (boyd2004convex, Section 8.4.2).

[TABLE]

As we wish our polytopes to cover diverse areas of TC-free, we will grow each polytope $\mathcal{P}$ around some nominal configuration $s_{s}$ we call the seed point. New seed points are typically chosen using rejection sampling to obtain a point outside of the existing certified regions. The polytope $\mathcal{P}$ is required to contain $s_{s}$ as it grows.

A maximal volume, certified polytope around $s_{s}$ can be obtained by solving the following optimization program which combines the ellipsoidal program (34) with the certification program (23) from Section 4.1.

[TABLE]

The condition $\mathcal{E}_{\mathcal{P}}\subset\mathcal{P}$ is given by the constraints (35b). Constraint (35c) enforces that $\mathcal{P}$ grows around $s_{s}$. The added constraint (35d) prevents numerically undesirable scaling. Finally, (35e) enforces that we search for hyperplanes $(a_{\mathcal{A},\mathcal{B}}(s),b_{\mathcal{A},\mathcal{B}}(s))$ which separate each collision pair $\mathcal{A}(s)$ and $\mathcal{B}(s)$.

While this program is attractive as a specification, it is not convex due to bilinearity between $Q$ and $c_{i}$ in (34b) and the bilinearity between the multipliers and the defining equations of $\mathcal{P}$ implicit in (35e) (see Section 4.1). This bilinearity precludes simultaneous search of the polytope $\mathcal{P}$, inscribed ellipsoid $\mathcal{E}_{\mathcal{P}}$, and the corresponding certificate $\mathcal{C}_{\mathcal{P}}$. Therefore, we will approximate the solution to (35) by alternating between two convex programs; one of which will generate certificates of non-collision and one which will improve our polytope without violating the previous certificate.

Remark 3**.**

It is possible to replace (35e) with the equivalent constraints from program (33). We prefer to base our algorithm on (23) as it can be visualized (i.e. planes in the task space) and the polynomials contain fewer indeterminates and hence the optimization problem size is smaller. Also the separating planes approach produces separating certificates with quantifiable margins by measuring the distance from the collision geometries to the plane in task space.

We begin by demonstrating how a certified polytopic region can be improved. Suppose that a convex polytope $\mathcal{P}=\{s|Cs\leq d\}$ has been certified with certificate $\mathcal{C}_{\mathcal{P}}$ and the maximum inscribed ellipse $\mathcal{E}_{\mathcal{P}}$ has been computed using (34). A new, larger polytope $\mathcal{P}^{\prime}$ can be found by solving the convex optimization program (36) which pushes the faces of $\mathcal{P}^{\prime}$ as far away from the surface of $\mathcal{E}_{\mathcal{P}}$ without violating the certificate $\mathcal{C}_{\mathcal{P}}$. This procedure is visualized in Figure 4.

This can be achieved with the following optimization program:

[TABLE]

where $\varepsilon_{0}>0$ is some positive constant ensuring that the objective is never [math]. We recall that (36c) is either a constraint of the form (19) or (22). We emphasize that in (36), $\lambda_{i1},\lambda_{i2},\mu_{i1},\mu_{i2},i\geq 1$ are all fixed and it is the variables $c_{j}$ and $d_{j}$ which are searched over.

Our complete algorithm proceeds in three steps. First, an initial, collision-free polytope $\mathcal{P}_{0}$ containing a seed point $s_{s}$ is certified using (24) to obtain $\mathcal{C}_{\mathcal{P}_{0}}$. Next, the maximum inscribed ellipsoid $\mathcal{E}_{\mathcal{P}_{0}}$ is computed using (34). Finally, $\mathcal{P}_{0}$ is improved using (36) to obtain a new polytope $\mathcal{P}_{1}$. This polytope $\mathcal{P}_{1}$ has the same number of defining inequalities as $\mathcal{P}_{0}$. We iterate this process until the volume of $\mathcal{E}_{\mathcal{P}}$ stops improving. This algorithm is formalized in Algorithm 1. Every step of this process involves solving an convex program for which very fast, commercial solvers exist mosek; andersen2000mosek.

Remark 4**.**

Some practical considerations for improving the runtime of Algorithm 1 are discussed in the appendices. Specifically, in Appendix F we expand on design choices which substantially impact the size of the optimization programs as well as which part of Algorithm 1 can be parallelized. Additionally, in Appendix G we discuss a heuristic strategy for proposing a large, initial regions $\mathcal{P}_{0}$.

6 Results

We demonstrate the use of Algorithm 1 on systems of varying complexity. We begin with very simple robots where both the task and configuration space can be visualized and demonstrate that our algorithm can find very large portions of TC-space and achieve near-complete coverage for simple systems in reasonable time.

We then demonstrate the use of Algorithm 1 on various robots commonly found in industry. These include a KUKA iiwa reaching into a shelf, a bimanual KUKA iiwa, and similar setups for the Franka UR3. Our objective is show the scalability of our algorithm in realistic settings as well as demonstrate the diversity of shapes our approach can handle.

A mature implementation of our algorithm is available in the open-source robotics toolbox Drake drake. We furthermore provide examples of our algorithm in interactive Python notebooks. Animations of various figures in this section can also be found on this project’s website.

The implementation details of all experiments in this section, such as the choice of reference frame for each plane, the degree of the polynomials parametrizing the hyperplanes, and the degree of the multipliers polynomials in each program are expounded on in Appendix F.

6.1 Simple Robots

In this section, we consider two simple robots each containing only two degrees of freedom. This enables us to visualize both the task space, as well as the configuration space. Though containing few degrees of freedom, each environment maintains rich, realistic collision geometries.

6.1.1 Pendulum on a Rail

Our first robot shown in Figure 5 consists of a single arm, shown in orange, connected to a base via a revolute joint and placed within a box. The base of the robot is connected to the box via a prismatic joint. The collision geometries of the robot and box are approximated using polytopic boxes. A total of $42$ pairs of geometries can collide in this scene (i.e. certifying non-collision requires solving $42$ instances of either (23) or (33)). In Figure 5, we visualize the two dimensional tangent configuration space of our robot with the TC-space obstacle shown in red. We emphasize the highly non-convex shape of TC-free.

We run Algorithm 1 starting with a regular octagon of side length $0.01$ centered at the configuration $(0,0)$, a configuration with the arm fully extended upwards and centered in the box. We obtain a sequence of certified polytopes of increasing size in the TC-space which are plotted in varying colors in Figure 5.

The algorithm terminates after 86 iterations of the while loop from Algorithm 1 taking a total of 314 seconds of wall time. During the course of the algorithm, the volume of the maximum inscribed ellipsoid improves by a factor of $83$, from a starting value of $0.021$ to $1.746$. The improvement in the volume of the inscribed ellipsoid, as well as the average time to solve both the certification program (23) and (36) are reported in Figure 6(a) and Table 6(b) respectively.

After completion, we select a single random configuration within our final certified region. In Figure 5, we highlight the tip of the pendulum in black. Additionally, we color each collision body for which the tip can collide in a separate color and plot the separating plane certificate between the tip and the body in the same color.

6.1.2 Pinball Flipper

We refer to our second system shown in Figure 7(a) as the pinball flipper. Each orange arm is connected to its gray base via a revolute joint. Each collision geometry in the scene is approximated with a box and a total of $130$ collision pairs exist. We similarly plot the TC-space in Figure 7(b) with the TC-space obstacle highlighted in red. In this experiment, we attempt to almost completely cover TC-free with polytopic regions in order to enable a motion plan where the flippers exchange positions. Overall, this scene exhibits a much more complicated TC-space obstacle as well as substantially more collision pairs when compared to the system from Section 6.1.1.

We run Algorithm 1 seeded with octagonal regions of side length $0.01$, each centered at one of $5$ different configurations shown as the black dots in Figure 7(b). The resulting regions are also plotted in Figure 7(b) and almost completely cover the space. Though each region was initially seeded with a polytope of the same shape, our algorithm successfully adapts the shape of each polytope to fill the space. Our algorithm also is not conservative; it successfully finding regions which are tight to the TC-space obstacle in all cases.

The change in volume of the maximum inscribed ellipsoid of each region is shown in Figure 8(a). We remark that the volume of each region exhibits a diverse set of behaviors over the iterations. Each region was grown sequentially, with a total wall time to cover the space of $1439$s. This wall time could easily be improved by growing each region in parallel.

In Figure 9, we demonstrate the behavior of our certificates for various poses of our robot. In the top panel, we highlight in black the two tips of each flipper. The current configuration is highlighted as the green dot in the bottom panel. For each configuration, we also plot the hyperplane that proves the separation between the two black tips. Notice that in Figures 9(g), 9(h), and 9(i), the current configuration is contained in multiple regions at once. Therefore, each hyperplane in Figure 9(a)

• 9(e) is drawn in the same color as its associated TC-space region in Figures 9(f)
• 9(j).

We draw attention to the fact that at every configuration $s_{0}$ in TC-free, many different separating hyperplanes exist. The hyperplane obtained by evaluating the output of our certifier at $s_{0}$ is highly dependent on the region which is being certified. For example, in Figure 9(h), the blue region corresponds largely to a change in the position of the left flipper, while the green region corresponds largely to a change in the right flipper. We see in Figure 9(c), that the algorithm finds different separating planes for the blue and the green region, even for the same configuration, so as to accommodate the different range of robot motion in each region. For the blue region, which includes a large rotation of the left flipper, the blue plane would continue to separate the left flipper from the right flipper as the left flipper moves. Similarly, the green plane would continue to separate the right flipper from the left as the right flipper moves.

6.2 KUKA IIWA robot

In this section we demonstrate our algorithm deployed on the KUKA iiwa arm in two scenes relevant to robot manipulation. The collision geometry of the iiwa is approximated as a union of convex polytopes as are all obstacles in the scene. We begin by considering a single iiwa to demonstrate the practicality of our algorithm before considering a bimanual manipulator to demonstrate the scalability of our approach.

6.2.1 7-DOF IIWA With a Shelf

We apply Algorithm 1 to the scene shown in Figure 10: a 7-DOF KUKA iiwa arm reaching into a shelf. Our approach successfully finds many collision-free configurations, and we plot in green the separating hyperplane certificate between the end-effector, highlighted in blue, and the top shelf highlighted in red.

The run time of Algorithm 1 is dominated by the certification of non-collision between the pairs with the longest kinematic chain, as this leads to the highest degree polynomials and hence semidefinite variables in programs (23) and (36). For this program, the largest positive semidefinite matrix variable has $16$ rows. Overall, the largest certification program (23) takes 54s to solve, while the program (36) takes on average 8s to solve.

In Figure 11, we demonstrate the behavior of one certified region. In Figure 11(a), we show that the configurations of one of our certified polytopic region of TC-space (with 24 faces in the polytope) corresponds to many task-space end-effector positions. The configurations from Figure 11(a) are drawn from a region which grows by a factor of $10,000$ using $11$ iterations of Algorithm 1. This improvement in volume is reported in Figure 11(b), where we also compare the volume of the maximum volume inscribed ellipsoid against the volume of the polytopic region.

6.2.2 12-DOF Bimanual KUKA IIWA Example

We next consider designing regions to avoid self-collision for a robot consisting of two KUKA iiwa arms with the final joint welded (rotation of the final joint does not change the configuration of any geometry for this robot). This robot contains 12-DOF. This system tests the scalability of our algorithm due to the degree of the polynomials involved in the forward kinematics, as well as the complexity of the collision geometries.

Solving the largest certification program in (23) takes 105 minutes, while the program in (36) takes 4 minutes. The increase in solve times compared to the single iiwa environment from Section 6.2.1 is best attributed to the increase in the size of the semidefinite variables due to the larger DOF. The largest semidefinite matrix in both programs have $64$ rows and correspond to certifying that the two tips of the iiwas do not collide.

Nonetheless, our algorithm again finds certified, 30-face polytopic regions of TC-space which correspond to a wide range of task-space positions as seen in Figure 12(a). Moreover, the same region is quite tight to the TC-space obstacle; one sampled configuration in the certified region, shown in Figure 12(b), corresponds to just $7.3$mm of separation between the two arms.

6.3 UR3e Robot

In this section, we test our algorithm on a UR3e robot with a gripper mounted at the wrist. The robot’s links are approximated by cylinders and we weld the gripper’s prismatic joints so that each UR3e has a total of 6 DOFs. This section differs from the KUKA iiwa experiment in Section 6.2 due to the introduction of non-polytopic collision geometries into the scene. Similar to Section 6.2, we test our approach for a scene where the robot is reaching into a shelf, as well as a bimanual set up.

6.3.1 6-DOF UR3e With a Shelf

In Figure 13, we consider a UR3e robot reaching into a shelf to grasp a small box shaped object. To simulate a situation where the robot is attempting to pick up the red object, we use Algorithm 1 to grow a certified, TC-free polytope (with 12 faces) near the object. Figure 13 shows a variety of postures sampled from the final TC-free polytope and demonstrates that within a single region, our robot is able to reach into the shelf to grasp the object, retract away from the shelf, and maneuver within the shelf while avoiding the object.

Similar to Section 6.2.1, the largest semidefinite variables in programs (23) and (36) has $16$ rows with program (23) taking about $56$s to solve.

6.3.2 12-DOF Bimanual UR3e

Finally, we demonstrate our algorithm on a dual UR3e platform shown in Figure 14. Again, we emphasize that we are able to find large regions of TC-configuration space which correspond to diverse positions in task space with the postures in Figure 14(a) and 14(b) being drawn from the same certified region (a 13-face polytope). Moreover, these regions are very tight to the TC-configuration space obstacle with the two bodies highlighted in red in Figure 14(b) being just $0.3$mm apart. For this example, the largest positive semidefinite matrices in (23) and (36) has 128 rows with the largest program taking about $35$ minutes to solve. This program solves faster than the analogous program for the bimanual iiwa from Section 6.2.2 because we require fewer polynomial positivity conditions to certify that the UR3e’s cylindrical geometries are on a given side of a plane compared to the polytopic approximation used for the iiwa.

7 Conclusion

Understanding the complicated geometry of C-free is an essential step to designing safe, collision-free motion plans. In this work, we presented an approach for describing a rational parametrization of C-free, known as TC-free, using a union of polytopes. Our primary contributions are two Sums-of-Squares program (23) and (33) which can certify that a polytopic region of TC-space is collision-free, as well as another program (36) which finds a local improvement that increases the size of a TC-free polytope. We prove that feasibility of our certification programs (23) and (33) are both necessary and sufficient for proving that a polytopic region of TC-space is collision-free and we combine programs (23) and (36) into a practical algorithm for describing TC-free as a union of certified, collision-free polytopes in the TC-space. We deployed our algorithm on both simple and realistic environments and demonstrate that Algorithm 1 finds large TC-space regions which correspond to diverse positions in task space. We demonstrate that these regions are not conservative and very tight to the TC-space obstacle even for 12-DOF systems by showing postures with just millimeters of separation.

The presented method works for TC-spaces of arbitrary dimensions, makes only very mild assumptions on the kinematics of our robot, and makes no assumptions about the shape of the TC-space obstacles. Moreover, it only relies on the mild assumption that obstacles in the task space are described as unions of convex sets, an assumption that is frequently satisfied whenever a given environment is simulated.

Such certified descriptions of TC-free find practical application in both randomized and optimization-based collision-free motion planning algorithms, providing a means to certify safety of an entire trajectory by checking membership in a set rather than by finite sampling which can be prone to false assertions of safety. Moreover, the convexity of the generated regions is particularly attractive to optimization-based methods such as the GCS framework of marcucci2022motion. Future work intends to further explore these applications as well as practical algorithms for seeding Algorithm 1 to obtain good coverage of TC-free with few regions.

8 Acknowledgement

This work was supported by the MIT Quest For Intelligence.

Appendix A Algebraic Kinematics

An in depth review of algebraic kinematics and low order pairs can be found in (wampler2011numerical, Chapter 4). We include a brief review in this appendix for completeness.

A mechanism composed of $N+1$ links is considered algebraic if each link is connected by one of the following five joints:

• •

Revolute (R): a 1-DOF joint permitting revolution about an axis of symmetry. An example is a door handle.

• •

Prismatic (P): a 1-DOF joint permitting translation along an axis. An example is a linear rail.

• •

Cylindrical (C): a 2-DOF joint permitting both revolution about an axis of symmetry and independent translation along a given axis. An example is the rods of a Foosball table.

• •

Planar (E): A 3-DOF joint permitting translation and rotation in a two-dimensional plane. An example is hockey puck moving on the surface of the ice.

• •

Spherical (S): A 3-DOF joint permitting free rotation between two links. An example is the human shoulder.

We recall from Section 3.3 that the pose of a point $A$ expressed in the reference frame $F$, written as a function of the robot configuration $q$ can be expressed as

[TABLE]

where $\leftidx{{}^{P_{i}}}X^{C_{i}}(q_{i})$ is a rigid transform describing the relative motion allowed by the $i$th joint. The matrices $\leftidx{{}^{P_{i}}}X^{C_{i}}(q_{i})$ are in general restriction of the following forms

[TABLE]

The specific restrictions for R, P, C, and E joints are given in Table 3. The matrix $U$ is an element of $SO(3)$ parametrized using Euler angles $\{\phi_{i,x},\phi_{i,y},\phi_{i,z}\}$.

We remark that the joints C, E, and S can be constructed by the composition of R and P joints.

• •

A C joint is a composition of an R joint and a P joint:

[TABLE]

• •

An E joint is the composition of one R joint and two P joints

[TABLE]

• •

An S joint is the composition of three R joints expressed as Euler angles.

[TABLE]

Our approach presented for a robot composed of R and P joints can be extended to handle any algebraic mechanism by consider the other algebraic joints as compositions of R and P joints.

Appendix B Definition of Archimedean

In this section we formally define the Archimedean property that appears in Theorem 1 and Theorem 2.

Definition 1**.**

A semialgebraic set $\mathcal{S}_{g}=\{x\mid g_{i}(x)\geq 0,i\in[n]\}$ is Archimedean if there exists $N\in\mathbb{N}$ and $\lambda_{i}(x)\in\bm{\Sigma}$ such that:

[TABLE]

Appendix C Semialgebraic Descriptions of Set Membership for Common Convex Bodies

Appendix D Parametrized Hyperplane Separation Condition for the Cylinder

To derive the hyperplane separation condition for cylinder, we first attach a geometric frame $G$ to the cylinder, as shown in Fig.15. The cylinder’s geometric frame $G$’s origin coincides with the cylinder’s center, with the $z$ axis of the $G$ frame along the cylinder axis. The height of the cylinder is $2h$, with the top/bottom circle radius being $r_{1}$ and $r_{2}$ respectively.

We first write the plane $\mathcal{H}$ with its parameters $\leftidx{{}^{G}}a(s),\leftidx{{}^{G}}b(s)$ in the cylinder’s geometric frame $G$ and derive the conditions on $\leftidx{{}^{G}}a(s),\leftidx{{}^{G}}b(s)$. The cylinder is in the positive side of the plane if and only if both its top and bottom rim are on the positive side of the plane, namely

[TABLE]

Taking the infimum of both sides with respect to $\alpha$ makes the above conditions equivalent to

[TABLE]

Next, we use the Schur complement, to reformulate (43a) and (43b) the positive semidefinite matrix conditions. For example, (43a) is equivalent to

[TABLE]

As explained in Section 4.1, this polynomial PSD condition can be reformulated as the condition

[TABLE]

To avoid the trivial solution $\leftidx{{}^{G}}a(s)=0,\leftidx{{}^{G}}b(s)=0$ (which is not in fact a separating plane), we add the extra constraint $\leftidx{{}^{G}}a^{T}\left(\frac{\leftidx{{}^{G}}p^{o_{1}}+\leftidx{{}^{G}}p^{o_{2}}}{2}\right)+\leftidx{{}^{G}}b\geq 1$. Here $\frac{\leftidx{{}^{G}}p^{o_{1}}+\leftidx{{}^{G}}p^{o_{2}}}{2}$ is the position of the cylinder center expressed in the geometric frame $G$ which coincides with the frame’s origin. Therefore $\frac{\leftidx{{}^{G}}p^{o_{1}}+\leftidx{{}^{G}}p^{o_{2}}}{2}=0$, and so it is sufficient to introduce the constraint

[TABLE]

to exclude the trivial solution $\leftidx{{}^{G}}a=0,\leftidx{{}^{G}}b=0$.

In our optimization program, we express the separating plane in a frame $F$ (where the choice of frame $F$ is discussed in F.1), not in cylinder’s geometric frame $G$. Hence we need to compute $\leftidx{{}^{G}}a(s),\leftidx{{}^{G}}b(s)$ from their corresponding terms $\leftidx{{}^{F}}a(s),\leftidx{{}^{F}}b(s)$ expressed in frame $F$ and the relative transform $\leftidx{{}^{F}}X^{G}$ between the two frames

[TABLE]

As described in Section 3.3, both the position $\leftidx{{}^{F}}p^{G}(s)$ and orientation $\leftidx{{}^{G}}R^{F}(s)$ are rational functions of $s$. By replacing $\leftidx{{}^{G}}a(s),\leftidx^{G}b(s)$ in (44b) and (45) with (46) and requiring the resulting numerator of the rational function to be non-negative, we derive that the plane separating cylinders can be enforced via a polynomial non-negativity condition which can be formulated as sums-of-squares condition.

Appendix E The Certification Programs are Necessary and Sufficient

In this section, we expand our discussion on the power of the certification programs presented in Sections 4.1 and 4.2. As remarked previously, Theorems 4 and 3 are necessary as programs (33) and (23) are infinite dimensional. It is not immediately obvious that for every robot and every scene, there exists a finite degree where in each program must become feasible when $\mathcal{P}$ truly contains no collisions.

A second subtlety applies specifically to (23). When generalizing (3), we argued that it was beneficial to search for a parametric hyperplane as a function of our TC-space variable $s$ and asserted that a polynomial parameterization was a good choice. However, it is not obvious that a polynomial parameterization is sufficient, and perhaps we require a rational or even more complicated parameterization of the plane.

These questions about the power of SOS programming arise in other domains. For example, SOS is commonly used to search for polynomial Lyapunov functions to prove the stability of polynomial dynamical systems majumdar2017funnel. However, it is known that not every stable polynomial dynamical system admits a polynomial Lyapunov function ahmadi2011globally, and therefore SOS programming is a sufficient, but not necessary tool for proving the stability of dynamical systems.

Fortunately, our certification programs from 4.1 and 4.2 are indeed necessary and sufficient, in the sense that there will always exist a finite degree such that the programs become feasible if $\mathcal{P}$ contains no collision. The proof of this for the program (33) follows immediately from Theorem 2.

Proof.

(of Theorem 4) Our assumptions on $\mathcal{P},~{}\mathcal{A}$, and $\mathcal{B}$ imply that $\mathcal{S}_{\mathcal{P},\mathcal{A},\mathcal{B}}$ is an Archimedean set. Therefore, the feasibility of (33) for sufficiently high degree $\rho$ follows immediately from “effective” versions of Theorem 2 such as those given in nie_complexity_2007; baldi2021moment which give explicit degree bounds. ∎∎

Though the proof of Theorem 3 is more technically involved, the key idea is simple. In short, we construct a family of continuous functions which map each TC-space configuration $s\in\mathcal{P}$ to a separating plane. We then argue that this family of continuous functions must contain hyperplanes which are parametrized as polynomials. Finally, we again appeal to “effective” versions of Theorem 1 such as those given in nie_complexity_2007; baldi2021moment to show that these polynomials can be found using SOS programming.

We proceed in steps, first establishing that the set of separating planes at a point $s$ in TC-free is open.

Proposition 1**.**

Let $\Phi(s)$ denote the set of strictly separating hyperplanes at the point $s$ for bodies $\mathcal{A}(s)$ and $\mathcal{B}(s)$ and let $\mathcal{P}$ be a non-empty, polytopic subset of TC-free. Then $s\in\mathcal{P}$ implies that $\Phi(s)$ is a non-empty, open set. ∎

Proof.

By definition, a hyperplane $\begin{bmatrix}a\\ b\end{bmatrix}\in\mathbb{R}^{4}$ strictly separates $\mathcal{A}(s)$ and $\mathcal{B}(s)$ if and only if there exist positive constants $\varepsilon_{\mathcal{A}}$ and $\varepsilon_{\mathcal{B}}$ such that $a^{T}x+b\geq\varepsilon_{\mathcal{A}}~{}\forall x\in\mathcal{A}(s)$and $a^{T}x+b\leq-\varepsilon_{\mathcal{B}}~{}\forall x\in\mathcal{B}(s)$. Since the bodies $\mathcal{A}(s)$ and $\mathcal{B}(s)$ are strictly separating for every point $s\in\mathcal{P}$, the Separating Hyperplane theorem guarantees the existence of such a vector and so $\Phi(s)$ is non-empty.

Now consider $\begin{bmatrix}a\\ b\end{bmatrix}\in\Phi(s)\subseteq\mathbb{R}^{4}$ and its $\delta$ neighborhood

[TABLE]

with $\delta>0$.

We have that for all $x\in\mathcal{A}$

[TABLE]

and similarly for all $x\in\mathcal{B}$

[TABLE]

Letting $M_{l}=\displaystyle{\min_{\left\|v\right\|=1,~{}x\in\mathcal{A}}}v^{T}_{a}x+v_{b}$ and $M_{u}=\displaystyle{\max_{\left\|v\right\|=1,~{}x\in\mathcal{B}}}v^{T}_{a}x+v_{b}$, we have that all planes $\mathcal{N}(\delta)$ are separating if

[TABLE]

and so $\Phi(s)$ is open. ∎∎

Proposition 2**.**

Define

[TABLE]

For all $s\in\mathcal{P}$ there exists $\delta_{\min}(s)>0$, not necessarily finite such that, $\mathcal{N}(s,\delta)$ is non-empty and open for every $0<\delta<\delta_{\min}$.

Proof.

Recall that the position of every point in $\mathcal{A}(s)$ is a continuous function of $s$ and that the distance from a point to a set is a continuous function rudin1976principles. Therefore, the distance of every point in $\mathcal{A}(s)$ to every element of $\Phi(s)$ changes continuously. For every $\delta>0$ and $\begin{bmatrix}a\\ b\end{bmatrix}\in\Phi(s)$ we define:

[TABLE]

We have that

[TABLE]

Moreover, if $\delta_{2}<\delta_{1}$, then $M_{\delta_{2}}(s)\leq M_{\delta_{1}}(s)$. By continuity and monotonicity, $M_{\delta}(s)\rightarrow 0$ as $\delta\rightarrow 0$ and so there exists $\delta$ sufficiently small such that $\varepsilon_{\mathcal{A}}-M_{\delta}(s)>0$. A similar argument shows that $\delta$ can be chosen sufficiently small such that the plane $\begin{bmatrix}a\\ b\end{bmatrix}$ continues to satisfy the separating plane conditions for $\mathcal{B}$. Therefore $\begin{bmatrix}a\\ b\end{bmatrix}\in\Phi(s+\delta v)$ for all $v$ such that $\left\|v\right\|\leq 1$ if $\delta$ is chosen sufficiently small. It is clear that choosing $\delta$ smaller continues to ensure that $\mathcal{N}(s,\delta)$ is non-empty. Openness is immediate following a similar argument to Proposition 1. ∎∎

The above proposition enables us to establish that there exists an open family of continuous functions $f(s)$ such that their outputs are always separating hyperplanes.

Proposition 3**.**

Let $\mathcal{F}$ be the set of continuous functions mapping

[TABLE]

such that $f(s)\in\Phi(s)$ for all $s\in\mathcal{P}$. The set $\mathcal{F}$ is non-empty and open under the pointwise metric

[TABLE]

Proof.

Suppose $\mathcal{F}$ were empty. Then every function satisfying $f(s)\in\Phi(s)~{}\forall s\in\mathcal{P}$ is not a continuous function. Namely, for every $f$ there exists a point $s_{0}$ such that for all $\delta>0$, $f(s_{0})\in\Phi(s_{0})$ but $f(s_{0})\notin\mathcal{N}(s_{0},\delta)$. This contradicts the openness of $\mathcal{N}(s_{0},\delta)$ for a sufficiently small $\delta$ from Proposition 2 and so $\mathcal{F}$ is non-empty. Openness follows from the fact that if $\delta>0$ is chosen sufficiently small, then for every continuous $g$ satisfying $d(f,g)<\delta$, then $g$ must also separate $\mathcal{A}(s)$ and $\mathcal{B}(s)$ for every $s\in\mathcal{P}$.

∎∎

We are now ready to prove Theorem 3

Proof.

(of Theorem 3) By Proposition 3, $\mathcal{F}$ is a non-empty open subset of continuous functions defined on the compact domain $\mathcal{P}$. The Stone-Weierstrass theorem rudin1976principles states that the set of polynomial functions on a compact domain is dense in the set of continuous functions in that domain under the pointwise metric. Therefore, $\mathcal{F}$ must contain a map $p:s\mapsto\begin{bmatrix}a(s)\\ b(s)\end{bmatrix}$ such that each component is a polynomial. This polynomial is of finite degree and is a strictly separating hyperplane and therefore by “effective” versions of Theorem 1 such as nie_complexity_2007; baldi2021moment, there exists a Putinar certificates of finite degree certifying that $p(s)$ is a separating hyperplane. ∎∎

Appendix F Practical aspects

In this section, we discuss some practical aspects for essential for enabling Algorithm 1 to realistic examples. These include the choice of reference frame in which to express the forward kinematics, the selection of a finite basis for the polynomials in our SOS programs, and which aspects of 1 can be parallelized.

F.1 Choosing the Reference Frame

The polynomial implications upon which the certification program (23) and polytope growth program (36) are based require choosing a coordinate frame between each collision pair $\mathcal{A}$ and $\mathcal{B}$. However, as the collision-free certificate between two different collision pairs can be computed independently of each other, we are free to choose a different coordinate frame to express the kinematics for each collision pair. This is important in light of (10) and (14) that indicate that the degree of the polynomials $\leftidx{{}^{F}}f^{\mathcal{A}_{j}}$ and $\leftidx{{}^{F}}g^{\mathcal{A}_{j}}$ are equal to two times the number of joints lying on the kinematic chain between frame $F$ and the frame for $\mathcal{A}$. For example, the tangent-configuration-space polynomial in the variable $s$ describing the position of the end-effector of a 7-DOF robot is of total degree $14$ when written in the coordinate frame of the robot base. However, when written in the frame of the third link, the polynomial describing the position of the end effector is only of total degree $(7-3)\times 2=8$. This observation is also used in trutman2020globally to reduce the size of the optimization program.

The size of the semidefinite variables in (23) and (36) scale as the square of the degree of the polynomial used to express the forward kinematics. Supposing there are $n$ links in the kinematics chain between $\mathcal{A}$ and $\mathcal{B}$, then choosing the $j$th link along the kinematics chain as the reference frame $F$ leads to scaling of order $j^{2}+(n-j)^{2}$. Choosing the reference frame in the middle of the chain minimizes this complexity to scaling of order $\frac{n^{2}}{2}$ and we therefore adopt this convention in our experiments.

F.2 Basis Selection

The condition that a polynomial can be written as a sum of squares can be equivalently formulated as an equality constraint between the coefficients of the polynomial and an associated semidefinite variable known as the Gram matrix parrilo2004sum. Namely, a polynomial $p(s)$ is sums-of-squares if and only if $p(s)=z(s)^{T}Xz(s),X\succeq 0$ where $z(s)$ is a vector of monomials and $X$ is the Gram matrix. The number of rows in the positive semidefinite Gram matrix equals to the size of the vector $z(s)$. In general, a sums-of-squares polynomial in $k$ variables of total degree $2d$ requires a Gram matrix of size ${k+d}\choose{d}$ to represent which can quickly become prohibitively large. Fortunately, the polynomials in our programs contain substantially more structure which will allow us to select a small-sized vector of monomials $z(s)$, and hence drastically reduce the size of the Gram matrices and speed up the optimization problem.

F.2.1 Polytopic collision geometry

We begin with the separating plane condition for polytopic collision geometries. Note that from (14) that while both the numerator and denominator of the forward kinematics are of total degree $2n$, with $n$ the number of links of the kinematics chain between frame $A$ and $F$, both polynomials are of coordinate degree of at most two (i.e. the highest degree of $s_{i}$ in any term is $s_{i}^{2}$). We will refer to this basis as $\nu(s)$ which is a vector containing terms of the form $\prod_{i=1}^{n}s_{i}^{\text{degree}(s_{i})}$ with $\text{degree}(s_{i})\in\{0,1,2\}$ for all $3^{n}$ possible permutations of the exponents $\text{degree}(s_{i})$.

We recall that we parametrize our hyperplane using polynomial entries. If $a_{\mathcal{A},\mathcal{B}}(s)=a^{T}_{\mathcal{A},\mathcal{B}}\eta(s)$, $b_{\mathcal{A},\mathcal{B}}(s)=b^{T}_{\mathcal{A},\mathcal{B}}\eta(s)$ for some basis $\eta$ in the variable $s$. The position of $x(s)\in\mathcal{A}(s)$ is expressed in basis $\nu(s)$, then the left hand side of (19) can be expressed as a linear function of the basis $\gamma(s)$, where $\gamma(s)$ contains all the possible entries that appear in the outer product $\eta(s)\nu(s)^{T}$.

Example 3**.**

Suppose

[TABLE]

and

[TABLE]

.

Then:

[TABLE]

Namely $\gamma(s)$ contains the monomials whose degree for each $s_{i}$ is at most 3, and only one of $s_{i}$ can have degree 3 (hence $s_{1}^{3}s_{2}^{3}$ is not included in $\gamma(s)$).

Similarly, we must select a basis $\rho(s)$ for our multiplier polynomials $\lambda_{ij}^{\mathcal{A},\mathcal{B}}(s)$. The equality in (19) determines the minimum necessary basis $\rho(s)$. If the polynomial $p(s)$ is expressed in basis $\gamma(s)$, then the minimal such basis is related to an object known in computational algebra as the Newton polytope of $\gamma$ denoted $\textbf{New}(\gamma(s))$ sturmfels1994newton. Denoting the linear basis

[TABLE]

then exact condition is that

[TABLE]

where the sum in this case is the Minkowski sum.

By using affine polynomials for separating plane parameters $a_{\mathcal{A},\mathcal{B}}(s),b_{\mathcal{A},\mathcal{B}}(s)$, we know that $\eta(s)$ is the same as the linear basis $l(s)$, then we obtain the condition that $\textbf{New}(\rho(s))=\textbf{New}(\nu(s))$ and since $\nu(s)$ is a dense, even degree basis we must take $\rho(s)=\nu(s)$. A sums-of-squares polynomial in the basis of $\nu(s)$ has Gram matrix with $2^{n}$ rows. Choosing $\eta(s)$ as the constant basis would in fact result in the same condition, and therefore searching for separating planes which are linear functions of the tangent-configuration-space variable does not increase the size of the semidefinite variables. As the complexity of (23) and (36) are dominated by the size of these semidefinite variables, separating planes which are linear functions changes do not substantially affect the solve time but can dramatically increase the size of the regions which we can certify.

Because of this, we choose to parametrize all of our hyperplanes throughout our experiments as linear functions of the TC-space variables. We stress that in general, the choice of a linearly parametrized hyperplane, and the selection of $\rho(s)$ to be the minimum size to match the degree of the left hand side of (19) may not be sufficient to prove that a region $\mathcal{P}$ is collision-free, even if $\mathcal{P}$ truly is collision-free. Indeed due of many complexity-theoretic results, we expect that in general $\eta(s)$ and $\rho(s)$ may need to have exponentially high degree for some robots, scenes, and polytopes $\mathcal{P}$ stengle1996complexity. However, in practice we have observed that the choices in this section are sufficient to certify many regions of interest, while keeping the optimization problem size tractable for state-of-art numerical solvers.

Remark 5**.**

Attempting to certifying that the end-effector of a 7-DOF robot will not collide with the base using program (23) using linearly parametrized hyperplanes and choosing to express conditions (19) in the world frame with naïvely chosen bases would result in semidefinite variables of size ${7+7\choose 7}=3432$. Choosing to express the same conditions according to the discussion in Section F.1 and choosing the basis $\gamma(s)$ described in this section results in semidefinite matrices of rows at most $2^{\lceil 7/2\rceil}=2^{4}=16$. The division by 2 comes from choosing the middle link as the expressed frame, hence halving the kinematic chain length.

F.2.2 Non-polytopic collision geometry

In this section, we use the sphere as a running example for explaining how we choose the monomial bases for certifying separation of the non-polytopic geometries; the monomial bases for capsules and cylinders can be derived in a similar manner.

As mentioned in (20), we need to impose

[TABLE]

By the definition of positive semidefinite matrix 111111A matrix $X$ is positive semidefinite if and only if $\forall\bar{u},\begin{bmatrix}\bar{u}\\ 1\end{bmatrix}^{T}X\begin{bmatrix}\bar{u}\\ 1\end{bmatrix}\geq 0$ , we know that the $4\times 4$ matrix in the right of $\implies$ in (47) is positive semidefinite if and only if

[TABLE]

We impose the following sufficient condition for (47), where $\mathcal{P}=\{s|c_{j}^{T}(s)\leq d_{j},j=1,\ldots,m\}$

[TABLE]

Now we analyze the degree of the polynomial $\sigma(\bar{u},s)$ defined in (48). As mentioned in the previous subsection, each monomial in $\leftidx^{F}f^{o}(s),\leftidx{{}^{F}}g^{o}(s)$ are of the form $\prod_{i=1}^{n}s_{i}^{\text{degree}(s_{i})},\text{degree}(s_{i})\in\{0,1,2\}$. Combining this with the choice of a separating plane $a(s),b(s)$ being affine functions of $s$, we derive that each monomial in $\sigma(\bar{u},s)$ is of the form $\bar{u}_{j}^{\text{degree}(\bar{u}_{j})}\prod_{i=1}^{n}s_{i}^{\text{degree}(s_{i})},\text{ where }\text{degree}(\bar{u}_{j})\in\{0,1,2\}$, $\text{degree}(s_{i})\in\{0,1,2,3\}$, and at most one of $\text{degree}(s_{i})$ can be 3. As an example, $\bar{u}_{1}^{2}s_{1}^{3}s_{2}s_{3}^{2}$ is a valid monomial in $\sigma(\bar{u},s)$ but $\bar{u}_{1}\bar{u}_{2}$ is not (because $\sigma(\bar{u},s)$ doesn’t contain the cross product between $\bar{u}_{j},\bar{u}_{k},j\neq k$). Similarly, $s_{1}^{3}s_{2}^{3}$ is not in the basis because at most one of $s_{i}$ can have degree 3. Given these properties on the monomials in $\sigma(\bar{u},s)$- specifically there being no cross-product term $\bar{u}_{j}\bar{u}_{k},j\neq k$ in $\sigma(\bar{u},s)$- we can write the positive polynomials $\lambda_{j}(\bar{u},s)$ as the summation of three SOS polynomials

[TABLE]

For each monomial in the SOS polynomial $\lambda_{j,k}(\bar{u}_{k},s)$, the degree of $\bar{u}_{k}\text{ and }s_{i}$ for $i=1,\ldots,n$ is either 0, 1, or 2. Hence the number of rows in the Gram matrix in $\lambda_{j,k}(\bar{u}_{k},s)$ is of size $2^{n+1}$. By choosing the reference frame according to the convention from Appendix F.1, $n$ is no larger than $\left\lceil N/2\right\rceil$ where $N$ is the number of joints in the robot.

Remark 6**.**

For a 6-DOF UR3erobot whose collision geometries are approximated by cylinders, to certify the collision-avoidance between the robot and objects in the world (or self-collision), the largest positive semidefinite matrix in our optimization problem has rows at most $2^{\lceil 6/2\rceil+1}=2^{4}=16$, where the division by 2 comes from choosing the middle link as the expressed link, hence halving the kinematic chain length to $\lceil 6/2\rceil$.

F.3 Parallelization

While it is attractive from a theoretical standpoint to write (23) as a single, large program it is worth noting that it can in fact be viewed as $K$ individual SOS programs, where $K$ is the number of collision pairs in the environment. Indeed, certifying whether pairs $(\mathcal{A}_{1},\mathcal{A}_{2})$ are collision-free for all $s$ in the polytope $\mathcal{P}$ can be done completely independently of the certification of another pair $(\mathcal{A}_{1},\mathcal{A}_{3})$ as the constraint are not coupled between any pairs. Similarly, the search for the largest inscribed ellipsoid can be done independently of the search for the separating hyperplanes.

Solving the certification program (23) as $K$ individual SOS programs has several advantages. First, as written (23) has $2(m+1)K\sum_{i}{\left|\mathcal{A}_{i}\right|}$ semidefinite variables of various sizes, where $m$ is the number of inequalities in $\mathcal{P}$ and ${\left|\mathcal{A}_{i}\right|}$ denotes the number of inequalities required to express that body $\mathcal{A}_{i}$ is on a particular side of the plane (see Table 4). In the example from Section 6.1.2 this corresponds to $18,720$ semidefinite variables. This can be prohibitively large to store in memory as a single program as the size of these semidefinite variables grow. Solving for the separating plane for each pair of collision bodies independently also enables us to determine which collision bodies cannot be certified as collision-free and allows us to terminate our search as soon as a single pair cannot be certified. Finally, decomposing the problems into subproblems enables us to increase computation speed by leveraging parallel processing.

The program (34) can also be solved completely independently of the certification program (23) and is in general a much smaller SDP than any individual certification program. Therefore, lines $3$ and $4$ of Algorithm 1 can be solved in parallel.

We note that (36) cannot be similarly decomposed as on this step the variables $c_{i}^{T}$ and $d_{i}$ affect all of the constraints. However, this program is substantially smaller as we have fixed $2mK\sum_{i}{\left|\mathcal{A}_{i}\right|}$ of the semidefinite variables as constants and replaced them with $2m$ linear variables representing the polytope. This program is much more amenable to being solved as a single program.

Appendix G Seeding Algorithm 1

Algorithm 1 must be initialized with a polytope $\mathcal{P}_{0}$ for which (23) is feasible. In principle, the alternation proposed in Section 5 can be seeded with an arbitrarily small polytope around a collision-free seed point. This seed polytope is then allowed to grow using Algorithm 1. However, this may require running several dozens of iterations of Algorithm 1 for each seed point which can become prohibitive as the number of degrees of freedom in our robot or the complexity of the scene grows. It is therefore advantageous to seed with as large a region as can be initially certified.

Here we discuss an extension of the Iris algorithm in deits2015computing which uses nonlinear optimization to rapidly generate large regions in TC-space. These regions are not guaranteed to be collision-free and therefore they must still be passed to Algorithm 1 to be certified, but do provide good initial guesses. In this section, we will assume that the reader is familiar with Iris and will only discuss the modification required to use it to grow TC-space regions. Detailed pseudocode is available in Appendix H.

Iris grows regions in a given space by alternating between two subproblems: SeparatingHyperplanes and InscribedEllipsoid. The InscribedEllipsoid is exactly the program described in (boyd2004convex, Section 8.4.2) and we do not need to modify it. The subproblem SeparatingHyperplanes finds a set of hyperplanes which separate the ellipse generated by InscribedEllipsoid from all of the obstacles. This subproblem is solved by calling two subroutines: ClosestPointOnObstacle and TangentPlane. The former finds the closest point on a given obstacle to the ellipse, while the latter places a plane at the point found in ClosestPointOnObstacle that is tangent to the ellipsoid.

The original work of deits2015computing assumes convex obstacles which enables ClosestPointOnObstacle to be solved as a convex program and for the output of TangentPlane to be globally separating plane between the obstacle and the ellipsoid of the previous step. Due to the non-convexity of the TC-space obstacles in our problem formulation, finding the closest point on an obstacle exactly becomes a computationally difficult problem to solve exactly ferrier2000computation. Additionally, placing a tangent plane at the nearest point will be only a locally separating plane, not a globally separating one.

To address the former difficulty, we formulate ClosestPointOnObstacle as a nonlinear program. Let the current ellipse be given as $\mathcal{E}=\{Qs+s_{0}\mid\left\|s\right\|_{2}\leq 1\}$ and suppose we have the constraint that $s\in\mathcal{P}=\{s\mid Cs\leq d\}$. Let $\mathcal{A}$ and $\mathcal{B}$ be two collision pairs and ${}^{\mathcal{A}}p_{\mathcal{A}},{}^{\mathcal{B}}p_{\mathcal{B}}$ be some point in bodies $\mathcal{A}$ and $\mathcal{B}$ expressed in some frame attached to $\mathcal{A}$ and $\mathcal{B}$. Also, let ${}^{W}X^{\mathcal{A}}(s)$ and ${}^{W}X^{\mathcal{B}}(s)$ denote the rigid transforms from the reference frames $\mathcal{A}$ and $\mathcal{B}$ to the world frame respectively. We remind the reader that this notation is drawn from tedrakeManip. The closest point on the obstacle subject to being contained in $\mathcal{P}$ can be found by solving the program

[TABLE]

This program searches for the nearest configuration in the metric of the ellipse such that two points in the collision pair come into contact. We find a locally optimal solution $(s^{\star},{}^{\mathcal{A}}p_{\mathcal{A}}^{\star},{}^{\mathcal{B}}p_{\mathcal{B}}^{\star})$ to the program using a fast, general-purpose nonlinear solver such as SNOPT gill2005snopt. The tangent plane to the ellipse $\mathcal{E}$ at the point $s^{\star}$ is computed by calling TangentPlane, then appended to the inequalities of $\mathcal{P}$ to form $\mathcal{P}^{\prime}$. This routine is looped until (51) is infeasible at which point InscribedEllipse is called again.

Once a region $\mathcal{P}=\{s\mid Cs\leq d\}$ is found by Algorithm 2, it will typically contain some minor violations of the non-collision constraint. To find an initial, feasible polytope $\mathcal{P}_{0}$ to use in Algorithm 1, we search for a minimal uniform contraction $\delta$ of $\mathcal{P}$ such that $\mathcal{P}_{\delta}=\{s\mid Cs\leq d-\delta*1\}$ is collision-free. This can be found by bisecting over the variable $\delta\in[0,\delta_{\max}]$ and solving repeated instances of (23).

Seeding Algorithm 1 with a $\mathcal{P}_{0}$ as above can dramatically reduce the number of alternations required to obtain a fairly large region and is frequently faster than seeding Algorithm 1 with an arbitrarily small polytope.

Appendix H Supplementary Algorithms

We present a pseudocode for the algorithm presented in Appendix G. A mature implementation of this algorithm can be found in Drake121212https://github.com/RobotLocomotion/drake/blob/2f75971b66ca59dc2c1dee4acd78952474936a79/geometry/optimization/iris.cc#L440.