Simultaneous Contact Selection and Planning for Contact-Rich Manipulation with Cascaded Optimization
Simultaneous Contact Selection and Planning for Contact-Rich Manipulation with Cascaded Optimization
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 robot and object target state and contact location sequence. However, most existing approaches lack the ability to autonomously reason and generatively produce 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.
Nonprehensile Benchmark I: Fingertip Experiments
Elephant
Bunny
Brick
Piggy Bank
Nonprehensile Benchmark II: Robot Experiments
CF-MPC
Sampling
DyWA
SCSP
CF-MPC
Sampling
DyWA
SCSP
Nonprehensile Benchmark III: Real-World Experiments
Rotating bunny
Trial 1
Trial 2
Trial 3
Flipping hand
Trial 1
Trial 2
Trial 3
Flipping bunny
Trial 1
Trial 2
Trial 3
Flip controller
Trial 1
Trial 2
Trial 3
Evaluation on different surfaces (with different friction)
Smooth
Rough
Consecutive pushing under disturbances
Generalization I: Bi-Manual & Multi-finger Grasping
Bi-Manual grasping
Grasping objects with different scales
Multi-finger dexterous grasping
Grasping objects with different scales
Generalization II: Tilted-Pushing Tasks
Pushing bunny on 15° slope
Pushing teapot on 15° slope
Generalization III: Soccer Dribbling Tasks
Soccer dribbling task. The task is to kick the soccer to the target (translucent). Red and green cubes show footworkspaces, and the yellow point indicates CSO-planned contact locations.
Generalization IV: Long-Horizon Tasks
Opening drawer
Opening microwave
Generalization V: Producing Augmented Data for VLA
Auto-generation of trajectories with different contact locations for "CloseDrawer" task in RoboCasa.
Auto-generation of trajectories with different contact locations for "CloseOvenDoor" task.