SlowGuess/NeurIPS_2023_OCR huggingface dataset:
Notably, we use RRT*/I-RRT* as a feasibility indicator of the environments since they enjoy probabilistic completeness, i.e., at an infinite time budget if a solution exists these search-based methods will find the plan. Optimization-based motion planners, like MPOT, GPMP2, CHOMP, and STOMP are only local optimizers. Therefore, if a solution cannot be found by RRT*/I-RRT*, then it is not possible that optimization-based approaches can recover a solution.
I.1 MPOT experiment settings
For MPOT, we apply  $\epsilon$ -annealing, normalize the configuration space limits (e.g., position limits, joint limits) into the  $[-1, 1]$  range, and do the Sinkhorn Step in the normalized space. MPOT is cost-sensitive due to exponential terms inside the Sinkhorn algorithm, hence, in practice, we normalize the cost matrix to the range  $[0, 1]$ . The MPOT hyperparameters used in the experiments are presented in Table 4.
I.2 Environments
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 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
Table 4: Experiment hyperparameters of MPOT.  $\alpha_0, \beta_0$  are the initial stepsize and probe radius.  $h$  is the number of probe points per search direction. eps is the annealing rate.  $P$  is the polytope type, and  $\lambda$  is the entropic scaling of OT problem.
Point-mass	Panda	TIAGo++
α0	0.38	0.03	0.03
β0	0.5	0.15	0.1
h	10	3	3
ε	0.032	0.035	0.05
P	d-cube	d-orthoplex	d-orthoplex
λ	0.01	0.01	0.01
$[-0.7, 0.7], [-0.7, 0.7], [0.1, 1.]]$  (cf. Fig. 6), ensuring that the Panda's initial configuration has no collisions. Then, we sample 5 random collision-free (including self-collision-free) configurations, we check with RRT* the feasibility of solutions connecting initial and goal configurations, and then compute the  $SE(3)$  pose of the end-effector as a possible goal. Thus, we create a total of 500 planning tasks and plan in parallel 10 trajectories containing 64 timesteps. To construct the GP prior, we first solve inverse kinematics (IK) for the  $SE(3)$  goal in PyBullet, and then create a constant-velocity straight line to that goal. A trajectory is considered successful when the robot reaches the  $SE(3)$  goal within a distance threshold with no collisions.
In the  $TIAGo++$  environment, 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 in total. To sample initial trajectories with the GP, we randomly place the robot's base at the front side of the table or the shelf and solve IK using PyBullet. We designed a holonomic base for this experiment. A successful trajectory finds collision-free plans, successfully grasping the cup and placing it on the shelf.
I.3 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. We tune all baselines to the best performance possible for the respective experimental settings and then set the convergence threshold and a maximum number of iterations for each planner.
In all experiments, we consider  $N_{s}$  environment-seeds and  $N_{t}$  tasks for each environment-seed. For each task, we optimize  $N_{p}$  plans having  $T$  horizon.
Planning Time. We aim to benchmark not only the success rate but also the parallelization quality of planners. Hence, we tune all baselines for each experiment, and then measure the planning time  $\mathrm{T}[s]$  of trajectory optimizers until convergence or till maximum iteration is reached.  $\mathrm{T}[s]$  is averaged over  $N_{s} \times N_{t}$  tasks.
Success Rate. We measure the success rate of task executions over environment-seeds. Specifically,  $\mathrm{SUC}[\%] = N_{st} / N_t \times 100$  with  $N_{st}$  being the number of successful task executions (i.e., having at least a successful trajectory in a batch). The success rate is averaged over  $N_s$  environment-seeds.
Parallelization Quality. We measure the parallelization quality, reflecting the success rate of trajectories in a single task. Specifically,  $\mathrm{GOOD}[\%] = N_{sp} / N_p \times 100$  with  $N_{sp}$  being the number of successful trajectories in a task, and it is averaged over  $N_s \times N_t$  tasks.
Smoothness. We measure changing magnitudes of the optimized velocities as smoothness criteria, reflecting energy efficiency. This measure can be interpreted as accelerations multiplied by the time discretization. Specifically,  $\mathbf{S} = \frac{1}{T}\sum_{t=0}^{T-1}\|\dot{\mathbf{x}}_{t+1} - \dot{\mathbf{x}}_t\|$ . S is averaged over successful trajectories in  $N_s \times N_t$  tasks.
Path Length. We measure the trajectory length, reflecting natural execution and also smoothness. Specifically,  $\mathrm{PL} = \sum_{t=0}^{T-1} \|\mathbf{x}_{t+1} - \mathbf{x}_t\|$ . PL is averaged over successful trajectories in  $N_s \times N_t$  tasks.
I.4 Motion planning costs
For the obstacle costs, we use an occupancy map having binary values for gradient-free planners (including MPOT) while we implement signed distance fields (SDFs) of obstacles for the gradient-based planners. For self-collision costs, we use the common practice of populating with spheres the robot mesh and transforming them with forward kinematics onto the task space [7, 8]. To be consistent for all planners, joint limits are enforced as an L2 cost for joint violations. Besides the point-mass experiment, all collisions are checked by PyBullet. The differentiable forward kinematics implemented in PyTorch is used for all planners.
Goal Costs. For the  $SE(3)$  goal cost, given two points  $\pmb{T}_1 = [\pmb{R}_1,\pmb{p}_1]$  and  $\pmb{T}_2 = [\pmb{R}_2,\pmb{p}_2]$  in  $SE(3)$ , we decompose a translational and rotational part, and choose the following distance as cost  $d_{SE(3)}(T_1,T_2) = \| \pmb{p}_1 - \pmb{p}_2\| +\|(LogMap(\pmb{R}_1^\top \pmb{R}_2))\|$ , where  $\mathrm{LogMap}(\cdot)$  is the operator that maps an element of  $SO(3)$  to its tangent space [73].
Collision Costs. Similar to CHOMP and GPMP2, we populate  $K$  collision spheres on the robot body (shown in Fig. 6). Given differentiable forward kinematics implemented in PyTorch (for propagating gradients back to configuration space), the obstacle cost for any configuration  $q$  is
$$
C _ {\mathrm {o b s}} (\boldsymbol {q}) = \frac {1}{K} \sum_ {j = 1} ^ {K} c (\boldsymbol {x} (\boldsymbol {q}, S _ {j})) \tag {34}
$$
with  $\pmb{x}(\pmb{q}, S_j)$  is the forward kinematics position of the  $j^{\mathrm{th}}$ -collision sphere, which is computed in batch. For gradient-based motion optimizers, we design the cost using the signed-distance function  $d(\cdot)$  from the sphere center to the closest obstacle surface (plus the sphere radius) in the task space with a  $\epsilon > 0$  margin
$$
c (\boldsymbol {x}) = \left\{ \begin{array}{l l} d (\boldsymbol {x}) + \epsilon & \text {i f} d (\boldsymbol {x}) \geq - \epsilon \\ 0 & \text {i f} d (\boldsymbol {x}) <   - \epsilon \end{array} . \right. \tag {35}
$$
For gradient-free planners, we discretize the collision spheres into fixed probe points, check them in batch with the occupancy map, and then average the obstacle cost over probe points.
Self-collision Costs. We group the collision spheres that belong to the same robot links. Then, we compute the pair-wise link sphere distances. The self-collision cost is the average of the computed pair-wise distances.
Joint Limits Cost. We also construct a soft constraint on joint limits (and velocity limits) by computing the L2 norm violation as cost, with a  $\epsilon > 0$  margin on each dimension  $i$
$$
C _ {\text {l i m i t s}} \left(q _ {i}\right) = \left\{ \begin{array}{l l} \| q _ {\min } + \epsilon - q _ {i} \| & \text {i f} q _ {i} <   q _ {\min } + \epsilon \\ 0 & \text {i f} q _ {\min } + \epsilon \leq q _ {i} \leq q _ {\max } - \epsilon . \\ \| q _ {\max } - \epsilon - q _ {i} \| & \text {e l s e} \end{array} \right. \tag {36}
$$
J Ablation Study
In this section, we study different algorithmic aspects, horizons and number of paralleled plans, and also provide an ablation on polytope choices.
J.1 Algorithmic ablations
We study the empirical convergence and the parallelization quality over Sinkhorn Steps between the main algorithm MPOT and its variants: MPOT-NoRot - no random rotation applied on the polytopes, and MPOT-NoAnnealing - annealing option is disabled. This ablation study is conducted on the point-mass experiment due to the extremely narrow passages and non-smooth, non-convex objective function, contrasting the performance difference between algorithmic options.
The performance gap between MPOT-NoRot and the others in Fig. 7 is significant. The absence of random rotation on waypoint polytopes leads to biases in the planning cost approximation due to the fixed probe set  $H^{P}$ . This approximation bias from non-random rotation becomes more prominent in higher-dimensional tasks due to the sparse search direction set. This experiment result confirms the robustness gained from the random rotation for arbitrary objective function conditions.
Figure 7: Ablation study on algorithmic choices in the point-mass environment. All planners are terminated at 100 Sinkhorn Steps. All statistics are evaluated on 1000 tasks as described in Section 5.2.
Figure 8: Planning time heatmap in seconds while varying the horizons and number of paralleled trajectories on both the point-mass (left) and the Panda (right) environments.
Between MPOT and MPOT-NoAnnealing, the performance gap depends on the context. MPOT has a faster convergence rate due to annealing the step and probe radius, which leads to a better approximation of local minima. However, it requires careful tuning of the annealing rate to avoid premature convergence and missing better local minima. MPOT-NoAnnealing converges slower and thus takes more time, but eventually discovers more successful local minima (nearly  $80\%$ ) than MPOT with annealing (cf. Table 1). This is a trade-off between planning efficiency and parallelization quality with the annealing option.
J.2 Flattening ablations
In both the point-mass and the Panda environments, we experiment with different horizons  $T$  and the number of parallel plans  $N_{p}$ . For each  $(T,N_p)$  combination, we tune MPOT to achieve a satisfactory success rate and then measure the planning time until convergence, as shown in Fig. 8. The planning time heatmap highlights the batch computation property of MPOT, resulting in a nearly symmetric pattern. Despite long horizons and large batch trajectories, the planning time remains reasonable
Table 5: Polytope ablation study on the Panda environment. All statistics are evaluated on 500 tasks as described in Section 5.2.
T[s]	SUC[%]	GOOD[%]	S	PL
MPOT-Random	2.5 ± 0.0	70.1 ± 23.7	58.3 ± 44.3	0.03 ± 0.01	4.7 ± 1.2
MPOT-Simplex	0.5 ± 0.0	65.8 ± 24.5	52.1 ± 45.3	0.01 ± 0.01	4.6 ± 1.1
MPOT-Orthoplex	0.8 ± 0.1	71.6 ± 23.2	60.2 ± 44.4	0.01 ± 0.01	4.6 ± 0.9
(under a minute) and can be run efficiently on a single GPU without excessive memory usage, making it suitable, for example, for collecting datasets for learning neural network models.
J.3 Polytope ablations
In Table 5, we compare the performance of MPOT-Orthoplex (i.e., MPOT in the Panda experiments) with its variants: MPOT-Simplex using the  $d$ -simplex vertices as search direction set  $D^{P}$ , and MPOT-Random, i.e., not using any polytope structure. For MPOT-Random, we generate 100 points on the 13-sphere ( $d = 14$  for the Panda environment) as the search direction set  $D$  for each waypoint at each Sinkhorn Step, using the Marsaglia method [74]. As expected, since the  $d$ -simplex has fewer vertices than the  $d$ -orthoplex, MPOT-Simplex has better planning time but sacrifices some success rate due to a more sparse approximation. MPOT-Random, while achieving a comparable success rate, performs even worse in both planning time and smoothness criteria. We also observe that increasing the number of sampled points on the sphere improves the smoothness marginally. However, increasing the sample points worsens the planning time in general, inducing more matrix columns and instabilities in the already large dimension cost matrix (cf. Appendix E) of the OT problem. This ablation study highlights the significance of the polytope structure for the Sinkhorn Step in high-dimensional settings.
J.4 Smooth gradient approximation ablations
We conduct an ablation on the gradient approximation of Sinkhorn Step w.r.t. different important hyperparameter settings for sanity check of Sinkhorn Step's optimization behavior on a smooth objective function. We choose the Styblinski-Tang function (cf. Fig. 9) in 10D as the smooth objective function due to its variable dimension and multi-modality for non-convex optimization benchmark [75]. We target the most important hyperparameters of polytope type  $P$ , and entropic regularization scalar  $\lambda$ . These parameters sensitively affect the Sinkhorn Step's optimization performance. We set the other important hyperparameters of step size and probe size  $\alpha = \beta = 0.1$  to be constant, the number of probing points per vertices to be 5 and turn off the annealing option for all optimization runs. The cosine similarity is defined for each particle  $x_{i} \in X$  as follows:
$$
\mathbf {C S} _ {i} = \frac {\boldsymbol {s} \left(\boldsymbol {x} _ {i}\right) \cdot \left(- \nabla f \left(\boldsymbol {x} _ {i}\right)\right)}{\| \boldsymbol {s} \left(\boldsymbol {x} _ {i}\right) \| \| - \nabla f \left(\boldsymbol {x} _ {i}\right) \|} \tag {37}
$$
Regarding this smooth objective, we observe the gradient approximation quality is consistent with Lemma 1, with increasing cosine similarities for all curves from left to right column (cf. Fig. 10). However, regarding entropic regularization scalar  $\lambda$ , we observe higher cosine similarity and lower curve variance for larger  $\lambda$ . Interestingly, this means higher  $\lambda$  induces both computational benefit solving entropic OT [19] and higher entropic smoothing bias [76], where the latter regularizes the gradient approximation directions, while it contrarily blurs the result barycenters in the barycenter problem. Notably, this Sinkhorn Step smoothing effect is more necessary in the case of  $P =$  cube toward the end of optimization (i.e., near the local minima/fixed points), where the gradients have small magnitudes and may be noisy while the Sinkhorn Step size is constant. High  $\lambda = 0.5$  (red curve) keeps high cosine similarity toward fixed points (cf. Fig. 10), while the lower/sharper  $\lambda$  exhibits degradation due to noisy random rotated 10-cube with constant-size having  $2^{10}$  vertices.
Note that the conclusion drawn from this ablation may not apply to the motion planning application in the main paper since we are evaluating the Sinkhorn Step on smooth objective functions, while the motion planning costs may have an ill-formed cost landscape. Further investigation of (sub)-gradient approximation in various objective function conditions is very interesting for future work.
Iter s 0
Iter 30
Figure 9: An example optimization run of 1000 points on the Styblinski-Tang function with Sinkhorn Step. The points are uniformly sampled at the start of optimization. This plot shows the projected optimization run in the first two dimensions.
Figure 10: Ablation study on gradient approximation with cosine similarity between Sinkhorn Step directions and true gradients. We choose the Styblinski-Tang function as the test objective function. Each curve represents an optimization run of 1000 points w.r.t to entropic regularization scalar  $\lambda$  and polytope choice (corresponding to each column), where each iteration shows the mean and variance of cosine similarity of points w.r.t their true gradients. We conduct 50 seeds for each curve, where for all seeds we concatenate the cosine similarities of all optimizing points across the seeds at each iteration.