Combining Sampling- and Gradient-based Planning for Contact-rich Manipulation

Planning for contact-rich manipulation involves discontinuous dynamics, which presents challenges to planning methods. Sampling-based planners have higher sample complexity in high-dimensional problems and cannot efficiently handle state constraints such as force limits. Gradient-based solvers can suffer from local optima and their convergence rate is often worse on non-smooth problems. We propose a planning method that is both sampling- and gradient-based, using the Cross-entropy Method to initialize a gradient-based solver, providing better initialization to the gradient-based method and allowing explicit handling of state constraints. The sampling-based planner also allows direct integration of a particle filter, which is here used for online contact mode estimation. The approach is shown to improve performance in MuJoCo environments and the effects of problem stiffness and planing horizon are investigated. The estimator and planner are then applied to an impedance-controlled robot, showing a reduction in solve time in contact transitions to only gradient-based.