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

TL;DR

This work proposes 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.

Abstract

Planning over discontinuous dynamics is needed for robotics tasks like contact-rich manipulation, which presents challenges in the numerical stability and speed of planning methods when either neural network or analytical models are used. On the one hand, sampling-based planners require higher sample complexity in high-dimensional problems and cannot describe safety constraints such as force limits. On the other hand, gradient-based solvers can suffer from local optima and convergence issues when the Hessian is poorly conditioned. We propose a planning method with both sampling- and gradient-based elements, using the Cross-entropy Method to initialize a gradient-based solver, providing better search over local minima and the ability to handle explicit constraints. We show the approach allows smooth, stable contact-rich planning for an impedance-controlled robot making contact with a stiff environment, benchmarking against gradient-only MPC and CEM.

Figures (8)

• Figure 1: Experimental setup showing a robot making contact with a table, moving towards a hinge contact condition, providing a task with three modes: free space, vertical stiff contact, and hinge contact. The soft contacts are used for validating the effect of contact stiffness on performance.
• Figure 2: Block diagram of the proposed approach
• Figure 3: Performance on the MuJoCo environments with planning horizon $h=20$. .
• Figure 4: Comparing state and control trajectory from CEM vs MPC for stiff dynamics, the x-, y-, and z-positions shown in red, green, blue, respectively.
• Figure 5: Effect of CEM warmstart iterations on total solve time and trajectory cost for proposed approach on trajectory tracking problem. The CEM does not significantly change performance, but can improve solve time.