SlowGuess/NeurIPS_2023_OCR huggingface dataset:
We presented MPOT—a gradient-free and efficient batch motion planner that optimizes multiple high-dimensional trajectories over non-convex objectives. In particular, we proposed the Sinkhorn Step—a zero-order batch update rule parameterized by a local optimal transport plan with a nice property of cost-agnostic step bound, effectively updating waypoints across trajectories independently. We demonstrated that in practice, our method converges, scales very well to high-dimensional tasks, and provides practically smooth plans. This work opens multiple exciting research questions, such as investigating further polytope families that can be applied for scaling up to even more high-dimensional settings, conditional batch updates, or different strategies for adapting the step-size. Furthermore, while classical motion planning considers single planning instance for each task, which under-utilizes the modern GPU capability, this work encourages future work that benefits from vectorization in the algorithmic choices, providing multiple plans and covering several modes, leading to execution robustness or even for dataset collection for downstream learning tasks. At last, we foresee potential applications of Sinkhorn Step to sampling methods or variational inference.
An T. Le was supported by the German Research Foundation project METRIC4IMITATION (PE 2315/11-1). Georgia Chalvatzaki was supported by the German Research Foundation (DFG) Emmy Noether Programme (CH 2676/1-1). We also gratefully acknowledge Alexander Lambert for his implementation of the Gaussian Process prior; Pascal Klink, Joe Watson, João Carvalho, and Julien Urain for the insightful and fruitful discussions; Snehal Jauhri, Puze Liu, and João Carvalho for their help in setting up the TIAGo++ environments.
[1] Jean-Paul Laumond et al. Robot motion planning and control, volume 229. Springer, 1998.
[2] Laurene Claussmann, Marc Revilloud, Dominique Gruyer, and Sébastien Glaser. A review of motion planning for highway autonomous driving. IEEE Transactions on Intelligent Transportation Systems, 21(5):1826-1848, 2019.
[3] Dario Izzo and Lorenzo Pettazzi. Autonomous and distributed motion planning for satellite swarm. Journal of Guidance, Control, and Dynamics, 30(2):449-459, 2007.
[4] Ibrahim Al-Bluwi, Thierry Simeon, and Juan Cortés. Motion planning algorithms for molecular simulations: A survey. Computer Science Review, 6(4):125–143, 2012.
[5] Lydia E Kavraki, Petr Svestka, J-C Latombe, and Mark H Overmars. Probabilistic roadmaps for path planning in high-dimensional configuration spaces. IEEE Transactions on Robotics and Automation, 1996.
[6] James J Kuffner and Steven M LaValle. Rrt-connect: An efficient approach to single-query path planning. In IEEE ICRA, 2000.
[7] Nathan Ratliff, Matt Zucker, J Andrew Bagnell, and Siddhartha Srinivasa. Chomp: Gradient optimization techniques for efficient motion planning. In IEEE ICRA, 2009.
[8] Mustafa Mukadam, Jing Dong, Xinyan Yan, Frank Dellaert, and Byron Boots. Continuous-time gaussian process motion planning via probabilistic inference. *IJRR*, 2018.
[9] Mrinal Kalakrishnan, Sachin Chitta, Evangelos Theodorou, Peter Pastor, and Stefan Schaal. Stomp: Stochastic trajectory optimization for motion planning. In IEEE ICRA, 2011.
[10] Sertac Karaman and Emilio Frazzoli. Sampling-based algorithms for optimal motion planning. The international journal of robotics research, 30(7):846-894, 2011.
[11] Chonhyon Park, Jia Pan, and Dinesh Manocha. High-dof robots in dynamic environments using incremental trajectory optimization. International Journal of Humanoid Robotics, 11(02): 1441001, 2014.
[12] Anca D Dragan, Nathan D Ratliff, and Siddhartha S Srinivasa. Manipulation planning with goal sets using constrained trajectory optimization. In 2011 IEEE International Conference on Robotics and Automation, pages 4582-4588. IEEE, 2011.
[13] Julien Urain, An T Le, Alexander Lambert, Georgia Chalvatzaki, Byron Boots, and Jan Peters. Learning implicit priors for motion optimization. In 2022 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), 2022.
[14] Richard Sinkhorn and Paul Knopp. Concerning nonnegative matrices and doubly stochastic matrices. Pacific Journal of Mathematics, 21(2):343-348, 1967.
[15] Marc Toussaint. Robot trajectory optimization using approximate inference. In Proceedings of the 26th annual international conference on machine learning, pages 1049-1056, 2009.
[16] Cédric Villani. Optimal transport: old and new, volume 338. Springer, 2009.
[17] Gabriel Peyre, Marco Cuturi, et al. Computational optimal transport: With applications to data science. Foundations and Trends® in Machine Learning, 11(5-6):355-607, 2019.
[75] S. Surjanovic and D. Bingham. Virtual library of simulation experiments: Test functions and datasets. Retrieved October 26, 2023, from http://www.sfu.ca/~ssurjano, 2013.
[76] Hicham Janati, Marco Cuturi, and Alexandre Gramfort. Debiased sinkhorn barycenters. In International Conference on Machine Learning, pages 4692-4701. PMLR, 2020.
A Theoretical Analysis & Proofs
We investigate the proposed Sinkhorn Step (Definition 2 in Section 3.2) properties for a non-convex and smooth objective function (Assumption 1). Our preliminary analysis performs at arbitrary iteration  $k > 0$  and depends on Assumption 1 and Assumption 2 stated in Section 3.
First, we state a proof sketch of Proposition 1.
Proof. Observe that by construction of  $d$ -dimensional regular polytope  $P \in \mathcal{P}$ , the convex hull of its vertex set  $\mathcal{V}^P$
$$
\operatorname {c o n v} \left(\mathcal {V} ^ {P}\right) = \left\{\sum_ {i} w _ {i} \boldsymbol {v} _ {i} \mid \boldsymbol {v} _ {i} \in \mathcal {V} ^ {P}, \sum_ {i} w _ {i} = 1, w _ {i} > 0, \forall i \right\}
$$
has  $\dim (\mathrm{conv}(\mathcal{V}^P)) = d$  dimensions. Hence, trivially, the conic hull of  $D^{P}$  positively spans  $\mathbb{R}^d$ .
Now, we can investigate the quality of  $D^P$  in the following lemma.
Lemma 1. For any  $\mathbf{a} \in \mathbb{R}^d$ ,  $\mathbf{a} \neq \mathbf{0}$ ,  $\exists \mathbf{d} \in D^P$  such that
$$
\langle \boldsymbol {a}, \boldsymbol {d} \rangle \geq \mu_ {P} \| \boldsymbol {a} \|, 0 \leq \mu_ {p} \leq 1
$$
where  $\mu_P = 1 / \sqrt{d(d + 1)}$  for  $P = d$ -simplex,  $\mu_P = 1 / \sqrt{d}$  for  $P = d$ -orthoplex, and  $\mu_P = 1 / \sqrt{2}$  for  $P = d$ -cube.
Proof. From Proposition 1,  $D^{P}$  is a positive spanning set, then for any  $\mathbf{a} \in \mathbb{R}^{d}$ ,  $\exists \mathbf{d} \in D^{P}$  such that  $\langle \mathbf{a}, \mathbf{d} \rangle > 0$  (Theorem 2.6, [28]). This property results in the positive cosine measure of  $D^{P}$  (Proposition 7, [42])
$$
1 \geq \mu_ {P} := \min  _ {\mathbf {0} \neq \mathbf {a} \in \mathbb {R} ^ {d}} \max  _ {\mathbf {d} \in D ^ {P}} \frac {\langle \mathbf {a} , \mathbf {d} \rangle}{\| \mathbf {a} \| \| \mathbf {d} \|} > 0 \tag {11}
$$
Equivalently,  $\mu_P$  is the largest scalar such that  $\langle \pmb{a},\pmb{d}\rangle \geq \mu_P\| \pmb {a}\| \| \pmb {d}\| = \mu_P\| \pmb {a}\|$ ,  $\pmb {d}\in D^{P}$ .
Next, due to the symmetry of the regular polytope family  $\mathcal{P}$ , there exists an inscribing hypersphere  $S_r^{d-1}$  with radius  $r$  for any  $P \in \mathcal{P}$  [29]. For  $\mathcal{P}$ , the tangent points of the inscribing hypersphere to the facets are also the centroid of the facets. Then, the centroid vectors pointing from the origin towards these tangent points form equal angles to all nearby vertex vectors. Thus, the cosine measure attains its saddle points Eq. (11) at these centroid vectors having the value
$$
\mu_ {P} = \frac {r}{R}
$$
with  $R = 1$  is the radius of the circumscribed unit hypersphere. The inradius  $r$  for  $d$ -simplex,  $d$ -orthoplex, and  $d$ -cube are  $1 / \sqrt{d(d + 1)}, 1 / \sqrt{d}, 1 / \sqrt{2}$ , respectively [29].
This lemma has a straightforward geometric implication - for every  $\pmb{v} \neq \mathbf{0}, \pmb{v} \in \mathbb{R}^d$ , there exists a search direction  $\pmb{d} \in D^{P}$  such that the cosine angle between these vectors is acute (i.e.,  $\mu_P > 0$ ). Then, if we consider the negative gradient vector, which is unknown, there exists a direction in  $D^{P}$  that approximates it well with  $\mu_P$  being the quality metric (i.e., larger  $\mu_P$  is better). The values of  $\mu_P$  for each polytope type also confirm the intuition that, for  $d$ -cube with an exponential number of vertices  $m = 2^d$  has a constant cosine measure, while the cosine measure of  $d$ -simplex having  $m = d + 1$  vertices scales  $O(1 / d)$  with dimension. Now, we state the key lemma used to prove the main property of Sinkhorn Step.
Lemma 2 (Key lemma). If Assumption 1 and Assumption 2 holds, then  $\forall \pmb{x}_k\in X_k,\forall k > 0$
$$
f \left(\boldsymbol {x} _ {k + 1}\right) \leq f \left(\boldsymbol {x} _ {k}\right) - \mu_ {P} \alpha_ {k} \| \nabla f \left(\boldsymbol {x} _ {k}\right) \| + \frac {L}{2} \alpha_ {k} ^ {2} \tag {12}
$$
Proof. If the Assumption 2 holds, by Proposition 4.1 in [17], the OT solution  $\mathbf{W}_{\lambda} \rightarrow \mathbf{W}_0$  converges to the optimal solution with maximum entropy in the set of solutions of the original problem
$$
\min _ {\boldsymbol {W} \in U (\mathbf {1} _ {n} / n, \mathbf {1} _ {n} / n)} \langle \boldsymbol {W}, \boldsymbol {C} \rangle .
$$
Moreover, Birkhoff doubly stochastic matrix theorem [43] states that the set of extremal points of  $U(\mathbf{1}_n / n, \mathbf{1}_n / n)$  is equal to the set of permutation matrices, and the fundamental theorem of linear programming (Theorem 2.7 in [44]) states that the minimum of a linear objective in a finite non-empty polytope is reached at a vertex or a face of the polytope (i.e., the feasible space of the linear program), leading to the following two cases.
- Case 1:  $W_{\lambda} / n \to W_0 / n$  converges to a permutation matrix representing the bijective mapping between the optimizing points and the polytope vertices. There exists a vertex evaluation permutation forming the cost matrix such that, the update step  $s_k$  is a descending step for each optimizing point
$$
\forall \boldsymbol {x} _ {k} \in X _ {k}, \boldsymbol {s} _ {k} = \alpha_ {k} \frac {1}{n} \boldsymbol {w} _ {0} ^ {*} \boldsymbol {D} ^ {P} = \underset {\boldsymbol {d} \in D ^ {P}} {\operatorname {a r g m i n}} \left\{f \left(\boldsymbol {x} _ {k} + \alpha_ {k} \boldsymbol {d}\right) \right\} \tag {13}
$$
with  $\pmb{w}_0^*$  as a row in  $\pmb{W}_0^*$ , then  $\pmb{w}_0^* / n$  is a one-hot vector. Let  $\pmb{a} = -\nabla f(\pmb{x}_k)$ ,  $\pmb{s}_k$  is a descending step  $f(\pmb{x}_k + \pmb{s}_k) \leq f(\pmb{x}_k)$ , then, by Lemma 1,  $\langle \nabla f(\pmb{x}_k), \pmb{s}_k \rangle \leq -\alpha_k \mu_P \| \nabla f(\pmb{x}_k) \|$ .
- Case 2:  $W_{\lambda} / n \to W_0 / n$  converges to a linear interpolation between the permutation matrices defining the neighboring vertices of the polytope. In this case, there are infinite solutions as the linear interpolation between the two bijective maps. There still exists a vertex evaluation permutation forming the cost matrix such that, the update step  $s_k$  is the linear interpolation of multiple tied descending steps for each optimizing point, with  $s_k = \sum_i b_i d_i$ ,  $\sum_i b_i = 1$ ,  $b_i \geq 0$ ,  $d_i = \operatorname{argmin}_{d \in D^P} \{f(\pmb{x}_k + \alpha_k d)\}$ . Following the argument of Case 1, since  $s_k$  is the linear interpolation of descending steps, we also conclude that  $\langle \nabla f(\pmb{x}_k), \pmb{s}_k \rangle = \sum_i b_i \langle \nabla f(\pmb{x}_k), d_i \rangle \leq -\sum_i b_i \alpha_k \mu_P \| \nabla f(\pmb{x}_k) \| = -\alpha_k \mu_P \| \nabla f(\pmb{x}_k) \|$ .
Finally, starting the L-smooth property of  $f$ , we can write
$$
\begin{array}{l} \forall \boldsymbol {x} \in X, \forall k > 0, f (\boldsymbol {x} _ {k + 1}) = f (\boldsymbol {x} _ {k} + \boldsymbol {s} _ {k}) \leq f (\boldsymbol {x} _ {k}) + \left\langle \nabla f (\boldsymbol {x} _ {k}), \boldsymbol {s} _ {k} \right\rangle + \frac {L}{2} \| \boldsymbol {s} _ {k} \| ^ {2} \tag {14} \\ \leq f (\boldsymbol {x} _ {k}) - \mu_ {P} \alpha_ {k} \| \nabla f (\boldsymbol {x} _ {k}) \| + \frac {L}{2} \alpha_ {k} ^ {2} \\ \end{array}
$$
recalling that  $\| \pmb {d}\| = 1,\forall \pmb {d}\in D^P$
If the sufficient decrease condition does not hold  $f(\pmb{x}_k) - f(\pmb{x}_{k + 1}) < c\alpha_k^2$  with some  $c > 0$ , then the iteration is deemed unsuccessful. In fact, Lemma 2 is similar to (Lemma 10, [42]), which states that the gradients for these unsuccessful iterations are bounded above by a scalar multiplied with the stepsize. We can see this by rewriting Eq. (12) as
$$
\begin{array}{l} \| \nabla f (\boldsymbol {x} _ {k}) \| \leq \frac {1}{\mu_ {P}} \left(\frac {f (\boldsymbol {x} _ {k}) - f (\boldsymbol {x} _ {k + 1})}{\alpha_ {k}} + \frac {L}{2} \alpha_ {k}\right) \\ <   \frac {1}{\mu_ {P}} \left(c + \frac {L}{2}\right) \alpha_ {k}. \\ \end{array}
$$
We can implement a check if the sufficient decrease condition holds for ensuring monotonicity in each iteration, as a variant of the Sinkhorn Step.
Lemma 2 also enables analyzing each optimizing point separately, and hence we can state the following main theorem separately for each  $\pmb{x}_k \in X_k$ .
Theorem 1 (Main result). If Assumption 1 and Assumption 2 hold at each iteration and the stepsize is sufficiently small  $\alpha_{k} = \alpha$  with  $0 < \alpha < 2\mu_{P}\epsilon /L$ , then with a sufficient number of iteration
$$
K \geq k (\epsilon) := \frac {f (\pmb {x} _ {0}) - f _ {*}}{(\mu_ {P} \epsilon - \frac {L \alpha}{2}) \alpha} - 1,
$$
Proof. We attempt the proof by contradiction, thus we assume  $\| \nabla f(\pmb{x}_k) \| > \epsilon$  for all  $k \leq k(\epsilon)$ . From Lemma 2, we have  $\forall \pmb{x}_k \in X_k, \forall k > 0$
$$
f (\boldsymbol {x} _ {k + 1}) \leq f (\boldsymbol {x} _ {k}) - \mu_ {P} \alpha \| \nabla f (\boldsymbol {x} _ {k}) \| + \frac {L}{2} \alpha^ {2}.
$$
From Assumption 1, the objective is bounded below  $f_{*} \leq f(x)$ . Hence, we can write
$$
\begin{array}{l} f _ {*} \leq f (\boldsymbol {x} _ {K + 1}) <   f (\boldsymbol {x} _ {K}) - \mu_ {P} \alpha \| \nabla f (\boldsymbol {x} _ {K}) \| + \frac {L}{2} \alpha^ {2} \\ \leq f (\boldsymbol {x} _ {K - 1}) - \mu_ {P} \alpha \left(\| \nabla f (\boldsymbol {x} _ {K}) \| + \| \nabla f (\boldsymbol {x} _ {K - 1}) \|\right) + 2 \frac {L}{2} \alpha^ {2} \\ \leq f \left(\boldsymbol {x} _ {0}\right) - \mu_ {P} \alpha \sum_ {k = 0} ^ {K} \| \nabla f \left(\boldsymbol {x} _ {k}\right) \| + (K + 1) \frac {L}{2} \alpha^ {2} \tag {15} \\ \leq f \left(\boldsymbol {x} _ {0}\right) - (K + 1) \mu_ {P} \alpha \epsilon + (K + 1) \frac {L}{2} \alpha^ {2} \\ \leq f \left(\boldsymbol {x} _ {0}\right) - (K + 1) \left(\mu_ {P} \alpha \epsilon - \frac {L}{2} \alpha^ {2}\right) \\ \leq f \left(\boldsymbol {x} _ {0}\right) - \left(f \left(\boldsymbol {x} _ {0}\right) - f _ {*}\right) \\ = f _ {*} \\ \end{array}
$$
by applying recursively Lemma 2 and the iteration lower bound at the second last line, which is a contradiction  $f_{*} \leq f_{*}$ . Hence,  $\| \nabla f(\pmb{x}_k) \| \leq \epsilon$  for some  $k \leq k(\epsilon)$ .
If  $L$  is known, we can compute the optimal stepsize  $\alpha = \mu_P\epsilon /L$ . Then, the complexity bound is  $k(\epsilon) = \frac{2L(f(\pmb{x}_0) - f_*)}{\mu_P^2\epsilon^2} -1$ . Note that Theorem 1 only guarantees the gradient of some points in the sequence of Sinkhorn Steps will be arbitrarily small. If in practice, we implement the sufficient decreasing condition  $f(\pmb {x}_k) - f(\pmb {x}_{k + 1})\geq c\alpha_k^2$ , then  $f(\pmb {x}_K)\leq f(\pmb {x}_i)$ ,  $\| \nabla f(\pmb {x}_i)\| \leq \epsilon$  holds. However, this sufficient decrease check may waste some iterations and worsen the performance. We show in the experiments that the algorithm exhibits convergence behavior without this condition checking. Finally, we remark on the complexity bounds when using different polytope types for Sinkhorn Step under Assumption 1 and Assumption 2, by substituting  $\mu_{P}$  according to Lemma 1.
Remark 1. By Theorem 1, with the optimal stepsize  $\alpha = \mu_P\epsilon /L$ , the complexity bounds for  $d$ -simplex,  $d$ -orthoplex and  $d$ -cube are  $O(d^{2} / \epsilon^{2})$ ,  $O(d / \epsilon^{2})$ , and  $O(1 / \epsilon^2)$ , respectively.
The optimal stepsize with  $d$ -simplex reports the same complexity  $O(d^{2} / \epsilon^{2})$  as the best-known bound for directional-direct search [45]. Within the directional-direct search scope,  $d$ -cube reports the new best-known complexity bound  $O(1 / \epsilon^2)$ , which is independent of dimension  $d$  since the number of search directions is also increased exponentially with dimension. However, in practice, solving a batch update with  $d$ -cube for each iteration is expensive since now the column-size of the cost matrix is  $2^{d}$ .
B Gaussian Process Trajectory Prior
To provide a trajectory prior with tunable time-correlated covariance for trajectory optimization, either as initialization prior or as cost, we introduce a prior for continuous-time trajectories using a GP [46, 47, 8]:  $\tau \sim \mathcal{GP}(\boldsymbol{\mu}(t), \boldsymbol{K}(t, t'))$ , with mean function  $\boldsymbol{\mu}$  and covariance function  $\boldsymbol{K}$ . As described in [47, 48, 13], a GP prior can be constructed from a linear time-varying stochastic differential equation
$$
\dot {\boldsymbol {x}} = \boldsymbol {A} (t) \boldsymbol {x} (t) + \boldsymbol {u} (t) + \boldsymbol {F} (t) \mathbf {w} (t) \tag {16}
$$
with  $\pmb{u}(t)$  the control input,  $\pmb{A}(t)$  and  $\pmb{F}(t)$  the time-varying system matrices, and  $\mathbf{w}(t)$  a disturbance following the white-noise process  $\mathbf{w}(t) \sim \mathcal{GP}(\mathbf{0}, \mathbf{Q}_c\delta(t - t'))$ , where  $\mathbf{Q}_c \succ 0$  is the power-spectral density matrix. With a chosen discretization time  $\Delta t$ , the continuous-time GP can be parameterized by a mean vector of Markovian support states  $\pmb{\mu} = [\pmb{\mu}(0), \dots, \pmb{\mu}(T)]^{\intercal}$  and covariance matrix  $\pmb{K} = [\pmb{K}(i,j)]_{ij,0 \leq i,j \leq T}$ ,  $\pmb{K}(i,j) \in \mathbb{R}^{d \times d}$ , resulting in a multivariate Gaussian  $q(\tau) = \mathcal{N}(\pmb{\mu}, \pmb{K})$ .
The inverse of the covariance matrix has a sparse structure  $\pmb{K}^{-1} = \pmb{D}^{\top}\pmb{Q}^{-1}\pmb{D}$  (Theorem 1 in [47]) with
$$
\boldsymbol {D} = \left[ \begin{array}{c c c c c} \boldsymbol {I} & & & & \\ - \Phi_ {1, 0} & \boldsymbol {I} & & & \\ & & \dots & & \\ & & & \boldsymbol {I} & \boldsymbol {0} \\ & & & - \Phi_ {T, T - 1} & \boldsymbol {I} \\ & & & \boldsymbol {0} & \boldsymbol {I} \end{array} \right], \tag {17}
$$
and the block diagonal time-correlated noise matrix  $Q^{-1} = \mathrm{diag}(\pmb{\Sigma}_s^{-1}, \pmb{Q}_{0,1}^{-1}, \dots, \pmb{Q}_{T-1,T}^{-1}, \pmb{\Sigma}_g^{-1})$ . Here,  $\Phi_{t,t+1}$  is the state transition matrix,  $Q_{t,t+1}$  the covariance between time step  $t$  and  $t+1$ , and  $\pmb{\Sigma}_s, \pmb{\Sigma}_g$  are the chosen covariance of the start and goal states. In this work, we mainly consider the constant-velocity prior (i.e., white-noise-on-acceleration model  $\ddot{\mathbf{x}}(t) = \mathbf{w}(t)$ ), which can approximately represent a wide range of systems such as point-mass dynamics, gravity-compensated robotics arms [8], differential drive [47], etc., while enjoying its sparse structure for computation efficiency. As used in our paper, this constant-velocity prior can be constructed from Eq. (16) with the Markovian state representation  $\pmb{x} = [\mathbf{x}, \dot{\mathbf{x}}] \in \mathbb{R}^d$  and
$$
\boldsymbol {A} (t) = \left[ \begin{array}{c c} \mathbf {0} & \boldsymbol {I} _ {d / 2} \\ \mathbf {0} & \mathbf {0} \end{array} \right], \quad \boldsymbol {u} = \mathbf {0}, \quad \boldsymbol {F} (t) = \left[ \begin{array}{c} \mathbf {0} \\ \boldsymbol {I} _ {d / 2} \end{array} \right] \tag {18}
$$
Then, following [47, 8], the state transition and covariance matrix are
$$
\boldsymbol {\Phi} _ {t, t + 1} = \left[ \begin{array}{c c} \boldsymbol {I} _ {d / 2} & \Delta t \boldsymbol {I} _ {d / 2} \\ \boldsymbol {0} & \boldsymbol {I} _ {d / 2} \end{array} \right], \quad \boldsymbol {Q} _ {t, t + 1} = \left[ \begin{array}{c c} \frac {1}{3} \Delta t ^ {3} \boldsymbol {Q} _ {c} & \frac {1}{2} \Delta t ^ {2} \boldsymbol {Q} _ {c} \\ \frac {1}{2} \Delta t ^ {2} \boldsymbol {Q} _ {c} & \Delta t \boldsymbol {Q} _ {c} \end{array} \right] \tag {19}
$$
with the inverse
$$
\boldsymbol {Q} _ {t, t + 1} ^ {- 1} = \left[ \begin{array}{c c} 1 2 \Delta t ^ {- 3} \Delta t ^ {3} \boldsymbol {Q} _ {c} ^ {- 1} & - 6 \Delta t ^ {- 2} \Delta t ^ {3} \boldsymbol {Q} _ {c} ^ {- 1} \\ - 6 \Delta t ^ {- 2} \Delta t ^ {3} \boldsymbol {Q} _ {c} ^ {- 1} & 4 \Delta t ^ {- 1} \boldsymbol {Q} _ {c} ^ {- 1} \end{array} \right], \tag {20}
$$
which are used to compute  $\pmb{K}$ .
Consider priors on start state  $q_{s}(\pmb{x}) = \mathcal{N}(\pmb{\mu}_{s},\pmb{\Sigma}_{s})$  and goal state  $q_{g}(\pmb{x}) = \mathcal{N}(\pmb{\mu}_{g},\pmb{\Sigma}_{g})$ , the GP prior for discretized trajectory can be factored as follows [47, 8]
$$
\begin{array}{l} q _ {F} (\boldsymbol {\tau}) \propto \exp \left(- \frac {1}{2} \| \boldsymbol {\tau} - \boldsymbol {\mu} \| _ {\boldsymbol {K} ^ {- 1}} ^ {2}\right) \\ \propto q _ {s} \left(\boldsymbol {x} _ {0}\right) q _ {g} \left(\boldsymbol {x} _ {T}\right) \prod_ {t = 0} ^ {T - 1} q _ {t} \left(\boldsymbol {x} _ {t}, \boldsymbol {x} _ {t + 1}\right), \tag {21} \\ \end{array}
$$
where each binary GP-factor is defined
$$
\left. q _ {t} \left(\boldsymbol {x} _ {t}, \boldsymbol {x} _ {t + 1}\right) = \exp \left\{- \frac {1}{2} \| \boldsymbol {\Phi} _ {t, t + 1} \left(\boldsymbol {x} _ {t} - \boldsymbol {\mu} _ {t}\right) - \left(\boldsymbol {x} _ {t + 1} - \boldsymbol {\mu} _ {t + 1}\right) \| _ {\boldsymbol {Q} _ {t, t + 1} ^ {- 1}} ^ {2} \right\}. \right. \tag {22}
$$
In the main paper, we use this constant-velocity GP formulation to sample initial trajectories. The initialization GP is parameterized by the constant-velocity straight line  $\pmb{\mu}_0$  connecting a start configuration  $\pmb{\mu}_s$  to a goal configuration  $\pmb{\mu}_g$ , having moderately high covariance  $\pmb{K}_0$ . For using this GP as the cost, we set the zero-mean  $\pmb{\mu} = \mathbf{0}$  to describe the uncontrolled trajectory distribution. The conditioning  $q_g(\pmb{x}_T)$  of the final waypoint to the goal configuration  $\pmb{\mu}_g$  is optional (e.g., when the goal configuration solution from inverse kinematics is sub-optimal), and we typically use the  $SE(3)$  goal cost.
C Additional Discussions Of Batch Trajectory Optimization
Direct implications of batch trajectory optimization. MPOT can be used as a strong oracle for collecting datasets due to the solution diversity covering various modes, capturing homotopy classes of the tasks and their associated contexts. For direct execution, with high variance initialization, an abundance of solutions vastly increases the probability of discovering good local minima, which we can select the best solution according to some criteria, e.g., collision avoidance, smoothness, model consistency, etc.
Solution diversity of MPOT. Batch trajectory optimization can serve as a strong oracle for collecting datasets or striving to discover a global optimal trajectory for execution. Three main interplaying factors contribute to the solution diversity, hence discovering better solution modes. They are
- the step radius  $\alpha_{k}$  annealing scheme,
- the variances of GP prior initialization,
- the number of plans in a batch.
Additional sampling mechanism that promotes diversity, such as Stein Variational Gradient Descent (SVGD) [49] can be straightforwardly integrated into the trajectory optimization problem [23]. This is considered in the future version of this paper to integrate the SVGD update rule with the Sinkhorn Step (i.e., using the Sinkhorn Step to approximate the score function) for even more diverse trajectory planning.