Certified Polyhedral Decompositions of Collision-Free Configuration Space

Hongkai Dai, Alexandre Amice, Peter Werner, Annan Zhang, Russ Tedrake

Introduction

Understanding the geometry of collision-free configuration space (C-free) is fundamental to motion planning. While checking for collisions at a single point is straightforward using standard algorithms, computing entire certified C-free regions is notoriously difficult due to the complex, non-linear mapping of task-space obstacles through robot kinematics. This work introduces C-IRIS (C-space Iterative Regional Inflation by Semidefinite programming), the first rigorous method to decompose a robot's configuration space into large, convex, and mathematically certified collision-free regions.

The Challenge of Configuration Space

In robotics, "Configuration Space" (C-space) represents all possible joint positions. While obstacles are often simple convex shapes in physical "task space," they transform into complex, non-convex "blobs" in C-space. Most planners rely on point-sampling, which offers only probabilistic guarantees—a planner might miss a small obstacle between samples. C-IRIS shifts the paradigm toward a "positive approach": finding entire volumes of C-space and rigorously proving they are safe.

Methodology: The C-IRIS Algorithm

C-IRIS generates large convex polytopes in a rational parameterization of C-space. The algorithm relies on three key pillars:

  1. Tangent Configuration Space (TC-space): By using a stereographic projection ($t = \tan(\theta/2)$), the authors transform trigonometric kinematics into rational functions (polynomials), making them amenable to convex optimization solvers.
  2. Sums-of-Squares (SOS) Certification: The algorithm searches for a mathematical "certificate"—a proof that no point in a given C-space region results in a collision. This is achieved using the Positivstellensatz (Psatz) theorem, which converts the search into a convex optimization problem.
  3. Iterative Inflation: The process alternates between two steps:
  4. Certification: Finding hyperplanes in task space that separate the robot from obstacles for all configurations in the current region.
  5. Inflation: Expanding the C-space polytope until it hits the boundaries defined by the safety certificates.
Separating hyperplane theorem: If two convex bodies do not intersect, there exists a hyperplane that separates them.

Handling Diverse Geometries

A significant strength of C-IRIS is its ability to handle various collision geometries beyond simple boxes. By employing the Schur Complement, the method incorporates spheres, capsules, and cylinders into the semidefinite programming framework, allowing for more realistic robot and environment modeling.

Mathematical formulations for certifying separation and membership for various geometric bodies.

Key Insights and Results

The authors found that the complexity of the certification can be managed by optimizing the reference frame—expressing kinematics from the middle of a chain rather than the base. This reduces the degree of the polynomials involved, significantly speeding up the optimization process.

Scalability and Performance: * High-Dimensional Success: The method scales to 12-DOF bimanual systems, a major advancement for certified methods. * Rapid Inflation: For a 7-DOF KUKA arm, the algorithm increased a region's volume by 10,000x in just 11 iterations. * Precision: The regions are not overly conservative; they can safely bring a robot within 0.3mm of an obstacle while maintaining a rigorous proof of safety.

Conclusion and Future Work

C-IRIS provides a powerful tool for generating safe "corridors" for motion planning. While the computation is currently an offline process (taking up to 105 minutes for 12-DOF problems), the resulting regions enable extremely fast real-time planning. Future work includes improving seeding strategies to cover the entire C-space and addressing the limitations of stereographic projections for continuous 360-degree joints. The implementation is open-sourced in the Drake robotics toolbox.

Summary updated

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.
BibTeX
@article{dai2024certified,
  author = {Hongkai Dai and Alexandre Amice and Peter Werner and Annan Zhang and Russ Tedrake},
  title = {{Certified Polyhedral Decompositions of Collision-Free Configuration Space}},
  journal = {Int. J. Robotics Res.},
  year = {2024},
  doi = {10.1177/02783649231201437},
}
Figure