Certified polyhedral decompositions of collision-free configuration space

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

TL;DR

This paper introduces C-IRIS, the first rigorous method to decompose a robot's configuration space into large, convex, and mathematically certified collision-free regions. By combining algebraic kinematics with Sums-of-Squares (SOS) optimization, the authors provide a way to "carve out" safe zones in high-dimensional spaces (up to 12-DOF) that are guaranteed to be free of obstacles.

What Problem Does This Paper Address?

In robotics, "Configuration Space" (C-space) is the set of all possible positions a robot can take. Planning a path means finding a line through C-space that doesn't hit an obstacle. However, while obstacles are easy to define in the physical world (task space), they become incredibly complex, non-convex "blobs" when mapped into C-space due to the non-linear nature of robot joints.

Most current methods rely on sampling: checking if a single point is in collision. While fast, sampling only gives probabilistic guarantees—you might miss a tiny obstacle between your samples. This paper tackles the "positive approach": instead of just checking points, can we find entire volumes of C-space and rigorously prove they are 100% safe?

Methods

The authors present C-IRIS (C-space Iterative Regional Inflation by Semidefinite programming). The core innovation lies in how they handle the math:

  1. Tangent Configuration Space (TC-space): Standard robot kinematics use sines and cosines, which are hard for optimization solvers. The authors use a "stereographic projection" ($t = \tan(\theta/2)$) to transform these into rational functions (polynomials divided by polynomials).
  2. Sums-of-Squares (SOS) Certification: To prove a region is safe, the algorithm looks for a "certificate"—a mathematical proof that no point in a given polytope can possibly result in a collision. They use the Positivstellensatz (Psatz), a powerful theorem from algebraic geometry, to turn this search into a convex optimization problem.
  3. The Iterative Loop: The algorithm alternates between two steps:
  4. Certification: Find a set of hyperplanes (planes in task space) that separate the robot from obstacles for every configuration in the current region.
  5. Inflation: Grow the region (a polytope in TC-space) to be as large as possible without violating the safety certificates.

Datasets

As a theoretical and algorithmic robotics paper, the authors evaluate their method on several simulated robot models rather than static datasets:

  • 2-DOF Toy Models: A pendulum on a rail and a "pinball flipper" used for visualization of the complex C-space obstacles.
  • 7-DOF KUKA iiwa: A standard industrial arm reaching into a shelf.
  • 6-DOF UR3e: An arm using cylindrical collision geometries.
  • 12-DOF Bimanual Manipulators: Two KUKA or UR3e arms working together, testing the scalability of the approach to high dimensions.

Key Insights and Ideas

  • Reference Frame Optimization: The degree of the polynomials involved depends on the length of the kinematic chain. The authors found that expressing the kinematics from a frame in the middle of the robot arm (rather than the base) significantly reduces the mathematical complexity and solve time.
  • Beyond Polytopes: While previous work focused on box-like obstacles, C-IRIS handles spheres, capsules, and cylinders by using the "Schur Complement" to turn distance constraints into semidefinite constraints.
  • Necessity and Sufficiency: The authors prove that their method is "complete"—if a safe region exists, there is a finite-degree polynomial certificate that can find it.

Key Results

  • Scalability: The method successfully generated certified regions for a 12-DOF bimanual system, a significant jump in complexity over prior "certified" methods.
  • Efficiency: For a 7-DOF KUKA arm, the algorithm could grow a certified region's volume by a factor of 10,000x in just 11 iterations.
  • Tightness: The algorithm isn't overly conservative; it can find safe regions that bring the robot within 0.3mm of an obstacle while still maintaining a rigorous proof of safety.
  • Open Source: The implementation is integrated into Drake, a widely-used robotics toolbox, making it accessible to the broader research community.

Limitations and Future Work

  • Offline Computation: While the resulting regions are extremely fast to use for real-time planning, generating them is an "offline" process. Solving the largest 12-DOF problems can take between 35 and 105 minutes.
  • Joint Limits: The stereographic projection used ($t = \tan(\theta/2)$) cannot represent a full 360-degree continuous rotation in a single coordinate (it goes to infinity at $\pm\pi$). Robots with continuous joints must have their C-space split into segments.
  • Seeding: The algorithm needs a starting "seed" point. While the paper discusses a seeding strategy, finding the optimal set of seeds to cover the entire C-space with the fewest number of regions remains an open challenge.

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

Loading related content...

Figure