Simulation-Guided Path Optimization for Resolving Interlocked Hook-Shaped Components

Manipulators performing pick-and-place tasks with objects of complex shapes must consider not only how to grasp the objects but also how to maneuver them out of a bin. In this paper, we explore the industrial challenge of picking hook-shaped components, whose interlocking nature often leads to failed attempts at safely retrieving a single component at a time. Rather than explicitly modeling contact-rich interactions within optimization-based motion planners, we tackle this challenge by leveraging recent advances in sampling-based optimization and parallelizable physics simulators to predict the impact of motion on the separating subgoal, aimed at resolving interlocking. The proposed framework generates candidate trajectories initialized from a user-provided demonstration, which are then simulated and evaluated in a physics simulator to optimize robot trajectories in joint space while considering the entire planning horizon. We validate our approach through real-world experiments on a manipulator, demonstrating improved success rates in terms of separating interlocked objects compared to the industrial baseline.