# Simulation-Guided Path Optimization for Resolving Interlocked Hook-Shaped Components
## 1 Introduction  
Due to the increasing shortage of manual labor and consumer demand, industrial robots are widely used for pick-and-place applications. These applications rely on a control pipeline consisting of several sub-steps, including object detection, approaching, grasping, transporting, and placing the object in a precise location. While controllable and predictable industrial environments simplify task and motion planning to some extent, they also present challenges in terms of desired cycle time, perception, and scalability to accommodate various object shapes. In this paper, we address the motion-planning problem of picking up hook-shaped components, such as car brake caliper brackets, which are stored unsorted in large bags. Their shape causes the components to become interlocked, often resulting in multiple components being picked simultaneously and disrupting the grasp pose due to the additional weight, see Figure 1. Consequently, the task's success rate significantly decreases, the cycle time increases, and human intervention is required to manually separate these components.  
The industrial baseline approach for pick-and-place applications is based on a finite-state machine (FSM) framework, where a user defines a sequence of high-level states. FSM is responsible for transitioning between these states based on predefined conditions or sensor feedback. Each state represents a specific action, such as a robot movement or a command for the gripper or vision system. In its simplest yet most common form, movement is defined by a set of user-specified poses relative to the object, including thepre-grasp pose, grasp pose, and post-grasp pose. This approach works well in industrial environments with simple-shaped objects. However, when dealing with more complex tasks or objects, such as ours, additional states are often introduced in the finite-state machine to handle the increased complexity. Yet, this approach becomes tedious and often still fails to account for all possible variations that may arise. On the other hand, instead of relying on predefined relative Cartesian-space waypoints, advanced motion planners enhance the robot's capabilities and expand its range of motion [1,2]. Optimization-based planners allow for the incorporation of arbitrary aspects of behavior through constraints or objective functions. However, modeling the interlocking nature of the components studied in this paper remains a challenging task.  
We propose a framework which adapts the motion required to separate the interlocked components within a pick-and-place application. Instead of explicitly optimizing dense trajectory joint-space waypoints, we simplify the optimization problem by using sparse joint position waypoints \(\mathbf{q}_{1:N_{\text{via}}}\). To leverage sampling-based optimization while ensuring smoothness in the sampled candidate waypoints, we interpolate them using optimal basis functions [3,4], resulting in dense, continuous candidate trajectories. Moreover, sampling-based optimization eliminates the need to design a differentiable cost term for separating the components, as it does not require gradient information. This allows us to use a parallelizable physics simulator [5] to forward-simulate candidate trajectories and predict their effects on the components' state. The optimized motion is then used as a reference trajectory for a low-level controller.  
This paper provides the following contributions:  
* A motion-planning framework for optimizing trajectories to separate interlocked hook-shaped components in a pick-and-place task.
* Real-world experimental results demonstrating that the proposed framework improves the success rate of the separating subtask compared to the industrial baseline.