Finding and Optimizing Certified, Collision-Free Regions in Configuration Space for Robot Manipulators

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

TL;DR

This paper introduces C-IRIS, the first practical method to compute large, convex, and rigorously certified collision-free regions in a robot's configuration space (C-space). By leveraging a clever mathematical reparameterization and Sum-of-Squares (SOS) optimization, the authors move beyond point-wise collision checking to create "safe zones" that are guaranteed to be free of obstacles, even in high-dimensional systems like 12-DOF bimanual manipulators.

What Problem Does This Paper Address?

In robotics, "Configuration Space" (C-space) is the mathematical space representing all possible positions of a robot's joints. Planning a path means finding a line through C-space that doesn't hit any "C-space obstacles."

The challenge is that while obstacles are often simple shapes (like boxes or cylinders) in the 3D physical world, they become incredibly complex, non-linear "blobs" when mapped into C-space due to the robot's joint geometry. Most current planners rely on sampling: they check if a specific point is safe, but they can't easily prove that the space between two safe points is also safe. This lack of a "certificate" makes optimization-based motion planning difficult and computationally expensive.

Methods

The authors present C-IRIS (C-space Iterative Regional Inflation by Semidefinite programming). The core technical workflow involves:

  1. Rational Parametrization: Standard robot kinematics use sines and cosines, which are hard for optimization solvers. The authors use a "stereographic projection" ($s = \tan(q/2)$) to turn these trigonometric functions into polynomials.
  2. Certified Separating Hyperplanes: To prove two objects aren't touching, you find a "separating plane" between them. C-IRIS searches for a family of these planes that change as the robot moves, ensuring that for every configuration within a specific region, a valid separating plane exists.
  3. Sum-of-Squares (SOS) Programming: This is a powerful optimization technique used to prove that a polynomial is always non-negative over a certain area. The authors use SOS to provide the "deterministic certificate" that a region is collision-free.
  4. Alternating Optimization: The algorithm grows these regions by alternating between two steps:
  5. Finding the largest possible ellipse inside a safe region and certifying it.
  6. Expanding the boundaries of the region (the polytope) as far as possible without breaking the safety certificate.

Datasets

The paper does not use traditional machine learning datasets. Instead, it evaluates the algorithm on three increasingly complex robotic simulation environments:

  • 3-DOF Flipper System: A simplified setup used to visualize the C-space and demonstrate that the algorithm can fill ~90% of the free space.
  • 7-DOF KUKA iiwa: A standard industrial arm placed near a shelf to test realistic manipulation constraints.
  • 12-DOF Bimanual System: Two KUKA iiwa arms working together, testing the algorithm's ability to scale to high-dimensional spaces and handle self-collision.

Key Insights and Ideas

  • Algebraic Kinematics is the Key: By switching from joint angles ($q$) to the tangent of half-angles ($s$), the authors transformed a transcendental geometry problem into an algebraic one that convex optimization can solve.
  • Reference Frame Matters: The authors found that choosing a coordinate frame in the middle of a kinematic chain (rather than at the base) significantly reduces the degree of the polynomials involved, making the optimization much faster.
  • Deterministic vs. Probabilistic: Unlike RRTs or PRMs which are "probabilistically certain" (they might find a path if you wait long enough), C-IRIS provides a "deterministic certificate"—a mathematical proof that the entire region is safe.

Key Results

  • Coverage: In the 3-DOF example, the algorithm successfully filled 90% of the available collision-free C-space using just 11 regions.
  • Growth: For the 7-DOF arm, a single region's volume increased by a factor of 10,000 over 11 iterations.
  • Scalability: The method successfully generated certified regions for a 12-DOF system. While the "heavy lifting" (the largest SOS program) took about 105 minutes for the 12-DOF case, the resulting regions are extremely "tight" (within 7.3mm of obstacles), maximizing the usable space for the robot.

Limitations and Future Work

  • Computational Cost: As the Degrees of Freedom (DOF) increase, the time required to solve the SOS programs grows significantly. 105 minutes for a 12-DOF certificate is "offline" territory.
  • Joint Types: The current formulation focuses on revolute joints. While the authors suggest it can extend to other "algebraic joints" (prismatic, cylindrical), those were not explicitly demonstrated.
  • Obstacle Geometry: The method assumes obstacles in the task space are convex (or can be decomposed into convex parts).
  • Future Directions: The authors intend to explore non-linear optimization to "propose" regions more quickly before certifying them and to integrate these regions more deeply into global trajectory optimizers.

Summary updated

Abstract

Configuration space (C-space) has played a central role in collision-free motion planning, particularly for robot manipulators. While it is possible to check for collisions at a point using standard algorithms, to date no practical method exists for computing collision-free C-space regions with rigorous certificates due to the complexities of mapping task-space obstacles through the kinematics. In this work, we present the first to our knowledge method for generating such regions and certificates through convex optimization. Our method, called C-Iris (C-space Iterative Regional Inflation by Semidefinite programming), generates large, convex polytopes in a rational parametrization of the configuration space which are guaranteed to be collision-free. Such regions have been shown to be useful for both optimization-based and randomized motion planning. Our regions are generated by alternating between two convex optimization problems: (1) a simultaneous search for a maximal-volume ellipse inscribed in a given polytope and a certificate that the polytope is collision-free and (2) a maximal expansion of the polytope away from the ellipse which does not violate the certificate. The volume of the ellipse and size of the polytope are allowed to grow over several iterations while being collision-free by construction. Our method works in arbitrary dimensions, only makes assumptions about the convexity of the obstacles in the task space, and scales to realistic problems in manipulation. We demonstrate our algorithm’s ability to fill a non-trivial amount of collision-free C-space in a 3-DOF example where the C-space can be visualized, as well as the scalability of our algorithm on a 7-DOF KUKA iiwa and a 12-DOF bimanual manipulator.
BibTeX
@inproceedings{amice2022finding,
  author = {Alexandre Amice and Hongkai Dai and Peter Werner and Annan Zhang and Russ Tedrake},
  title = {{Finding and Optimizing Certified, Collision-Free Regions in Configuration Space for Robot Manipulators}},
  booktitle = {WAFR},
  year = {2022},
  url = {https://openreview.net/forum?id=qrogYs6BI0x},
}

Loading related content...

Figure