SlowGuess/NeurIPS_2023_OCR huggingface dataset:
Extension to optimizing batch of different trajectory horizons. Currently, for vectorizing the update of all waypoints across the batch of trajectories, we flatten the batch and horizon dimensions and apply the Sinkhorn Step. After optimization, we reshape the tensor to the original shape. Notice that what glues the waypoints in the same trajectory together after optimization is the log of the Gaussian Process as the model cost, which promotes smoothness and model consistency. Given this pretext, in case of a batch of different horizon trajectories, we address this case by setting maximum horizon  $T_{\mathrm{max}}$  and padding with zeros for those trajectories having  $T < T_{\mathrm{max}}$ . Then, we also set zeros for all rows corresponding to these padded points in the cost matrix  $\mathbf{C}^{T_{\mathrm{max}} \times m}$ . The padded points are ignored after the barycentric projection. Another way is to maintain an index list of start and end indices of trajectories after flattening, then the cost computation also depends on this index list. Finally, the trajectories with different horizons can be extracted based on the index list. Intuitively, we just need to manipulate cost entries to dictate the behavior of waypoints.
D Explicit Trust Region Of The Sinkhorn Step
In trajectory optimization, it is crucial to bound the trajectory update at every iteration to be close to the previous one for stability, and so that the updated parameter remains within the region where the linear approximations are valid. Given  $\mathcal{F}(\cdot):\mathbb{R}^{T\times d}\to \mathbb{R}$  to be the planning cost functional, prior works [7, 50, 8] apply a first-order Taylor expansion at the current parameter  $\pmb{\tau}_{k}$ , while adding a regularization norm
$$
\Delta \boldsymbol {\tau} ^ {*} = \operatorname {a r g m i n} \left\{\mathcal {F} \left(\boldsymbol {\tau} _ {k}\right) + \nabla \mathcal {F} \left(\boldsymbol {\tau} _ {k}\right) \Delta \boldsymbol {\tau} + \frac {\beta}{2} \| \Delta \boldsymbol {\tau} _ {k} \| _ {M} \right\}, \tag {23}
$$
resulting in the following update rule by differentiating the right-hand side w.r.t.  $\Delta \tau$  and setting it to zero
$$
\boldsymbol {\tau} _ {k + 1} = \boldsymbol {\tau} _ {k} + \Delta \boldsymbol {\tau} ^ {*} = \boldsymbol {\tau} _ {k} - \frac {1}{\beta} M ^ {- 1} \nabla \mathcal {F} (\boldsymbol {\tau} _ {k}). \tag {24}
$$
The metric  $M$  depends on the conditioning prior. Ratliff et al. [7] propose  $M$  to be the finite difference matrix, constraining the update to stay in the region of smooth trajectories (i.e., low-magnitude trajectory derivatives). Mukadam et al. [8] use the metric  $M = K$  derived from a GP prior, also enforcing the dynamics constraint. It is well-known that solving for  $\Delta \tau$  in Eq. (23) is equivalent to minimizing the linear approximation within the ball of radius defined by the third term (i.e., the regularization norm) [51]. Hence, these mechanisms can be interpreted as implicitly shaping the trust region - biasing perturbation region by the prior, connecting the prior to the weighting matrix  $M$  in the update rule.
In contrast, the Sinkhorn Step approaches the trust region problem with a gradient-free perspective and provides a novel way to explicitly constrain the parameter updates inside a trust region defined by the regular polytope, without relying on Taylor expansions, where cost functional derivatives are not always available in practice (e.g., planning with only occupancy maps, planning through contacts). In this work, the bound on the trajectory update by the Sinkhorn Step is straightforward
$$
\begin{array}{l} \left\| \boldsymbol {\tau} _ {k + 1} - \boldsymbol {\tau} _ {k} \right\| = \left\| \alpha_ {k} \operatorname {d i a g} (\boldsymbol {n}) ^ {- 1} \boldsymbol {W} _ {\lambda} ^ {*} \boldsymbol {D} ^ {P} \right\| \\ \leq \sum_ {t = 1} ^ {T} \left\| \alpha_ {k} \frac {1}{n} \boldsymbol {w} _ {\lambda} ^ {*} \boldsymbol {D} ^ {P} \right\| \tag {25} \\ \leq \sum_ {t = 1} ^ {T} \| \alpha_ {k} \boldsymbol {d} ^ {*} \| \leq T \alpha_ {k} \\ \end{array}
$$
Algorithm 2: Stabilized Sinkhorn Algorithm
$(\pmb{a}^0,\pmb{b}^0)\gets (\pmb {0}_T,\pmb {0}_m),(\tilde{\pmb{u}}^0,\tilde{\pmb{v}}^0)\gets (\pmb {1}_T,\pmb {1}_m),M = 10^3.$    
Compute stabilized kernel  $\pmb{P^{0}}$  using Eq. (29).   
while termination criteria not met do   
// Sinkhorn iteration   
 $\tilde{\pmb{u}}^{i + 1} = \pmb {n} / (\pmb{P}^i\tilde{\pmb{v}}^i),\quad \tilde{\pmb{v}}^{i + 1} = \pmb {m} / (\pmb{P}^{i\top}\tilde{\pmb{u}}^{i + 1}).$    
