# Simulation-Guided Path Optimization for Resolving Interlocked Hook-Shaped Components
## 4 Our Approach
### Framework Pipeline  
Our approach follows the standard structure of industrial pick-and-place applications, where the finite-state machine dictates the actions at each state of the high-level task. In general, whenever the robot needs to move, either the desired end-effector pose is directly set in the robot's controller, or a motion planner generates a trajectory, which is passed as waypoints. In this section, we focus on how our framework addresses the separating subtask, which is the primary focus of this paper. However, it is important to note that the same framework is uniformly applied to all subtasks to ensure consistency. Even though Algorithm 1 summarizes only the procedure for separation, the same pseudocode applies to the remaining subtasks, with separation-specific cost terms omitted. For these other subtasks, detected objects are manually repositioned outside the robot's workspace to effectively remove them from the scene, as detailed in Section 4.2. To improve readability, we omit explicit indices for individual rollouts within the batch, as these can be efficiently handled using tensor operations. At the beginning of the pick-and-place cycle, the robot starts in its home configuration, allowing the vision system to take an unobstructed scan of the current state of the components. This scan provides the set of object poses \(\mathcal{T}_{\text{obj}}\) with respect to the world frame, as shown in Figure 5. The deterministic nature of the industrial environment simplifies the task, ensuring that this set of object poses remains unchanged even after executing the initial approach and grasping motion. The set is sorted based on the \(z\)-coordinate, ensuring that objects at the top of the stack are prioritized for picking. The pose of the highest object is processed through the inverse kinematics solver [23], which also penalizes deviations from the home configuration, to obtain the target joint configuration \(\mathbf{q}_{T}\). The user-provided demonstration, shown in Figure 4, serves as the initial guess for the mean \(\mathbf{\mu}_{\text{via}_{0}}\) representing the initial stacked vector of joint configurations \(\mathbf{\tau}_{\text{via}}\) (Line 1). After sampling the normalized noise, we perform \(J\) optimization iterations. At the beginning of each iteration, we reset the simulation based on the current state of the robot and objects. However, the object of interest is ignored, as it is considered part of the robot's kinematic chain (Section 4.2). Thereafter, the sampled noise is scaled by the current version of the covariance matrix to enforce smooth behavior, or, more precisely, by its Cholesky decomposition \(L_{j}\) and then added to the current version of the mean (Line 5). Thereafter, the sparse candidate paths \(\mathbf{\mu}_{\delta}\) are interpolated into dense candidate paths using optimal basis functions (Line 6), as described in Section 3.3. These dense candidates \(\mathbf{\tau}_{\text{eval}}\) are then used as reference joint position trajectories for parallel simulations in Isaac Gym, where each trajectory is applied in full to assess feasibility and predict the object behavior resulting from the robot's motion. We compute the costs and weights based on the simulated data and apply the same policy update (Lines 7-9) as in path integral methods, described in Equations (3) and (4). After \(J\) optimization iterations, we interpolate the optimized mean to obtain the reference vector of joint configurations \(\tau_{\text{ref}}\), which is then transformed into the sequence of task-space poses \(\Xi_{\text{ee}}\) through the forward kinematics function \(\phi_{\text{ee}}\) and passed to a reference tracker that determines the local end-effector goal pose \(\zeta_{\text{ee}}\) for the low-level impedance controller [24] (Lines 10-12).