# Simulation-Guided Path Optimization for Resolving Interlocked Hook-Shaped Components
## 4 Our Approach
### Sampling-Based Optimization
#### 4.3.3 Cost Terms  
The sparse candidate paths, described in Section 4.3.1, are interpolated using the aforementioned spline representation, described in Section 3.3, yielding dense candidate trajectories consisting of \(N_{\text{eval}}\) joint configurations that satisfy the boundary conditions. These dense trajectories are then used as reference joint configurations for robots in parallelized simulations. Since the reference configurations may violate joint limits, be obstructed by obstacles, or, in general, result in collisions with the environment, making them impossible to fully execute, we instead rely on the actual trajectories and the data provided by the simulator in the following cost terms. Each cost term is computed for the \(i\)-th rollout being evaluated.  
Instead of explicitly modeling a cost term to enforce separation between the components of Equation (6c), we simply penalize the Euclidean norm of the contact forces \(\mathbf{f}_{\text{ee}}(t)\) of the end-effector, as provided by the physics simulator, at every timestep \(t\) of candidate trajectories, as follows:  
\[\hat{c}_{\text{sep}}(t)=||\mathbf{f}_{\text{ee}}(t)||_{2}. \tag{8}\]  
Since the end-effector frame is extended by the grasped object, as described in Section 4.2, the physics simulator is used to determine whether any additional components remain attached. Consequently, the same cost term also penalizes any contact between the end-effector and the environment, effectively serving as a collision avoidance constraint between the end-effector frame and the environment.  
To avoid position joint limits, we use a discontinuous cost term that penalizes joint limit violations:  
\[\hat{c}_{\text{lim}}(\mathbf{q})=\begin{cases}1+\mathbf{q}-\mathbf{q}_{\text{max}},&\text {if }\mathbf{q}\geq\mathbf{q}_{\text{max}}\\ 1+\mathbf{q}_{\text{min}}-\mathbf{q},&\text{if }\mathbf{q}\leq\mathbf{q}_{\text{min}}\\ 0&\text{otherwise}.\end{cases} \tag{9}\]  
Figure 4: The provided demonstration establishes pressing the components against the table as the initial strategy for separating the objects. The green line shows the end-effector’s path, with the labeled Cartesian-space waypoints recomputed via inverse kinematics into joint-space configurations for the initial guess. First, the robot lifts the component to a higher position (WP1), then moves downward (WP2) to press the additional components against the table. As the robot moves back, the additional components fall back onto the table (WP3–WP4).  
To further optimize movement efficiency, we penalize the sum of squared joint accelerations in the same way as in covariance matrix initialization:  
\[\delta_{\text{acc}}(\mathbf{\tau}_{\text{eval}})=\mathbf{\tau}_{\text{eval}}^{\top}\mathbf{R }\mathbf{\tau}_{\text{eval}}, \tag{10}\]  
where \(\mathbf{\tau}_{\text{eval}}\) denotes the stacked vector of joint configurations of a given candidate trajectory.  
The total cost \(S_{i}\) for the \(i\)-th interpolated dense candidate trajectory is  
\[S_{i}=\alpha_{\text{acc}}\hat{c}_{\text{acc}}(\mathbf{\tau}_{\text{eval}_{i}})+ \alpha_{\text{lim}}\sum_{t=1}^{N_{\text{kin}}}\hat{c}_{\text{lim}}(\mathbf{q}_{i,t} )+\alpha_{\text{sep}}\sum_{t=1}^{N_{\text{kin}}}\gamma^{t}\hat{c}_{\text{sep}} (t_{i}), \tag{11}\]  
where \(\alpha_{\text{sep}},\alpha_{\text{smooth}}\), and \(\alpha_{\text{lim}}\) are weight parameters used to prioritize different aspects of the behavior. We use a discounted cost for the \(\hat{c}_{\text{sep}}\) term, as waypoints at the beginning of the maneuver have a greater influence on achieving the separating subgoal. The discount factor \(\gamma\) was set to 0.95.