Simultaneous Contact Selection and Planning for Contact-Rich Manipulation with Cascaded Optimization

2026-05-27

AI Summary

Existing approaches struggle to autonomously generate diverse sequences of contact locations and manipulation trajectories, limiting their ability to perform complex, contact-intensive tasks. This work proposes SCSP, a cascaded optimization framework that unifies contact location selection and trajectory planning within a single online-executable optimization pipeline. By integrating a surrogate contact model, mixed discrete-continuous optimization, and prior-guided real-time trajectory generation, SCSP effectively addresses the challenges posed by complementarity in contact dynamics and sparse gradients. Implemented on redundant robotic arms, the method enables joint online synthesis of contact points and motion trajectories. Extensive simulations and real-world experiments demonstrate its capability to produce diverse manipulation behaviors, exhibit robustness against dynamic modeling errors and perception noise, and generalize effectively across complex contact-rich tasks.

Abstract

We propose an optimization-based framework for robust contact-rich manipulation. Recent contact-implicit methods enable online hybrid planning across contact modes, allowing closed-loop manipulation for a given target state and contact location sequence of the robot and object. However, most existing approaches lack the ability to autonomously reason and generate diverse contact location sequences and manipulation trajectories, i.e., active contact location selection, which limits their applicability to relatively simple tasks. Active contact location selection is challenging due to complementarity in contact dynamics and the sparse gradients, making the design of a unified framework for contact selection and planning difficult. To address these challenges, we introduce Simultaneous Contact Selection and Planning (SCSP), a cascaded optimization framework comprising Contact Selection Optimization (CSO) and Contact Planning Optimization (CPO). CSO leverages a surrogate contact model and discrete-continuous optimization to efficiently resolve the nonsmoothness and coupling in contact selection, enabling online global searching of optimal contact locations. CPO performs prior-guided contact planning by evaluating the reference contact locations produced by CSO and generating corresponding manipulation trajectories in real time for redundant manipulators. Extensive simulations and real-world experiments demonstrate that SCSP produces diverse manipulation behaviors and robust control under inaccurate dynamics and perceptual noise. We further validate the generalization of the framework on challenging manipulation tasks. Project website: \href{https://sites.google.com/view/scsp-robot}{https://sites.google.com/view/scsp-robot}.

Problem

• contact-rich manipulation
• contact selection
• complementarity constraints
• sparse gradients
• hybrid planning

Innovation

• contact-rich manipulation
• contact selection
• cascaded optimization
• contact-implicit planning
• discrete-continuous optimization