SlowGuess/NeurIPS_2023_OCR huggingface dataset:
which is similar to Eq. (6) with the energy function  $E = c \circ F(\mathbf{x}_0, \mathbf{U})$  being the composition of the cost  $c$  over  $\pmb{\tau}$  and the dynamics  $F$ . The dynamics  $F$  is also now integrated into the prior distribution  $q_{F}(\pmb{\tau})$ . The absorption of the dynamics into the prior becomes evident when we represent the prior as a zero-mean constant-velocity GP prior  $q_{F}(\pmb{\tau}) = \mathcal{N}(\mathbf{0}, \pmb{K})$ , with a constant time-discretization  $\Delta t$  and the time-correlated trajectory covariance  $\pmb{K}$ , as described in Appendix B.
Now, to apply Sinkhorn Step, consider the trajectory we want to optimize  $\pmb{\tau} = \{\pmb{x}_t\}_{t=1}^T$ , we can define the proposal trajectory distribution as a waypoint empirical distribution
$$
p (\boldsymbol {x}; \boldsymbol {\tau}) = \sum_ {t = 1} ^ {T} p (t) p (\boldsymbol {x} | t) = \sum_ {t = 1} ^ {T} n _ {t} \delta_ {\boldsymbol {x} _ {t}} (\boldsymbol {x}), \tag {8}
$$
with the histogram  $\pmb{n} = [n_1, \dots, n_T]$ ,  $p(t) = n_t = 1 / T$ , and  $\delta_{\pmb{x}_t}$  the Dirac on waypoints at time steps  $t$ . In this case, we consider the model-free setting for the proposal distribution. Indeed, this form of proposal trajectory distribution typically assumes no temporal or spatial (i.e., kinematics) correlation between waypoints. This assumption is also seen in [7, 9] and can be applied in a wide range of robotics applications where the system model is fully identified. We leverage this property for batch-wise computations and batch updates over all waypoints. The integration of model constraints in the proposal distribution is indeed interesting but is deferred for future work.
Following the planning-as-inference perspective, the motion planning problem can be formulated as the minimization of a Kullback-Leibler (KL) divergence between the proposal trajectory distribution  $p(\pmb{x};\pmb{\tau})$  and the target posterior distribution Eq. (7) (i.e., the I-projection)
$$
\begin{array}{l} \boldsymbol {\tau} ^ {*} = \underset {\boldsymbol {\tau}} {\operatorname {a r g m i n}} \left\{\mathrm {K L} (p (\boldsymbol {x}; \boldsymbol {\tau}) | | q ^ {*} (\boldsymbol {\tau})) = \mathbb {E} _ {p} [ \log q ^ {*} (\boldsymbol {\tau}) ] - H (p) \right\} \\ = \underset {\boldsymbol {\tau}} {\operatorname {a r g m i n}} \mathbb {E} _ {p} \left[ \eta c (\boldsymbol {\tau}) + \frac {1}{2} \| \boldsymbol {\tau} \| _ {\boldsymbol {K}} ^ {2} - \log Z \right] \tag {9} \\ = \operatorname *{argmin}_{\boldsymbol{\tau}}\sum_{t = 0}^{T - 1}\eta \underbrace{c(\boldsymbol{x}_{t})}_{\text{state cost}} + \underbrace{\frac{1}{2}\|\Phi_{t,t + 1}\boldsymbol{x}_{t} - \boldsymbol{x}_{t + 1}\|_{Q_{t,t + 1}^{-1}}^{2}}_{\text{transition model cost}}, \\ \end{array}
$$
with  $\Phi_{t,t+1}$  the state transition matrix, and  $Q_{t,t+1}$  the covariance between time steps  $t$  and  $t+1$  originated from the GP prior (cf. Appendix B), and the normalizing constant of the target posterior  $Z$  is absorbed. Note that the entropy of the empirical distribution is constant  $H(p) = -\int_{\pmb{x} \in \mathbb{R}^d} \frac{1}{T} \sum_{t=1}^{T} \delta_{\pmb{x}_t}(\pmb{x}) \log p(\pmb{x}; \pmb{\tau}) = \log T$ . Evidently, KL objective Eq. (9) becomes a standard motion planning problem Eq. (2) with the defined waypoint empirical distributions. Note that this objective is not equivalent to Eq. (3) due to the second coupling term. However, we demonstrate in Section 5.2 that MPOT still exhibits convergence. Indeed, investigating Sinkhorn Step in a general graphical model objective [34] is vital for future work. We apply Sinkhorn Step to Eq. (9) by realizing the trajectory as a batch of optimizing points  $\pmb{\tau} \in \mathbb{R}^{T \times d}$ . This realization also extends naturally to a batch of trajectories described in the next section.
The main goal of this formulation is to naturally inject the GP dynamics prior to MPOT, benefiting from the GP sparse Markovian structure resulting in the second term of the objective Eq. (9). This problem formulation differs from the moment-projection objective [33, 8, 13], which relies on importance sampling from the proposal distribution to perform parameter updates. Contrarily, we do not encode the model in the proposal distribution and directly optimize for the trajectory parameters, enforcing the model constraints in the cost.
4.2 Practical considerations for applying Sinkhorn Step
For the practical implementation, we make the following realizations to the Sinkhorn Step implementation for optimizing a trajectory  $\pmb{\tau}$ . First, we define a set of probe points for denser function evaluations (i.e., cost-to-go for each vertex direction). We populate equidistantly probe points along the directions in  $D^{P}$  outwards till reaching a probe radius  $\beta_{k} \geq \alpha_{k}$ , resulting in the probe set  $H^{P}$  with its matrix form  $\pmb{H}^{P} \in \mathbb{R}^{m \times h \times d}$  with  $h$  probe points for each direction (cf. Fig. 2). Second, we add stochasticity in the search directions by applying a random  $d$ -dimensional rotation  $\pmb{R} \in SO(d)$  to the polytopes to promote local exploration (computation of  $\pmb{R} \in SO(d)$  is discussed in Appendix G). Third, to further decouple the correlations between the waypoints updates, we sample the rotation matrices in batch and then construct the direction sets from the rotated polytopes, resulting in the
tensor  $D^{P} \in \mathbb{R}^{T \times m \times d}$ . Consequently, the probe set is also constructed in batch for every waypoint  $H^{P} \in \mathbb{R}^{T \times m \times h \times d}$ . The Sinkhorn Step is computed with the *einsum* operation along the second dimension (i.e., the  $m$ -dimension) of  $D^{P}$  and  $H^{P}$ . In intuition, the second and third considerations facilitate random permutation of the rows of the OT cost matrix.
With these considerations, the element of the  $t^{\mathrm{th}}$ -waypoint and  $i^{\mathrm{th}}$ -search directions in the OT cost matrix  $\pmb{C} \in \mathbb{R}^{T \times m}$  is the mean of probe point evaluation along a search direction (i.e., cost-to-go)
$$
\boldsymbol {C} _ {t, i} = \frac {1}{h} \sum_ {j = 1} ^ {h} \eta c \left(\boldsymbol {x} _ {t} + \boldsymbol {y} _ {t, i, j}\right) + \frac {1}{2} \| \boldsymbol {\Phi} _ {t, t + 1} \boldsymbol {x} _ {t} - \left(\boldsymbol {x} _ {t + 1} + \boldsymbol {y} _ {t + 1, i, j}\right) \| _ {\boldsymbol {Q} _ {t, t + 1} ^ {- 1}} ^ {2}, \tag {10}
$$
with the probe point  $\pmb{y}_{t,i,j} \in H^{P}$ . Then, we ensure the cost matrix positiveness for numerical stability by subtracting its minimum value. With uniform prior histograms  $\pmb{n} = \mathbf{1}_T / T$ ,  $\pmb{m} = \mathbf{1}_m / m$ , the problem  $W^{*} = \operatorname{argmin}\mathrm{OT}_{\lambda}(\pmb{n},\pmb{m})$  is instantiated and solved with the log-domain stabilization version [35, 36] of the Sinkhorn algorithm. By setting a moderately small  $\lambda = 0.01$  to balance between performance and blurring bias, the update does not always collapse towards the vertices of the polytope, but to a conservative one inside the polytope convex hull. In fact, the Sinkhorn Step defines an explicit trust region, which bounds the update inside the polytope convex hull. More discussions of log-domain stabilization and trust region properties are in Appendix E and Appendix D.
In the trajectory optimization experiments, we typically do not assume any cost structure (e.g., nonsmooth, non-convex). In MPOT, Assumption 2 is usually violated with  $T \gg m$ , but MPOT still works well due to the soft assignment of Sinkhorn distances. We observe that finer function evaluations, randomly rotated polytopes, and moderately small  $\lambda$  increase the algorithm's robustness against practical conditions. Note that these implementation technicalities do not incur much overhead due to the efficient batch computation of modern GPU.
4.3 Batch trajectory optimization
We leverage our Sinkhorn Step to optimize multiple trajectories in parallel, efficiently providing many feasible solutions for multi-modal planning problems. Specifically, we implement MPOT using PyTorch [37] for vectorization across different motion plans, randomly rotated polytope constructions, and probe set cost evaluations. For a problem instance, we consider  $N_{p}$  trajectories of horizon  $T$ , and thus, the trajectory set  $\mathcal{T} = \{\tau_1,\dots,\tau_{N_p}\}$  is the parameter to be optimized. We can flatten the trajectories into the set of  $N = N_{p}\times T$  waypoints. Now, the
Figure 2: Graphical illustration of Sinkhorn Step with practical considerations. In this point-mass example, we zoom-in one part of the discretized trajectory. The search-direction sets are constructed from randomly rotated 2-cube vertices at each iteration, depicted by the gray arrows and the green points. The gray numbers are the averaged costs over the red probe points in each vertex direction. Note that for clarity, we only visualize an occupancy obstacle cost. The red arrows describe the updates that transport the waypoints gradually out of the obstacles, depending on the (solid inner) polytope circumcircle  $\alpha_{k}$  and (dotted outer) probe circle  $\beta_{k}$ .
tensors of search directions and probe set  $D^{P} \in \mathbb{R}^{N \times m \times d}$ ,  $H^{P} \in \mathbb{R}^{N \times m \times h \times d}$  can be efficiently constructed and evaluated by the state cost function  $c(\cdot)$ , provided that the cost function is implemented with batch-wise processing (e.g., neural network models in PyTorch). Similarly, the model cost term in Eq. (9) can also be evaluated in batch by vectorizing the computation of the second term in Eq. (10).
At each iteration, it is optional to anneal the stepsize  $\alpha_{k}$  and probe radius  $\beta_{k}$ . Often we do not know the Lipschitz constant  $L$  in practice, so the optimal stepsize cannot be computed. Hence, the Sinkhorn Step might oscillate around some local minima. It is an approximation artifact that can be mitigated by reducing the radius of the ball-search over time, gradually changing from an exploratory to an exploitative behavior. Annealing the ball search radius while keeping the number of probe points increases the chance of approximating better ill-conditioned cost structure, e.g., large condition number locally.
To initialize the trajectories, we randomly sample from the discretized GP prior  $\mathcal{T}^0\sim \mathcal{N}(\pmb {\mu}_0,\pmb {K}_0)$  where  $\pmb{\mu}_{0}$  is a constant-velocity, straight-line trajectory from start-to-goal state, and  $K_{0}\in$ $\mathbb{R}^{(T\times d)\times (T\times d)}$  is a large GP covariance matrix for exploratory initialization [38, 39] (cf. Appendix B). In execution, we select the lowest cost trajectory  $\tau^{*}\in \mathcal{T}^{*}$ . For collecting a trajectory dataset, all collision-free trajectories  $\mathcal{T}^*$  are stored along with contextual data, such as occupancy map, goal state,
Algorithm 1: Motion Planning via Optimal Transport
$\mathcal{T}^0\sim \mathcal{N}(\pmb {\mu}_0,\pmb {K}_0)$  and  $\pmb {n} = \pmb {1}_N / N$ $m = 1_{m} / m$    
while termination criteria not met do (Optional)  $\alpha \leftarrow (1 - \epsilon)\alpha$ $\beta \leftarrow (1 - \epsilon)\beta$  //Epsilon Annealing for Sinkhorn Step Construct randomly rotated  $D^{P},H^{P}$  and compute the cost matrix  $C$  as in Eq. (10) Perform Sinkhorn Step  $\mathcal{T}\gets \mathcal{T} + \mathbf{S}$    
end
etc. See Algorithm 1 for an overview of MPOT. Further discussions on batch trajectory optimization are in Appendix C.
We experimentally evaluate MPOT in PyBullet simulated tasks, which involve high-dimensional state space, multiple objectives, and challenging costs. First, we benchmark our method against strong motion planning baselines in a densely cluttered 2D-point-mass and a 7-DoF robot arm (Panda) environment. Subsequently, we study the convergence of MPOT empirically. Finally, we demonstrate the efficacy of our method on high-dimensional mobile manipulation tasks with TIAGo++. Additional ablation studies on the design choices of MPOT, and gradient approximation capability on a smooth function of Sinkhorn Step w.r.t. different hyperparameter settings are in the Appendix J.
5.1 Experimental setup
In all experiments, all planners optimize first-order trajectories with positions and velocities in configuration space. The batch trajectory optimization dimension is  $N \times T \times d$ , where  $d$  is the full-state concatenating position and velocity.
For the point-mass environment, we populate 15 square and circle obstacles randomly and uniformly inside x-y limits of  $[-10, 10]$ , with each obstacle having a radius or width of 2 (cf. Fig. 1). We generate 100 environment-seeds, and for each environment-seed, we randomly sample 10 collision-free pairs of start and goal states, resulting in 1000 planning tasks. We plan each task in parallel 100 trajectories of horizon 64. A trajectory is considered successful if it is collision-free.
For the Panda environment, we also generate 100 environment-seeds. Each environment-seed contains randomly sampled 15 obstacle-spheres having a radius of  $10\mathrm{cm}$  inside the x-y-z limits of  $[-0.7, 0.7], [-0.7, 0.7], [0.1, 1.]]$ , ensuring that the Panda's initial configuration has no collisions (cf. Appendix I). Then, we sample 5 random collision-free (including self-collision-free) target configurations, resulting in 500 planning tasks, and plan in parallel 10 trajectories containing 64 timesteps.
In the last experiment, we design a realistic high-dimensional mobile manipulation task in PyBullet (cf. Fig. 4). The task comprises two parts: the fetch part and place part; thus, it requires solving two planning problems. Each plan contains 128 timesteps, and we plan a single trajectory for each planner due to the high-computational and memory demands. We generate 20 seeds by randomly spawning the robot in the room, resulting in 20 tasks.
The motion planning costs are the  $SE(3)$  goal, obstacle, self-collision, and joint-limit costs. The state dimension (configuration position and velocity) is  $d = 4$  for the point-mass experiment,  $d = 14$  for the Panda experiment, and  $d = 36$  (3 dimensions for the base, 1 for the torso, and 14 for the two arms) for the mobile manipulation experiment. As for polytope settings, we choose a 4-cube for the point-mass case, a 14-orthorplex for Panda, and a 36-orthorplex for TIAGo++. Further experiment details are in Appendix I.
Baselines. We compare MPOT to popular trajectory planners, which are also straightforward to implement and vectorize in PyTorch for a fair comparison (even if the vectorization is not mentioned in their original papers). The chosen baselines are gradient-based planners: CHOMP [7] and GPMP2 (no interpolation) [8]; sampling-based planners: RRT* [6, 10] and its informed version I-RRT* [40], Stochastic Trajectory Optimization for Motion Planning (STOMP) [9], and the recent work Stochastic Gaussian Process Motion Planning (SGPMP) [13]. We implemented all baselines in PyTorch except for RRT* and I-RRT*, which we plan with a loop using CPU. We found that resetting the tree, rather than reusing it, is much faster for generating multiple trajectories; hence, we reset RRT* and I-RRT* when they find their first solution.
2To the best of our knowledge, vectorization of RRT* is non-trivial and still an open problem.
Table 1: Trajectory generation benchmarks in densely cluttered environments. RRT* and I-RRT* success and collision-free rates depict the maximum achievable values for all planners. S and PL statistics are computed on successful trajectories only.
point-mass Experiment	Panda Experiment
T[s]	SUC[%]	GOOD[%]	S	PL	T[s]	SUC[%]	GOOD[%]	S	PL
RRT*	43.2 ± 15.2	100 ± 0.	100 ± 0.	0.43 ± 0.12	23.8 ± 4.6	186.9 ± 184.2	100 ± 0.	73.8 ± 26.7	0.17 ± 0.05	7.8 ± 2.9
I-RRT*	43.6 ± 13.8	100 ± 0.	100 ± 0.	0.43 ± 0.11	23.9 ± 4.8	184.2 ± 166.0	100 ± 0.	74.6 ± 29.0	0.17 ± 0.05	7.6 ± 3.2
STOMP	2.2 ± 0.1	31.4 ± 13.9	10.5 ± 25.7	0.01 ± 0.01	17.0 ± 1.4	4.3 ± 0.1	50.8 ± 28.3	35.3 ± 42.0	0.01 ± 0.0	4.5 ± 0.8
SGPMP	6.5 ± 0.9	98.6 ± 4.5	74.9 ± 28.9	0.03 ± 0.01	18.3 ± 2.0	5.0 ± 0.2	67.8 ± 23.5	58.1 ± 45.8	0.01 ± 0.0	4.5 ± 0.9
CHOMP	0.5 ± 0.1	70.9 ± 16.7	38.6 ± 40.7	0.03 ± 0.0	17.7 ± 1.7	3.1 ± 0.3	63.0 ± 25.5	51.6 ± 46.2	0.02 ± 0.0	4.6 ± 0.8
GPMP2	2.8 ± 0.1	98.3 ± 4.9	74.9 ± 32.1	0.07 ± 0.05	20.3 ± 3.1	3.3 ± 0.2	66.0 ± 25.2	53.2 ± 42.3	0.01 ± 0.0	4.9 ± 0.8
MPOT	0.4 ± 0.0	99.2 ± 3.1	73.6 ± 26.7	0.06 ± 0.03	19.3 ± 2.3	0.8 ± 0.1	71.6 ± 23.2	60.2 ± 44.4	0.01 ± 0.01	4.6 ± 0.9
Figure 3: Convergence analysis of MPOT in Panda benchmark. The plots show the cost convergence when applying step radius annealing  $\epsilon = 0.035$  and without. The plots imply that by following the Sinkhorn Steps, even without annealing, the cost converges exponentially (w.r.t. the update step size shown by the displacement norm). Slower convergence is observed without annealing. The right plots depict the number of iterations for solving the inner OT problem, whose stopping threshold is set at  $10^{-5}$ . The mean and median of the violin plots are shown in blue and red, respectively. This shows that later iterations require fewer OT iterations, which attributes to the efficiency of MPOT.
Metrics. Comparing various aspects among different types of motion planners is challenging. We aim to benchmark the capability of planners to parallelize trajectory optimization under dense environment settings. The metrics are  $\mathrm{T}[s]$  - planning time until convergence;  $\mathrm{SUC}[\%]$  - success rate over tasks in an environment-seed, where success means there is at least one successful trajectory found each task;  $\mathrm{GOOD}[\%]$  - success percentage of total parallelized plans in each environment-seed, reflecting the parallelization quality; S - smoothness measured by the norm of the finite difference of trajectory velocities, averaged over all trajectories and horizons; PL - path length.
5.2 Benchmarking results
We present the comparative results between MPOT and the baselines in Table 1. While RRT* and I-RRT* achieve perfect results on success criteria, their planning time is dramatically high, which reconfirms the issues of RRT* in narrow passages and high-dimensional settings. Moreover, solutions of RRT* need postprocessing to improve smoothness. For GPMP2, the success rate is comparable but requires computational effort. CHOMP, known for its limitation in narrow passages [41], requiring a small stepsize to work. This parallelization setting requires a larger step size for all trajectories to make meaningful updates, which incurs its inherent instability. With STOMP and SGPMP the comparison is "fairer," as they are both gradient-free methods. However, the sampling variance of STOMP is too restrictive, leading to bad solutions along obstacles near the start and goal configuration. Only SGPMP is comparable in success rate and parallelization quality. Nevertheless, we observe that tuning the proposal distribution variances is difficult in dense environments since they do not consider an obstacle model and cannot sample meaningful "sharp turns", hence requiring small update step size, more samples per evaluation, and longer iterations to optimize.
MPOT achieves better planning time, success rate, and parallelization quality, some with large margins, especially for the Panda experiments, while retaining smoothness due to the GP cost. We observe that MPOT performs particularly well in narrow passages, since waypoints across all trajectories are updated independently, thus avoiding the observed diminishing stepsize issue of the gradient-based planners in parallelization settings. Thanks to the explicit trust region property (cf. Appendix D), it is easier to tune the stepsize since it ensures that the update bound of each waypoint is the polytope convex hull. Notably, the MPOT planning time scales well with dimensionality. As seen in Fig. 3, solving OT is even more rapid at later Sinkhorn Steps; as the waypoints approach local minima, the OT cost matrix becomes more uniform and can be solved with only one or two Sinkhorn iterations.
5.3 Mobile manipulation experiment
We design a long-horizon, high-dimensional whole-body mobile manipulation planning task to stress-test our algorithm. This task requires designing many non-convex costs, e.g., signed-distance fields for gradient-based planners. Moreover, the task space is huge while the  $SE(3)$  goal is locally small (i.e., the local grasp-pose, while having hyper-redundant mobile robot, meaning the whole-body IK solution may be unreliable); hence, it typically requires long-horizon configuration trajectories and a small update step-size. Notably, the RRTs fail to find a solution, even with a very high time budget of 1000 seconds, signifying the environment's difficulty. These factors also add to the worst performance of GPMP2 in planning time (Table 2). Notably, CHOMP performs worse than GPMP2 and takes more iterations in a cluttered environment in Table 1. However, CHOMP beats GPMP2 in runtime complexity, in this case due to its simpler update rule. STOMP exploration mechanism is restrictive, and we could not tune it to work in this environment. MPOT achieves much better planning times by avoiding the propagation of gradients in a long computational chain while retaining the performance with the efficient Sinkhorn Step, facilitating individual waypoint exploration. However, due to the sparsity of the 36-othorplex ( $m = 72$ ) defining
Figure 4: A TIAGo++ robot has to fetch a cup from a table in a room, then put the cup back on the red shelf while avoiding collisions with the chairs.
Table 2: Mobile fetch & place.  $\mathrm{TF}[s]$  depicts the planning time for achieving first successful solution. The average S and PL are evaluated on successful trajectories only. RRT* fails to recover a solution with a very high time budget of 1000 seconds, signifying the environment difficulty.
TF[s]	SUC[%]	S	PL
RRT*	1000 ± 0.00	0	-	-
I-RRT*	1000 ± 0.00	0	-	-
STOMP	-	0	-	-
SGMPP	27.75 ± 0.29	25	0.010 ± 0.001	6.69 ± 0.38
CHOMP	16.74 ± 0.21	40	0.015 ± 0.001	8.60 ± 0.73
GPMP2	40.11 ± 0.08	40	0.012 ± 0.015	8.63 ± 0.53
MPOT	1.49 ± 0.02	55	0.022 ± 0.003	10.53 ± 0.62
the search direction bases in this high-dimensional case, it becomes hard to balance success rate and smoothness when tuning the algorithm, resulting in worse smoothness than the baselines.
Limitations. MPOT is backed by experimental evidence that its planning time scales distinctively with high-dimensional tasks in the parallelization setting while optimizing reasonably smooth trajectories. Our experiment does not imply that MPOT should replace prior methods. MPOT has limitations in some aspects. First, the entropic-regularized OT has numerical instabilities when the cost matrix dimension is huge (i.e., huge number of waypoints and vertices). We use log-domain stabilization to mitigate this issue [35, 36]. However, in rare cases, we still observe that the Sinkhorn scaling factors diverge, and MPOT would terminate prematurely. Normalizing the cost matrix, scaling down the cost terms, and slightly increasing the entropy regularization  $\lambda$  helps. Second, on the theoretical understanding, we only perform preliminary analysis based on Assumption 2 to connect directional-direct search literature. Analyzing Sinkhorn Steps in other conditions for better understanding, e.g., Sinkhorn Step gradient approximation analysis with arbitrary  $\lambda >0$ , Sinkhorn Step on convex functions for sharper complexity bounds, etc., is desirable. Third, learning motion priors [13, 24] can naturally complement MPOT to provide even better initializations, as currently, we only use GP priors to provide random initial smooth trajectories.
6 Conclusions and Broader Impacts