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

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.

• The paper introduces a hybrid method that combines CEM for initial guesses and gradient-based solvers for explicit constraint handling in contact-rich tasks.
• Simulation results in the MuJoCo environment show improved solve times and reduced performance variance compared to traditional methods.
• Real-world experiments validate robust performance in handling discontinuous dynamics and varying stiffness during robotic manipulation.

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

This essay explores a hybrid planning methodology integrating both sampling- and gradient-based approaches for contact-rich robotic manipulation tasks. The proposed method is evaluated through rigorous simulations and real-world experiments to address challenges associated with discontinuous dynamics and state constraints in manipulating environments.

Problem Statement and Proposed Methodology

Contact-rich manipulation tasks, such as door opening, present significant challenges due to discontinuous environmental dynamics and constraints like force limits. Traditional planning methods—sampling-based planners like CEM and gradient-based solvers—each have limitations. Sampling-based methods face sample complexity issues in high-dimensional spaces and struggle with state constraints, while gradient-based methods may suffer from convergence to local optima and poor performance on non-smooth problems.

The paper introduces a planning approach that combines these two paradigms. It utilizes the Cross-Entropy Method (CEM) to provide initial guesses for a gradient-based solver. This initialization helps overcome the sensitivity of gradient-based methods to initial conditions, allowing explicit handling of state constraints. The proposal includes integrating a particle filter for online estimation of contact modes to improve contact handling.

The implementation of the proposed approach in simulation environments demonstrates its effectiveness in improving robustness and solve time under various contact conditions and planning horizons.

Simulation Experiments and Performance Metrics

The performance of the hybrid method is extensively evaluated in the MuJoCo simulation environment, demonstrating better effectiveness than standalone planning methods in contact-rich tasks. The following figures depict relevant findings from the simulations:

The block diagram of the proposed approach outlines the integration of CEM and gradient-based solvers, showcasing the hybrid architecture.

Figure 1: Block diagram of the proposed approach showing the integration of CEM with gradient-based solvers.

Performance evaluations in MuJoCo with a planning horizon $h=20$ highlight how the proposed method competes favorably with traditional methods like CEM and MPC alone, particularly in tasks where discontinuous dynamics play a critical role.

Figure 2: Performance on the MuJoCo environments with planning horizon $h=20$.

In tasks marked by stiff dynamics, comparisons between state and control trajectories from CEM versus MPC reveal that the proposed approach maintains lower variance in performance and faster convergence, even under challenging conditions.

Figure 3: Comparing state and control trajectory from CEM vs MPC for stiff dynamics, the x-, y-, and z-positions shown in red, green, blue, respectively.

Real-world Validation and Analysis

To further affirm the efficacy of the approach, real-world experiments were conducted on a robot executing defined contact-rich tasks, such as transitioning between free space and contact with environments of varying stiffness. The effect of stiffness on contact detection was evident, with quicker adaptation in stiffer environments due to the particle filter's efficiency.

Figure 4: Effect of stiffness on contact detection highlights the sensitivity of mode detection to environment stiffness.

The method's real-world applicability is further confirmed in a pivot task, where the CEM+MPC approach outperforms a simple MPC method by reducing discontinuities observed in the impedance rest position during contact transitions.

Figure 5: Comparison between the simple MPC and CEM+MPC approaches for the pivot task.

Conclusion

The combination of the Cross-Entropy Method with a gradient-based planner offers a robust framework for handling contact-rich manipulation tasks with discontinuous dynamics. The hybrid approach benefits from the strengths of both sampling- and gradient-based methods, enabling better initialization, explicit constraint handling, and improved solve times in environments requiring adaptive contact management. Future research directions may explore further optimization of sampling and gradient steps and apply the framework to a broader set of robotic manipulation challenges.