# Simulation-Guided Path Optimization for Resolving Interlocked Hook-Shaped Components
## 3 Preliminaries
### Path Integral Optimization  
The path integral framework [16] has its roots in quantum mechanics, and it elegantly describes particle motion as a sum of all possible trajectories a particle could take in the given space and time. Each path contributes to the particle's behavior with a certain amplitude. The same idea has been applied to solve stochastic optimal control problems, resulting in one of the state-of-the-art control algorithms known as Model Predictive Path Integral (MPPI), as well as in the motion-planning domain [2; 21]. This approach, also known as particle-based optimization [21], can be efficiently parallelized on modern GPUs, leading to faster computation times. In the context of planning, a trajectory is represented as a distribution \(\mathbf{\pi}_{\mathbf{\phi}_{j}}\) represented as a set of time-indexed distributions \(\pi_{\mathbf{\phi}_{j}}\) for each timestep \(t\) along the time horizon \(T\), such that \(\mathbf{\pi}_{\mathbf{\phi}_{j}}=\prod_{t=0}^{T}\pi_{\mathbf{\phi}_{j},t}\), where \(j\) denotes the version of the policy after \(j\)-th optimization iteration. Each distribution is parameterized by \(\mathbf{\phi}_{j,t}\), modeled as a Gaussian with a mean \(\mathbf{\mu}_{j,t}\) and covariance matrix \(\mathbf{\Sigma}_{j,t}\). The full set of trajectory parameters at iteration \(j\) is thus \(\mathbf{\phi}_{j}=\{\mathbf{\mu}_{j,0:T},\mathbf{\Sigma}_{j,0:T}\}\). In this paper, each distribution \(\mathbf{\phi}_{j,t}\) corresponds to a waypoint in the joint space at timestep \(t\). The mean \(\mathbf{\mu}_{j,t}\) defines the nominal joint configuration, and the covariance matrix \(\mathbf{\Sigma}_{j,t}\) captures uncertainty or exploration around that configuration. To ensure the trajectory begins and ends at fixed configurations, the first and last distributions are defined with zero variance and are centered at the initial and goal joint configurations, respectively. As a result, any sample drawn from these boundary distributions always yields the exact initial or goal joint configuration. In the most general form, at each optimization iteration \(j\), the algorithm samples \(N_{\text{batch}}\) random disturbance vectors \(\delta_{\mathbf{\mu}_{j}}\sim\mathcal{N}(\mathbf{0},\mathbf{1})\), which are added to the current version of the policy to generate candidate actions as follows:  
\[\mathbf{\mu}_{\delta_{j,t}}=\mathbf{\mu}_{j}+\delta_{\mathbf{\mu}_{j},t},\quad\forall i\in[ 0,N_{\text{batch}}], \tag{1}\]  
which are used for rollouts with the system's dynamical model to produce a corresponding batch of states \(\mathbf{\hat{x}}_{i\in[0,N_{\text{batch}}],t\in[0,T]}\). The exact structure of the state \(\mathbf{\hat{x}}\) mainly depends on the task-related cost terms, so it may include joint poses and velocities, end-effector poses, or even the state of objects that the robot interacts with. The candidate actions \(\mathbf{\mu}_{\delta_{j,t}}\) represent control inputs to the dynamical model, such as joint accelerations, velocities, or, in our case, joint angles. In implementations that also update the covariance matrices, the disturbance vector \(\delta_{\mathbf{\mu}_{j}}\) is sampled only once at the beginning and then scaled using the current covariance matrices \(\mathbf{\Sigma}_{j;0:T}\) at each optimization iteration. In Section 4.3.1, we detail the specific method used to scale the disturbance vector used in our implementation. Each rollout is evaluated using task-related cost terms that encourage the desired behavior of the robot, and the total cost \(S_{i\in[0,N_{\text{batch}}]}\) of the \(i\)-th rollout consists of the weighted sum of these different cost terms. To update the policy, the importance sampling weights \(\mathbf{w}\) are computed as follows:  
\[w_{i}=\exp(-\frac{1}{\beta}(S_{i}-\min(S)), \tag{2}\]  
with the temperature \(\beta\) parameter, set to 1 in our implementation, and the cost \(S_{i}\) of the \(i\)-th rollout that is scaled with the minimum cost from the current batch.