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

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

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.

Introduction

In robotics, the "Configuration Space" (C-space) represents the set of all possible joint positions for a robot. Motion planning involves finding a continuous path through this space that avoids "C-space obstacles"—regions where the robot would collide with itself or the environment. While obstacles like boxes or cylinders are geometrically simple in 3D task space, they transform into highly complex, non-linear manifolds when mapped into C-space due to the non-linear nature of robot kinematics.

Historically, motion planners have relied on point-wise sampling (e.g., RRTs or PRMs). These methods check if specific points are safe but cannot easily guarantee that the entire space between those points is collision-free. This lack of a "deterministic certificate" makes optimization-based planning difficult. This paper introduces C-IRIS (C-space Iterative Regional Inflation by Semidefinite programming), the first practical method to compute large, convex, and rigorously certified collision-free regions in C-space.

Methodology

Rational Parametrization

Standard robot kinematics rely on trigonometric functions (sines and cosines), which are notoriously difficult for convex optimization solvers. C-IRIS overcomes this by employing a "stereographic projection" reparameterization. By defining $s = \tan(q/2)$, where $q$ represents the joint angles, the transcendental kinematic equations are transformed into rational polynomial functions. This algebraic shift allows the problem to be framed within the domain of polynomial optimization.

Certified Separating Hyperplanes

To prove a region is collision-free, one must demonstrate that for every configuration within that region, there exists a plane separating the robot's geometry from the environment's obstacles. C-IRIS searches for a family of these separating hyperplanes. These planes are not static; they are functions of the robot's configuration, ensuring that a valid certificate of non-collision exists for every point inside the designated C-space polytope.

Sum-of-Squares (SOS) Programming

The core of the certification process is Sum-of-Squares (SOS) optimization. SOS is used to prove that a polynomial (representing the distance between the robot and obstacles) remains non-negative over a specific region. This provides the mathematical proof—the deterministic certificate—that the entire volume of the computed region is safe.

The C-IRIS Algorithm

The algorithm grows these safe regions through an alternating optimization process:

  1. Certification and Ellipse Search: Given a polytope, the algorithm searches for the largest possible inscribed ellipse while simultaneously finding the SOS certificates that prove the polytope is collision-free.
  2. Polytope Expansion: The algorithm then expands the boundaries of the polytope as far as possible away from the ellipse without violating the safety certificates established in the previous step.

This cycle repeats, allowing the safe region to "inflate" within the free C-space over several iterations.

Experimental Evaluation

The authors evaluated C-IRIS across three distinct robotic systems to test coverage, growth, and scalability:

  • 3-DOF Flipper System: This simplified model allowed for direct visualization of the C-space. The algorithm successfully filled approximately 90% of the available collision-free space using only 11 convex regions.
  • 7-DOF KUKA iiwa: In a realistic industrial setup involving a shelf, the algorithm demonstrated remarkable growth. A single region's volume increased by a factor of 10,000 over 11 iterations, illustrating the efficiency of the inflation process.
  • 12-DOF Bimanual System: To test high-dimensional scalability, the method was applied to a dual-arm KUKA iiwa setup. C-IRIS successfully generated certified regions that accounted for both environmental obstacles and self-collision. While the computational cost for the 12-DOF certificate reached 105 minutes, the resulting regions were extremely "tight," coming within 7.3mm of obstacles.

Discussion and Future Work

Key Insights

A critical finding was that the choice of coordinate frames significantly impacts performance. By placing the reference frame in the middle of a kinematic chain rather than at the base, the authors reduced the degree of the resulting polynomials, making the optimization more computationally tractable.

Limitations

The primary limitation is computational complexity; the time required for SOS programming grows significantly with the robot's degrees of freedom (DOF). Additionally, while the method is theoretically extensible to prismatic or cylindrical joints, it was primarily demonstrated on revolute joints. The current framework also assumes that task-space obstacles are convex or can be decomposed into convex components.

Conclusion

C-IRIS represents a significant step forward in motion planning by moving from probabilistic safety to deterministic certification. By transforming kinematic constraints into an algebraic optimization problem, it enables the creation of large, safe "corridors" in C-space that can be directly utilized by trajectory optimizers to ensure guaranteed collision-free movement in complex environments.

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