// Check for numerical instabilities   
if  $\| \tilde{u}^i\|_{\infty} <   M\vee \| \tilde{v}^i\|_{\infty} <   M$  then   
// Absorption.   
 $(\pmb {a}^i,\pmb {b}^i)\gets (\pmb {a}^i +\lambda \log (\tilde{\pmb{u}}^i),\pmb {b}^i +\lambda \log (\tilde{\pmb{v}}^i)).$    
Compute stabilized kernel  $\pmb{P^{i}}$  using Eq. (29).   
 $(\tilde{u}^i,\tilde{v}^i)\gets (\mathbf{1}_T,\mathbf{1}_m)$    
end   
end   
Return  $W_{\lambda}^{*} = \mathrm{diag}(\tilde{u}^{*})P^{*}\mathrm{diag}(\tilde{v}^{*})$
resulting from  $D^{P}$  being a regular polytope inscribing the  $(d - 1)$ -unit hypersphere. In practice, one could scale the polytope in different directions by multiplying with  $M$  induced by priors, and, hence, shape the trust region in a similar fashion. Note that the bound in Eq. (25) does not depend on the local cost information.
For completeness of discussion, in sampling-based trajectory optimization, the regularization norm is related to the variance of the proposal distribution. The trajectory candidates are sampled from the proposal distribution and evaluated using the Model-Predictive Path Integral (MPPI) update rule [52]. For example, Kalakrishnan et al. [9] construct the variance matrix similarly to the finite difference matrix, resulting in a sampling distribution with low variance at the tails and high variance at the center. Recently, Urain et al. [13] propose using the same GP prior variance as in [8] to sample trajectory candidates for updates, leveraging them for tuning variance across timesteps.
E The Log-Domain Stabilization Sinkhorn Algorithm
Following Proposition 4.1 in [17], for sufficiently small regularization  $\lambda$ , the approximate solution from the entropic-regularized OT problem
$$
\boldsymbol {W} _ {\lambda} ^ {*} = \operatorname {a r g m i n} \mathrm {O T} _ {\lambda} (\boldsymbol {n}, \boldsymbol {m})
$$
approaches the true optimal plan
$$
\boldsymbol {W} ^ {*} = \operatorname * {a r g m i n} _ {\boldsymbol {W} \in U (\boldsymbol {n}, \boldsymbol {m})} \langle \boldsymbol {C}, \boldsymbol {W} \rangle .
$$
However, small  $\lambda$  incurs numerical instability for a high-dimensional cost matrix, which is usually the case for our case of batch trajectory optimization. Too high  $\lambda$ , which leads to "blurry" plans, also harms the MPOT performance. Hence, we utilize the log-domain stabilization for the Sinkhorn algorithm.
We provide a brief discussion of this log-domain stabilization. For a full treatment of the theoretical derivations, we refer to [35, 36]. First, with the marginals  $\pmb{n} \in \Sigma_T$ ,  $\pmb{m} \in \Sigma_m$  and the exponentiated kernel matrix  $\pmb{P} = \exp(-\pmb{C} / \lambda)$ , the Sinkhorn algorithm aims to find a pair of scaling factors  $\pmb{u} \in \mathbb{R}_+^T$ ,  $\pmb{v} \in \mathbb{R}_+^m$  such that
$$
\boldsymbol {u} \odot \boldsymbol {P} \boldsymbol {v} = \boldsymbol {n}, \quad \boldsymbol {v} \odot \boldsymbol {P} ^ {\intercal} \boldsymbol {u} = \boldsymbol {m}, \tag {26}
$$
where  $\odot$  is the element-wise multiplication (i.e., the Hadamard product). From a typical one vector initialization  $\pmb{v}^{0} = \mathbf{1}_{m}$ , the Sinkhorn algorithm performs a sequence of (primal) update rules
$$
\boldsymbol {u} ^ {i + 1} = \frac {\boldsymbol {n}}{\boldsymbol {P v} ^ {i}}, \quad \boldsymbol {v} ^ {i + 1} = \frac {\boldsymbol {m}}{\boldsymbol {P} ^ {\intercal} \boldsymbol {u} ^ {i + 1}}, \tag {27}
$$
leading to convergence of the scaling factors  $\pmb{u}^{*},\pmb{v}^{*}$  [53]. Then, the optimal transport plan can be computed by  $W_{\lambda}^{*} = \mathrm{diag}(\pmb{u}^{*})\pmb{P}\mathrm{diag}(\pmb{v}^{*})$
For small values of  $\lambda$ , the entries of  $P, u, v$  become either very small or very large, thus being susceptible to numerical problems (e.g., floating point underflow and overflow). To mitigate this
issue, at an iteration  $i$ , Chizat et al. [35] suggests a redundant parameterization of the scaling factors as
$$
\boldsymbol {u} ^ {i} = \tilde {\boldsymbol {u}} ^ {i} \odot \exp \left(\boldsymbol {a} ^ {i} / \lambda\right), \quad \boldsymbol {v} ^ {i} = \tilde {\boldsymbol {v}} ^ {i} \odot \exp \left(\boldsymbol {b} ^ {i} / \lambda\right), \tag {28}
$$
with the purpose of keeping  $\tilde{u},\tilde{v}$  bounded, while absorbing extreme values of  $\pmb {u},\pmb{v}$  into the logdomain via redundant vectors  $a,b$  . The kernel matrix  $P^i$  is also stabilized, having elements being modified as
$$
\boldsymbol {P} _ {t j} ^ {i} = \exp \left(\left(\boldsymbol {a} _ {t} ^ {i} + \boldsymbol {b} _ {j} ^ {i} - \boldsymbol {C} _ {t j}\right) / \lambda\right), \tag {29}
$$
such that large values in  $a, b$  and  $C$  cancel out before the exponentiation, which is crucial for small  $\lambda$ . With these ingredients, we state the log-domain stabilization Sinkhorn algorithm in Algorithm 2. Note that Algorithm 2 is mathematically equivalent to the original Sinkhorn algorithm, but the improvement in the numerical stability is significant.
Nevertheless, in practice, the extreme-value issues are still not resolved completely by Algorithm 2 due to the exponentiation of the kernel matrix  $P^i$ . Moreover, we only check for numerical issues once per iteration for efficiency. Note that multiple numerical issue checks can be done in an iteration as a trade-off between computational overhead and stability. Hence, tuning for the cost matrix  $C$  magnitudes and  $\lambda$ , for the values inside the exp function to not become too extreme, is still required for numerical stability.
F Uniform And Regular Polytopes
We provide a brief discussion on the  $d$ -dimensional uniform and regular polytope families used in the paper (cf. Section 3). For a comprehensive introduction, we refer to [29, 54]. In geometry, regular polytopes are the generalization in any dimensions of regular polygons (e.g., square, hexagon) and regular polyhedra (e.g., simplex, cube). The regular polytopes have their elements as  $j$ -facets  $(0 \leq j \leq d)$  - also called cells, faces, edges, and vertices - being transitive and also regular subpolytopes of dimension  $\leq d$  [29]. Specifically, the polytope's facets are pairwise congruent: there exists an isometry that maps any facet to any other facet.
To compactly identify regular polytopes, a Schläfli symbol is defined as the form  $\{a,b,c,\dots,y,z\}$ , with regular facets as  $\{a,b,c,\dots,y\}$ , and regular vertex figures as  $\{b,c,\dots,y,z\}$ . For example,
- a polygon having  $n$  edges is denoted as  $\{n\}$  (e.g., a square is denoted as  $\{4\}$ ),
- a regular polyhedron having  $\{n\}$  faces with  $p$  faces joining around a vertex is denoted as  $\{n,p\}$  (e.g., a cube is denoted as  $\{4,3\}$ ) and  $\{p\}$  is its vertex figure (i.e., a figure of an exposed polytope when one vertex is "sliced off"),
- a regular 4-polytope having cells  $\{n, p\}$  with  $q$  cells joining around an edge is denoted as  $\{n, p, q\}$  having vertex figure  $\{p, q\}$ , and so on.
A  $d$ -dimensional uniform polytope is a generalization of a regular polytope - only retaining the vertex-transitivity (i.e., only vertices are pairwise congruent), and is bounded by its uniform facets. In fact, nearly every uniform polytope can be constructed by Wythoff constructions, such as rectification, truncation, and alternation from either regular polytopes or other uniform polytopes [54]. This implies a vast number of possible choices of vertex-transitive uniform polytopes that can be applied to the Sinkhorn Step. Further research in this direction is interesting.
Figure 5: Examples of (left to right) 3-simplex, 3-orthorplex, 3-cube.
Table 3: Regular and uniform polytope families.
Dimension	Simplices	Orthoplexes	Hypercubes
d=2	regular trigon {3}	square {4}	square {4}
d=3	regular tetrahedron {3,3}	regular octahedron {3,4}	cube {4,3}
Any d	d-simplex {3d-1}	d-orthoplex {3d-2,4}	d-cube {4d-2,3}
We present three families of regular and uniform polytopes in Table 3, which are used in this work due to their construction simplicity (see Fig. 5), and their existence for any dimension. Note that there are regular and uniform polytope families that do not exist in any dimension [29]. The number of vertices is  $n = d + 1$  for a  $d$ -simplex,  $n = 2d$  for a  $d$ -orthoplex, and  $n = 2^d$  for a  $d$ -cube.
**Construction.** We briefly discuss the vertex coordinate construction of  $d$ -regular polytopes  $\mathcal{P}$  inscribing a  $(d - 1)$ -unit hypersphere with its centroid at the origin. Note that these constructions are GPU vectorizable. First, we denote the standard basis vectors  $e_1, \ldots, e_d$  for  $\mathbb{R}^d$ .
For a regular  $d$ -simplex, we begin the construction with the standard  $(d - 1)$ -simplex, which is the convex hull of the standard basis vectors  $\Delta^{d - 1} = \{\sum_{i = 1}^{d}w_{i}\pmb{e}_{i}\in \mathbb{R}^{d}\mid \sum_{i = 1}^{d}w_{i} = 1,w_{i}>$ $0$  , for  $i = 1,\ldots ,d\}$  . Now, we already got  $d$  vertices with the pairwise distance of  $\sqrt{2}$  . Next, the final vertex lies on the line perpendicular to the barycenter of the standard simplex, so it has the form  $(a / d,\dots ,a / d)\in \mathbb{R}^d$  for some scalar  $a$  . For the final vertex to form regular  $d$ -simplex, its distances to any other vertices have to be  $\sqrt{2}$  . Hence, we arrive at two choices of the final vertex coordinate  $\frac{1}{d} (1\pm \sqrt{1 + d})\mathbf{1}_d$  . Finally, we shift the regular  $d$ -simplex centroid to zero and rescale the coordinate such that its circumradius is 1, resulting in two sets of  $d + 1$  coordinates
$$
\left(\sqrt {1 + \frac {1}{d}} e _ {i} - \frac {1}{d \sqrt {d}} (1 \pm \sqrt {d + 1}) \mathbf {1} _ {d}\right) \text {f o r} 1 \leq i \leq d, \text {a n d} \frac {1}{\sqrt {d}} \mathbf {1} _ {d}. \tag {30}
$$
Note that we either choose two coordinate sets by choosing  $+$  or  $-$  in the computation.
For a regular  $d$ -orthoplex, the construction is trivial. The vertex coordinates are the positive-negative pair of the standard basis vectors, resulting in  $2d$  coordinates
$$
\boldsymbol {e} _ {1}, - \boldsymbol {e} _ {1}, \dots , \boldsymbol {e} _ {d}, - \boldsymbol {e} _ {d} \tag {31}
$$
For a regular  $d$ -cube, the construction is also trivial. The vertex coordinates are constructed by choosing each entry of the coordinate  $1/2$  or  $-1/2$ , resulting in  $2^d$  vertex coordinates.
G d-Dimensional Random Rotation Operator
We describe the random  $d$ -dimensional rotation operator applied on polytopes mentioned in Section 4. Focusing on the computational perspective, we describe the rotation in any dimension through the lens of matrix eigenvalues. For any  $d$ -dimensional rotation, a (proper) rotation matrix  $\mathbf{R} \in \mathbb{R}^{d \times d}$  acting on  $\mathbb{R}^d$  is an orthogonal matrix  $\mathbf{R}^\intercal = \mathbf{R}^{-1}$ , leading to  $\det(\mathbf{R}) = 1$ . Roughly speaking,  $\mathbf{R}$  does not apply contraction or expansion to the polytope convex hull  $\operatorname{vol}(\mathbf{D}^P) = \operatorname{vol}(\mathbf{D}^P \mathbf{R})$ .
For even dimension  $d = 2m$ , there exist  $d$  eigenvalues having unit magnitudes  $\varphi = e^{\pm i\theta_l}$ ,  $l = 1, \ldots, m$ . There is no dedicated fixed eigenvalue  $\varphi = 1$  depicting the axis of rotation, and thus no axis of rotation exists for even-dimensional spaces. For odd dimensions  $d = 2m + 1$ , there exists at least one fixed eigenvalue  $\varphi = 1$ , and the axis of rotation is an odd-dimensional subspace. To see this, set  $\varphi = 1$  in  $\det(R - \varphi I)$  as follows
$$
\begin{array}{l} \det (\boldsymbol {R} - \boldsymbol {I}) = \det (\boldsymbol {R} ^ {\intercal}) \det (\boldsymbol {R} - \boldsymbol {I}) = \det (\boldsymbol {R} ^ {\intercal} \boldsymbol {R} - \boldsymbol {R} ^ {\intercal}) \tag {32} \\ = \det  (\boldsymbol {I} - \boldsymbol {R}) = (- 1) ^ {d} \det  (\boldsymbol {R} - \boldsymbol {I}) = - \det  (\boldsymbol {R} - \boldsymbol {I}), \\ \end{array}
$$
with  $(-1)^{d} = -1$  for odd dimensions. Hence,  $\operatorname*{det}(R - I) = 0$ . This implies that the corresponding eigenvector  $\pmb{r}$  of  $\varphi = 1$  is a fixed axis of rotation  $\pmb{R}\pmb{r} = \pmb{r}$ . When there are some null rotations in the even-dimensional subspace orthogonal to  $\pmb{r}$ , i.e., when fixing some  $\theta_{l} = 0$ , an even number of real unit eigenvalues appears, and thus the total dimension of rotation axis is odd. In general, the odd-dimensional  $d = 2m + 1$  rotation is parameterized by the same number  $m$  of rotation
angles as in the  $2m$ -dimensional rotation. As a remark, in  $d \geq 4$ , there exist pairwise orthogonal planes of rotations, each parameterized by a rotation angle  $\theta$ . Interestingly, if we smoothly rotate a 4-dimensional object from a starting orientation and choose rotation angle rates such that  $\theta_{1} = w\theta_{2}$  with  $w$  is an irrational number, the object will never return to its starting orientation.
**Construction.** We only present random rotation operator constructions that are straightforward to vectorize. More methods on any dimensional rotation construction are presented in [55]. For an even-dimensional space  $d = 2m$ , by observing the complex conjugate eigenvalue pairs, the rotation matrix can be constructed as a block diagonal of  $2 \times 2$  matrices
$$
\boldsymbol {R} _ {l} = \left[ \begin{array}{c c} \cos \left(\theta_ {l}\right) & - \sin \left(\theta_ {l}\right) \\ \sin \left(\theta_ {l}\right) & \cos \left(\theta_ {l}\right) \end{array} \right], \tag {33}
$$
describing a rotation associated with the rotation angle  $\theta_l$  and the pairs of eigenvalues  $e^{\pm i\theta_l}$ ,  $l = 1, \ldots, m$ . In fact, this construction constitutes a maximal torus in the special orthogonal group  $SO(2m)$  represented as  $T(m) = \{\mathrm{diag}(e^{i\theta_1}, \ldots, e^{i\theta_m}), \forall l, \theta_l \in \mathbb{R}\}$ , describing the set of all simultaneous component rotations in any fixed choice of  $m$  pairwise orthogonal rotation planes [56]. This is also a maximal torus for odd-dimensional rotations  $SO(2m + 1)$ , where the group action fixes the remaining direction. For instance, the maximal tori in  $SO(3)$  are given by rotations about a fixed axis of rotation, parameterized by a single rotation angle. Hence, we construct a random  $d \times d$  rotation matrix by first uniformly sampling the angle vector  $\theta \in [0, 2\pi]^m$ , then computing in batch the  $2 \times 2$  matrices Eq. (33), and finally arranging them as block diagonal matrix.
Fortunately, in this paper, planning in first-order trajectories always results in an even-dimensional state space. Hence, we do not need to specify the axis of rotation. For general construction of a uniformly random rotation matrix in any dimension  $d \geq 2$ , readers can refer to the Stewart method [57] and our implementation of Steward method at https://github.com/anindex/ssax/blob/main/ssax/ss/rotation.py#L38.
H Related Works
Motion optimization. While sampling-based motion planning algorithms have gained significant traction [5, 6], they are typically computationally expensive, hindering their application in real-world problems. Moreover, these methods cannot guarantee smoothness in the trajectory execution, resulting in jerky robot motions that must be post-processed before executing them on a robot [11]. To address the need for smooth trajectories, a family of gradient-based methods was proposed [7, 21, 8] for finding locally optimal solutions. These methods require differentiable cost functions, effectively requiring crafting or learning signed-distance fields of obstacles. CHOMP [7] and its variants [58, 59, 50] optimize a cost function using covariant gradient descent over an initially suboptimal trajectory that connects the start and goal configuration. However, such approaches can easily get trapped in local minima, usually due to bad initializations. Stochastic trajectory optimizers, e.g., STOMP [9] sample candidate trajectories from proposal distributions, evaluate their cost, and weigh them for performing updates [52, 13]. Although gradient-free methods can handle discontinuous costs (e.g., planning with surface contact), they may cause oscillatory behavior or failure to converge, requiring additional heuristics for acquiring better performance [60]. Schulman et al. [61] addresses the computational complexity of CHOMP and STOMP, which require fine trajectory discretization for collision checking, proposing a sequential quadratic program with continuous time collision checking. Gaussian Process Motion Planning (GPMP) [8] casts motion optimization as a probabilistic inference problem. A trajectory is parameterized as a function of continuous-time that maps to robot states, while a GP is used as a prior distribution to encourage trajectory smoothness, and a likelihood function encodes feasibility. The trajectory is inferred via maximum a Posteriori (MAP) estimation from the posterior distribution of trajectories, constructed out of the GP prior and the likelihood function. In this work, we perform updates on waypoints across multiple trajectories concurrently. This view is also considered in methods that resolve trajectory optimization via collocation [62].
Optimal transport in robot planning. While OT has several practical applications in problems of resource assignment and machine learning [17], its application to robotics is scarce. Most applications consider swarm and multi-robot coordination [63-67], while OT can be used for exploration during planning [68] and for curriculum learning [69]. A comprehensive review of OT in control is available in [70]. Recently, Le et al. [71] proposed a method for re-weighting Riemannian motion policies [72]
Figure 6: (Left) An example of the Panda arm plan execution for three simulation frames. The green line denotes a  $SE(3)$  goal. (Right) An example of red collision spheres attached to TIAGo++ mesh at a configuration. The collision spheres are transformed with the robot links via forward kinematics.
using an unbalanced OT at the high level, leading to fast reactive robot motion generation that effectively escapes local minima.
I Additional Experimental Details
We elaborate on all additional experimental details omitted in the main paper. All experiments are executed in a single RTX3080Ti GPU and a single AMD Ryzen 5900X CPU. Note that due to the fact that all codebases are implemented in PyTorch (e.g., forward kinematics, planning objectives, collision checkings, environments, etc.), hence due to conformity reasons, we also implement RRT*/I-RRT* in PyTorch. However, we set using CPU when running RRT*/I-RRT* experiments and set using GPU for MPOT and the other baselines. An example of Panda execution for collision checking in PyBullet is shown in Fig. 6.
For a fair comparison, we construct the initialization GP prior  $\mathcal{N}(\pmb{\mu}_0, \pmb{K}_0)$  with a constant-velocity straight line connecting the start and goal configurations, and sample initial trajectories for all trajectory optimization algorithms. We use the constant-velocity GP prior Eq. (19), both in the cost term and for the initial trajectory samples. To the best of our knowledge, the baselines are not explicitly designed for batch trajectory optimization. Striving for a unifying experiment pipeline and fair comparison, we reimplement all baselines in PyTorch with vectorization design (beside RRT*) and fine-tune them with the parallelization setting, which is unavailable in the original codebases.