Logo
ScienceStack
Table of Contents
Fetching ...
Sign In

Certifying Bimanual RRT Motion Plans in a Second

Alexandre Amice, Peter Werner, Russ Tedrake

TL;DR

The paper tackles the challenge of certifying non-collision along dynamic, piecewise-polynomial motion plans in algebraic configuration spaces. It introduces a sums-of-squares ($SOS$) based certifier that allows time-varying separating hyperplanes to guarantee collision-free motion along a plan, leveraging univariate positivity certificates and algebraic forward kinematics in TC- or AC-space. The approach specializes to polynomial reparameterizations to achieve practical runtimes, demonstrated on a 12-DOF bimanual RRT and 7-DOF cubic plans, with certifications occurring in milliseconds to seconds and the ability to distinguish plans differing by millimeters. These results indicate that fully rigorous, collision-free certification can be integrated into real-world robotic planning workflows, providing provable safety guarantees for complex, high-DOF systems.

Abstract

We present an efficient method for certifying non-collision for piecewise-polynomial motion plans in algebraic reparametrizations of configuration space. Such motion plans include those generated by popular randomized methods including RRTs and PRMs, as well as those generated by many methods in trajectory optimization. Based on Sums-of-Squares optimization, our method provides exact, rigorous certificates of non-collision; it can never falsely claim that a motion plan containing collisions is collision-free. We demonstrate that our formulation is practical for real world deployment, certifying the safety of a twelve degree of freedom motion plan in just over a second. Moreover, the method is capable of discriminating the safety or lack thereof of two motion plans which differ by only millimeters.

Certifying Bimanual RRT Motion Plans in a Second

TL;DR

The paper tackles the challenge of certifying non-collision along dynamic, piecewise-polynomial motion plans in algebraic configuration spaces. It introduces a sums-of-squares () based certifier that allows time-varying separating hyperplanes to guarantee collision-free motion along a plan, leveraging univariate positivity certificates and algebraic forward kinematics in TC- or AC-space. The approach specializes to polynomial reparameterizations to achieve practical runtimes, demonstrated on a 12-DOF bimanual RRT and 7-DOF cubic plans, with certifications occurring in milliseconds to seconds and the ability to distinguish plans differing by millimeters. These results indicate that fully rigorous, collision-free certification can be integrated into real-world robotic planning workflows, providing provable safety guarantees for complex, high-DOF systems.

Abstract

We present an efficient method for certifying non-collision for piecewise-polynomial motion plans in algebraic reparametrizations of configuration space. Such motion plans include those generated by popular randomized methods including RRTs and PRMs, as well as those generated by many methods in trajectory optimization. Based on Sums-of-Squares optimization, our method provides exact, rigorous certificates of non-collision; it can never falsely claim that a motion plan containing collisions is collision-free. We demonstrate that our formulation is practical for real world deployment, certifying the safety of a twelve degree of freedom motion plan in just over a second. Moreover, the method is capable of discriminating the safety or lack thereof of two motion plans which differ by only millimeters.
Paper Structure (14 sections, 3 theorems, 15 equations, 5 figures)

This paper contains 14 sections, 3 theorems, 15 equations, 5 figures.

Table of Contents

  1. Introduction
  2. Contribution
  3. Notation
  4. Mathematical Preliminaries
  5. Separating Convex Bodies
  6. Polynomial Positivity on Intervals
  7. Algebraic Forward Kinematics
  8. Problem Formulation
  9. Proving Non-collision along a Plan
  10. Results
  11. Certifying an RRT for A Bimanual Manipulator
  12. Certifying Cubic Polynomial Plans for a 7-DOF iiwa
  13. Discussion and Conclusion
  14. Acknowledgements

Key Result

Theorem 1

Let $P(x) \in \mathbb{R}[x]_{2d}^{n \times n}$ be a symmetric polynomial matrix and let $y = $ be a vector of monomials. Define the scalar polynomial $p(x,y) = y^{T}P(x)y$. Then $P(x)$ is a SOS matrix if and only if $p(x,y)$ is SOS.

Figures (5)

  • Figure 1: A 7-DOF Kuka iiwa reaching into a pair of shelves. Despite differing by at most 20mm, the blue motion plan is collision-free, while the red motion plan contains minor collision when reaching into each shelf. The proposed certification method is capable of discriminating the safety of these two motion plans. A video describing the method is available at https://youtu.be/oTiDYeptKis
  • Figure 2: The closed, convex bodies $\mathcal{A}$ and $\mathcal{B}$ are collision-free if and only if there exists a hyperplane $a^{T}x + b = 0$ separating the two. As the bodies move in space, a different hyperplane may be needed to certify their non-collision.
  • Figure 3: The pendulum on rail system.
  • Figure 4: A pair of Kuka iiwa reaching from the straight up start configuration (translucent arms) into the shelf (opaque arms). The close confines of this motion plan make checking safety via finite sampling challenging. An animation of the motion plan is available https://alexandreamice.github.io/project/c-iris-path/14dof_hyperplanes.html
  • Figure 5: Timing statistics and aggregated statistics for certifying an RRT for the bimanual iiwa system from Figure \ref{['F: bimanul example']}. An instance of \ref{['E: cert by hyperplane poly']} is solved for all 246 collision pairs and all 105 edges in just over $30$ seconds.

Theorems & Definitions (17)

  • Example 1
  • Example 2
  • Definition 1
  • Definition 2
  • Theorem 1: blekherman2012semidefinite, Lemma 3.78
  • Theorem 2: Markov-Lucasz roh2006discrete
  • Theorem 3
  • proof
  • Remark 1
  • Definition 3
  • ...and 7 more