# Simulation-Guided Path Optimization for Resolving Interlocked Hook-Shaped Components
## 4 Our Approach  
### Problem Formulation  
Consider a robotic manipulator tasked with picking and placing hook-shaped objects. This task consists of multiple subtasks within a single cycle: 1. approaching and grasping the object, 2. separating and picking it up, 3. transporting it, and 4. placing it. Each subtask requires executing a certain trajectory. The most challenging part, and the focus of this paper, is the separation subtask, which requires disengaging interlocked components. While we still solve the full pick-and-place task, the remaining subtasks can be efficiently handled using industrial baseline methods or approaches, which are explained in more detail in Section 2. Therefore, the following sections focus on the separation process rather than detailing all stages of the pick-and-place task.  
Finding a trajectory to separate the components can be formulated as solving a non-linear mathematical program. The program minimizes a cost \(c_{k}\) along the path of length \(T\), represented by a stacked vector of joint configurations \(\mathbf{\tau}=[\mathbf{q}_{0},\mathbf{q}_{1},\dots,\mathbf{q}_{T}]^{\top}\), while satisfying the required constraints. The problem can be formulated as follows:  
\[\min_{\mathbf{\tau}} c(\mathbf{\tau}),\] (6a) s.t. \[\mathbf{g}(\mathbf{q}_{t})\leq\mathbf{0},\qquad\forall t\in[0,...,T], \tag{6b}\] \[||f(T)||\leq\sigma,\] (6c) \[\mathbf{\tau}(0)=\mathbf{q}_{\text{start}},\mathbf{\tau}(T)=\mathbf{q}_{\text{ goal}}. \tag{6d}\]  
The constraint Equation (6b) represents inequality constraints that ensure joint limits and collision avoidance along a given discrete path \(\mathbf{\tau}\) are satisfied. Equation (6c) enforces that the Euclidian norm of the forces of the end-effector at the last timestep of the separating maneuver must be lower than a threshold \(\sigma\), which corresponds to the maximum forces experienced when holding only one object. To ensure continuity between the separating motion and the paths of the other subtasks, the first and last joint configurations must match the grasp joint configuration, \(\mathbf{q}_{\text{start}}\), from the previous subtask and the starting joint configuration, \(\mathbf{q}_{\text{goal}}\), from the next one, Equation (6d).  
Figure 2: Example of the optimal basis function trajectory representation for \(\mathbf{q}\in\mathcal{R}^{2}\). The sampled joint-space waypoints (**a**) are combined with the boundary conditions to generate dense candidate trajectories (**b**). These dense trajectories start from the initial configuration (red point) with the desired joint velocity and terminate at the goal configuration (green point) with the specified joint velocity.  
Solving the mathematical program from Equations (6a)-(6d) requires explicitly designing differentiable constraints and modeling complex contact interactions between the robot and hook-shaped components, which is both challenging and computationally expensive. Instead, we utilize a parallelizable simulator to forward-simulate these interactions and optimize candidate trajectories, as described in the following sections.