Global Contact-Rich Planning with Sparsity-Rich Semidefinite Relaxations

1School of Engineering and Applied Sciences, Harvard University 2School of Mathematical Sciences, University of Science and Technology of China

SPOT is a toolbox for exploiting sparsity in polynomial optimization for robotics and beyond.

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.

Real-world push-T: whole procedure. (Left) “Clean” push T: no mismatches and disturbances; (Right) “Dirty” push T: severe mismatches and disturbances.

For each replanning, we reset the arm to a fixed position to make sure the Apriltag can give accurate state estimation. In the following videos, we only show the procedure that the pusher is pushing the T-block — for simplicity.

Real-world push-T: "clean” case

Real-world push-T: “dirty” case

More contact-rich trajectories with certifiable optimality...
Animated GIF

Push Bot in simulation

Animated GIF

Push Box in simulation

Animated GIF

Push T-block in simulation

Animated GIF

Push Box with a Tunnel in simulation

Animated GIF

Planar Hand in simulation