Global Contact-Rich Planning with Sparsity-Rich Semidefinite Relaxations

Abstract: We show that contact-rich motion planning is also sparsity-rich when viewed as polynomial optimization (POP). We can exploit not only the correlative and term sparsity patterns that are general to all POPs, but also specialized sparsity patterns from the robot kinematic structure and the separability of contact modes. Such sparsity enables the design of high-order but sparse semidefinite programming (SDPs) relaxations--building upon Lasserre's moment and sums of squares hierarchy--that (i) can be solved in seconds by off-the-shelf SDP solvers, and (ii) compute near globally optimal solutions to the nonconvex contact-rich planning problems with small certified suboptimality. Through extensive experiments both in simulation (Push Bot, Push Box, Push Box with Obstacles, and Planar Hand) and real world (Push T), we demonstrate the power of using convex SDP relaxations to generate global contact-rich motion plans. As a contribution of independent interest, we release the Sparse Polynomial Optimization Toolbox (SPOT)--implemented in C++ with interfaces to both Python and Matlab--that automates sparsity exploitation for robotics and beyond.

• The paper transforms contact-rich planning into sparse polynomial optimization problems solvable via efficient semidefinite relaxations.
• A novel Sparse Polynomial Optimization Toolbox (Spot) is introduced to exploit sparsity and achieve near-global optimal solutions with certified suboptimality gaps.
• The proposed methodology is validated through extensive simulations and real-world experiments, demonstrating its efficiency in solving complex planning problems to near-global optimality.

Overview of Global Contact-Rich Planning with Sparsity-Rich Semidefinite Relaxations

This paper presents a sophisticated approach to the challenge of contact-rich motion planning in robotics through the innovative use of sparsity-rich semidefinite relaxations. It rigorously demonstrates how contact-rich planning problems transform into polynomial optimization problems (POPs) exhibiting significant sparsity that can be exploited to enhance computational efficiency and solution accuracy.

Core Contributions

This work leverages two types of sparsity: correlative sparsity and term sparsity, both particularly valuable in the context of semidefinite programming (SDP) relaxations. The authors introduce a novel Sparse Polynomial Optimization Toolbox (Spot), providing a means to exploit these sparsity patterns effectively, leading to substantial improvements over general-purpose SDP solvers.

1. Exploitation of Sparsity Patterns: The paper details the identification and utilization of correlative and term sparsity patterns found within POPs arising from the kinematic structures of robots and separability of contact modes. By doing so, it is possible to derive high-order but computationally efficient SDP relaxations.
2. Robust SDP Implementations: The Spot toolbox is designed to handle the inherently complex nature of contact-rich environments in robotics, automating the process of identifying sparsity patterns and applying them to yield near globally optimal solutions with certified small suboptimality gaps.
3. Extensive Evaluation: Simulation and real-world experiments validate the proposed methodology. The performance is assessed in tasks such as Push Bot, Push Box, and Planar Hand, revealing that the approach can solve complex planning problems to near-global optimality efficiently.
4. Minimizer Extraction: The research enhances the robustness of solution extraction from SDP relaxations using the Gelfand-Naimark-Segal (GNS) construction, ensuring reliable plan execution even when relaxation tightness varies.

Implications and Future Directions

The implications of this research are profound for both theoretical development and practical applications in robotics. By reformulating complex contact planning as POPs and effectively addressing the nonconvex nature of resulting optimization problems, the research advances the capabilities of automated planning systems. Additionally, the release of the Spot toolbox paves the way for broader application across various robotics contexts, potentially improving the efficiency and reliability of automated robotic systems in unstructured environments.

Future developments could focus on enhancing the scalability of this approach to accommodate even more intricate robotic tasks, such as those involving higher-dimensional state spaces or more complex contact geometries. Moreover, integrating machine learning techniques for better initial guesses of feasible solutions could further optimize the planning process.

In sum, this research represents a crucial step forward in leveraging computational techniques to solve enduring challenges in robotic motion planning, providing a robust framework for further innovation in the field.