Accelerating Motion Planning via Optimal Transport
Authors: An Thai Le
Georgia Chalvatzaki
Armin Biess
Jan Peters
Venue: NeurIPS 2023 poster (2023)
Keywords: Motion Planning
Trajectory Optimization
Optimal Transport
TL;DR: The paper proposes a highly parallelizable, efficient gradient-free update rule formulated as an Optimal Transport problem, applying to trajectory optimization for optimizing a batch of high-dimensional trajectories on multiple non-convex objectives.

Abstract: Motion planning is still an open problem for many disciplines, e.g., robotics, autonomous driving, due to their need for high computational resources that hinder real-time, efficient decision-making. A class of methods striving to provide smooth solutions is gradient-based trajectory optimization. However, those methods usually suffer from bad local minima, while for many settings, they may be inapplicable due to the absence of easy-to-access gradients of the optimization objectives. In response to these issues, we introduce Motion Planning via Optimal Transport (MPOT)---a \textit{gradient-free} method that optimizes a batch of smooth trajectories over highly nonlinear costs, even for high-dimensional tasks, while imposing smoothness through a Gaussian Process dynamics prior via the planning-as-inference perspective. To facilitate batch trajectory optimization, we introduce an original zero-order and highly-parallelizable update rule----the Sinkhorn Step, which uses the regular polytope family for its search directions. Each regular polytope, centered on trajectory waypoints, serves as a local cost-probing neighborhood, acting as a \textit{trust region} where the Sinkhorn Step ``transports'' local waypoints toward low-cost regions. We theoretically show that Sinkhorn Step guides the optimizing parameters toward local minima regions of non-convex objective functions. We then show the efficiency of MPOT in a range of problems from low-dimensional point-mass navigation to high-dimensional whole-body robot motion planning, evincing its superiority compared to popular motion planners, paving the way for new applications of optimal transport in motion planning.

Accelerating Motion Planning via Optimal Transport
An T. Le', Georgia Chalvatzaki 1.3,5 Armin Biess, Jan Peters 1-4
'Department Computer Science, Technische Universitat Darmstadt, Germany Geerman Research Center for AI (DFKI) "Hessian.AI *Centre for Cognitive Science CCenter for Mind, Brain and Behavior, Uni. Marburg and JLU Giessen, Germany
Abstract
Motion planning is still an open problem for many disciplines, e.g., robotics, au- tonomous driving, due their need for high computational resources that hinder real-time, efficient decision-making A class of methods striving to provide smooth solutions is gradient-based trajectory optimization. However, those methods usually suffer from bad local minima, while for many settings, they may be inapplicable due to the absence of easy-to-access gradients of the optimization objectives. In response to these issues, we introduce Motion Planning via Optimal Transport (MPOT)-a gradient-free method that optimizes batch of smooth trajectories over highly nonlinear costs, even for high-dimensional tasks, while imposing smooth- ness through a Gaussian Process dynamics prior via the planning caauningg-s-infeenn as-inference perspective. To facilitate batch trajectory optimization, we introduce an original zero-order and paahlly-prallllizablle update rule rule-the Sinkhorn Step, which uses the regular polytope family for its search directions. Each regular polytope, cen- tered on trajectory waypoints, serves as a local cost-probing neighborhood, acting as trust region where the Sinkhorn Step "transports" local waypoints toward low-cost regions. We theoretically show that Sinkhorn Step guides the optimizing parameters toward local minima regions of non-convex objective functions. We then show the efficiency of MPOT in a range of problems from low-dimensional point-mass navigation evincing its superiority compared to popular motion planners, paving the way for new applications of optimal transport in motion planning.
high-dimensional whole-body robot motion planning,
1 Introduction
Motion planning is fundamental problem for various domains, spanning robotics [1], autonomous driving [2], spaelisatellite swarm [3], protein docking [4]. etc., aiming to find feasible, smooth, and collision-free paths from start-to-goal configurations. Motion planning has been studied both as compling-based search [5, 6] and as an optimization problem [7-9]. Both approaches have to deal with the complexity of high-dimensional configuration spaces, e.g., when considering high-degrees of freedom (DoF) robots, the multi-modality of objectives due to multiple constraints at both con- figuration and task space, and the requirement for smooth trajectories that low-level controllers can effectively execute. Sampling-basee and use different search techniques to find feasible and optimal path [6. 10, 5), but suffer from the complex sampling process and the need for large computational budgets to provide a solution, which increases w.r.t. the complexity of the problem (c.g.. highly redundant robots and narrow pas- sages) [11]. Optimization-be based approaches work on trajectory level, optimizing initial trajectory samples either via covariant gradient descent [7, 12] or through probabilistic inference [9, 8, 13). Nevertheless, as with every optimization pipeline, trajectory optimization depends on initialization and can get trapped in bad local minima due the non-convexity of complex objectives. Moreover, in some problem settings, objective gradients are unavailable or expensive to compute. Indeed, trajectory optimization is difficult tune and is often avoided in favor of sampling-base methods with probabilistic completeness. We refer to Appendix H for an extensive discussion of related works.
methods sample the high-dimensional manifold configurations
37th Conference on Neural Information Processing Systems (NcurIPS 2023).

---

Sinkhorn step:
Sinkhom step:
Sinkhorn stop:
Sinkhorn step:
Figure Example of MPOT in the multimodal planar navigation scenario with three different goals. For each goal, we sample five initial trajectories from a GP prior. We illustrate four snapshots of our proposed Sinkhorn Step that updates a batch of waypoints from multiple trajectories over multiple goals. For this example, the total planning time was 0.12s. More demos can be found on https://sites www.eeeggggglleeeeeeeeeeeooooeee com/ cem/view/ sinkhorn-sten step/
To address these issues of trajectory optimization, we propose zero-order, fast, and highly paral- lelizable update rule-the Sinkhorn Step. We apply this novel update rule in trajectory optimization, resulting in Motion Planning via Optimal Transport (MPOT) a gradient-free trajectory optimiza- tion method optimizing a batch of smooth trajectories. MPOT optimizes trajectories by solving a sequence of entropic-regularized Optimal Transport (OT) problems, where each OT instance is solved efficiently with the celebrated Sinkhorn- Knopp algorithm [14]. In particular, MPOT discretizes the trajectories into waypoints and structurally probes a local neighborhood around each of them, which effectively exhibits a trust region, where it "transports" local waypoints towards low-cost areas given the local cost approximated by the probing mechanism. Our method is simple and does not require computing gradients from cost functions propagating over long kinematics chains. Crucially, the planning-as-inie annifereneee perspective [15, 13] allows us impose constraints related to transition dynamics as planning costs, additionally imposing smoothness through a Gaussian Process (GP) prior. Delegating complex constraints the planning objective allows to locally resolve trajectory update as an OT problem at each iteration, updating the trajectory waypoints towards the local optima, thus effectively optimizing for complex cost functions formulated in configuration and task space. We also provide a preliminary theoretical analysis of the Sinkhorn Step, highlighting its core properties that allow optimizing trajectories toward local minima regions.
Further, our empirical evaluations on representative tasks with high-dimensionality and multimodal planning objectives demonstrate an increased benefit of MPOT, both in terms of planning time and success rate, compared to notable trajectory optimization methods. Moreover, we empirically demonstrate the convergence MPOT 7-DoF robotic manipulation setting, showcasing a fast convergence of MPOT, reflected also in its dramatically reduced planning time w.r.t. baselines. The latter holds even for 36-dimensional, highly redundant mobile manipulation systems in long-horizon fetch and place tasks (cf. Fig. 4).
Our contribution is twofold. (i) We propose the Sinkhorn Step - an efficient zero-order update rule for optimizing batch of parameters, formulated barycentric projection of the current points to the polytope vertices. (ii) We, then, apply the Sinkhorn Step to motion planning, resulting in a novel trajectory optimization method that optimizes batch of trajectories by efficiently solving sequence of linear programs. It treats every waypoint across trajectories equally. enabling fast batch updates of multiple trajectories- jejjectores-wayyointee over multiple goals by solving single OT instance while retaining smoothness due to integrating the GP prior as cost function.
2 Preliminaries
Entropic-regularized optimal transport. We briefly introduce discrete OT. For a thorough intro- duction, we refer to [16-18]. Notation. Throughout the paper, we consider the optimization
d-dimensional Euclidean space Rdd representing the parameter space (e.g., system state space). 1a is the vector of ones in Rd. The scalar product for vectors and matrices is x, E Rd (r.u) = Ei Tayh and A, B E RRdxd B) Ei A.j Bij. respectively. ||:Il the 12-norm, and I1-11 de- notes the Mahalanobis norm w.r.t. some positive definite matrix M > 0. For two histograms E Σn and m E Σm in the simplex Σg = {x E R$ xT1, = 1}, we define the set Un, ( gaxm W1m n, WT1, m) containing m matrices with row

---

and column sums n and m respectively. Correspondingly, the entropy for A E Unn m) is defined
as (A) Σnj=1 ai aog. azj.
Let C E 1 be the positive cost matrix, the OT between n and m given cost C' is OT m) := (nn C). Traditionally, OT does not scale well with high dimen- sions. To address this, Cuturi [19] proposes to regularize its objective with an entropy term, resulting the entropic-regularized OT
xn, m) == minwee((..) (W.C) -HWW).
Solving (I) with Sinkhom-Knnop [19] has a complexity of (nn//=*) [20], where is the approxima- tion enror w.r.t. the original OT. Higher À enables faster but "blurry" solution, and vice versa.
Trajectory optimization. Given a parameterized trajectory by a discrete set of support states and control inputs 7 [oo -o- ur u1 xr]", trajectory optimization aims to find the optimal trajectory T" which minimizes an objective function c(T). with X, being the start state. Standard motion planning costs, such as goal cost Cg defined as the distance to desired goal-state zgg, obstacle avoidance cost Cobs: and smoothness cost Cam can be included in the objective. Hence, trajectory optimization can be expressed as the sum of those costs while obeying the dynamics constraint
arg min [Cobs (T) + cg (T, xg) + Cam (T)] s.t. a fxx, u) and T() -0-
For many manipulation tasks with high-DoF robots, this optimization problem is typically highly non- linear due to many complex objectives and constraints. Besides Coba. Cem is crucial for finding smooth trajectories for better tracking control. Covariant Hamiltonian Optimization for Motion Planning (CHOMP) [7] designs finite difference matrix M resulting to the smoothness cost Cam = T MM.. This smoothness cost can be interpreted as a penalty on trajectory derivative magnitudes. Mukadam et al. [8] generalizes the smoothness cost by incorporating a GP prior as cost via the planning-as ass inference perspective [15, 21], additionally constraining the trajectories to be dynamically smooth. Recently, an emergent paradigm of multimodal trajectory optimization [22, 23, 13, 24] is promising for discovering different modes for non-convex objectives, thereby exhibiting robustness against bad local minima. Our work contributes to this momentum by proposing an efficient batch update-rule for vectorizing waypoint updates across timesteps and number of plans.
Sinkhorn Step
To address the problem of batch trajectory optimization in gradient-free setting, we propose Sinkhorn Step-a zero-order update rule for a batch of optimization variables. Our method draws inspiration from the free-support barycenter problem [25], where the mean support of a set of empirical measures is optimized w.r.t. the OT cost. Consider an optimization problem with some objective function without easy access to function derivatives. This barycenter problem can be utilized as a parameter update mechanism, i.e., by defining set of discrete target points (i.e., local search directions) and a batch of optimizing points as two empirical measures, the barycenter of these empirical measures acts as the updated optimizing points based on the objective function evaluation at the target points.
With these considerations in mind, we introduce Sinkhorn Step, consisting of components: polytope structure defining the unbiased search-direction bases, and a weighting distribution for eval- uating the search directions. Particularly, the weighting distribution has row-column unit constraints and must be efficient to compute. Following the motivation of [25], the entropic-regularizedd OT fits nicely into the second component, providing a solution for the weighting distribution as an OT plan, which is solved extremely fast, and its solution is unique [19]. In this section, we formally define Sinkhorn Step and perform a preliminary theoretical analysis to shed light on its connection to directional -itert search methods [26, 27], thereby motivating its algorithmic choices and practical implementation proposed in this paper.
3.1 Problem formulation
We consider the batch optimization problem
min f(X) x
= min Σ f(x;),
(3)
X
1=1
where X = { is a set of n optimizing points, f Rdd - R is non-convex, differentiable, bounded below, and has L-Lipschitz gradients.
3

---

Assumption 1. The objective f is smoothh with L > 0
| (xr) ( < L 1| 10yl yll, Va, y E IR and bounded below by (() 2 f. E R, Va E I
Throughout the paper, we assume that function evaluation is implemented batch-wise and is cheap to compute. Function derivatives are either expensive or impossible to compute. At the first iteration, we sample set of initial points Xu ~ Do. with its matrix form Xu E I from some prior distribution Du- The goal is to compute batch update for the optimizing points, minimizing the objective function. This problem setting suits trajectory optimization described in Section 4.
3.2 Sinkhorn Step formulation
Similar to directional-direct search, Sinkhorn Step typically evaluates the objective function over a seachh-irectionn-eet D, ensuring descent with sufficiently small stepsize. The search-direction-set typically& a vector-set requiring to be positive spanning set [28], i.e., its conic hull Rd {E, w;d;; d; E D. wi 2 0}, ensuring that every point (including the extrema) in R is reachable by a sequence of positive steps from any initial point.
Regular Polytope Search-Directions. Consider a (d - -niit hypersphere sd-1 = {ar E Rd
| 11 with the center at zero.
Definition (Regular Polytope Scarch-Directions). Let us denote the regular polytope family
P = {simplex, orthoplex, hypercube). Consider a d-dimensional polytope P E with m vertices, the search-direction set DP = {d; | = 1] is constructed from the vertex set of the regular polytope P inscribing 5d-1
The d-dimensional regular polytope family P has all of its dihedral angles equal and, hence, is an unbiased sparse approximation of the circumseribed (d ( 1)-spheee, i.e., Σ, d: = 0, ||d,|| = 1 Vi. There also exist other regular polytope families. However, the regular polytope types in P exist in every dimension (cf. [29]). Moreover, the algorithmic construction of general polytope is not trivial [30]. Vertex enumeration for P is straightforward for vectorization and simple to implement, which we found to work well in our settings-see also Appendix F. We state the connection between regular polytopes and the positive spanning set in the following proposition.
Proposition 1. VP P, DP forms positive spanning set.
This property ensures that any point R x= w: w; by a positively weighted sum of the set of directions defined by the polytopes.
0, d, DP can be represented
Batch Update Rule. At iteration k. given the current optimizing points X5 and their matrix form X5 E Raadd we first construct the direction set from a chosen polytope P. and denote the direction set DP Rmxd in matrix form. Similar to [25], let us define the prior histograms reflecting the importance of optimizing points n E Σn and the search directions m E Em, then the constraint space U Unn m) of OT is defined. With these settings, we define Sinkhorn Step.
Definition 2 (Sinkhorn Step). The batch update rule is the barycentric projection (Remark 11, [17]) that optimizes the free-support barycenter of the optimizing points and the batch polytope vertices
X = Xx + St, S₁ = andiaggn (n) " S.I. W argmin (W, C) XH(W),
(4)
with og > 0 as the stepsize, C E RR= Ct2- = f(x; + agdj), x, E Xk. dj E DP is the local objective matrix evaluated at the finear-translated polytope verfices.
Observe that the matrix diag(n) -1 - has n row vectors in the simplex Em.. The batch update transports X to barycenter shaping by the polytopes with weights defined by the optimal solution W However, in contrast with the barycenter problem [25], the target measure supports are constructed locally at each optimizing point, and, thus, the points are transported in accordance with their local search directions. By Proposition DP is positive spanning set, thus, W forms a generalized barycentric coordinate, defined w.r.t. the finite set of polytope vertices. This property implies any point in Rd can be reached by a sequence of Sinkhorn Steps. For the d-simplex case, any point inside the convex hull can be identified with unique barycentric coordinate [31], which is not the case for d-orthoplex or d-cube. However, coordinate uniqueness is not required for our analysis in this paper, given the following assumption.

---

At any iteration k > 0, the prior histogram on the optimizing points and the search- direction set is uniform n m 1n/n n, having the same dimension n m. Additionally, the entropic scaling approaches zero X + 0.
Assumption 2.
Assuming uniform prior importance of the optimizing points and their search directions is natural since, in many cases, priors for stepping are unavailable. However, our formulation also suggests conditional Sinkhorn Step, which is interesting study in future work. This assumption allows performing an analysis on Sinkhorn Step on the original OT solution.
With these assumptions, we can state the following theorem for each ay X5 separately, given that they follow the Sinkhorn Step rule.
Theorem 1 (Main result). If Assumptiio and Assumption 2 hold and the stepsize is sufficiently small 0% = a with 0 < a < 2ppe/ I, then with a sufficient number of iterations
K k(e) = f(20) -fo 1
(5)
(upe
Le 2 )
we have minosksK [f S e, VIx E Xn.
Note that we do not make any additional assumptions on f besides the smoothness and boundedness, and the analysis is performed non-convex settings. Theorem only guarantees that the gradients of some points in the sequence of Sinkhorn Steps are arbitrarily small, i.e., in the proximity of local minima. If in practice,
implement the sufficient decreasing condition f(zg) f(*++1) 2 caf.
then ((*x) S (ax),, ||V [(x:)] S e holds. However, this sufficient decrease check may waste some iterations and worsen the performance. We show in the experiments that the algorithm empirically exhibits convergence behavior without this condition checking. If J. is known, then we can compute pPe/LL L, leading to the complexity bound ket) = 2L(f(mo)
1. 1r,.2 Therefore, the complexity bounds for d-simplex, d-orthoplex and d-cube are Oed O(d/e), and O(1/²),, respectively. The d-cube case shows the same complexity bound O(1/e²) as the well- known gradient descent complexity bound on the L-smooth function [32]. These results are detailed in Appendix A. Generally, we perform preliminary study on Sinkhorn Step with Assumption and Assumption 2 to connect the well-known directional-direc search literature [26, 27], as many unexplored theoretical properties of Sinkhorn Step remain in practical settings described in Section 4.
the optimal stepsize
Motion Planning via Optimal Transport
Here, we introduce MPOT a method that applies Sinkhorn Step to solve the batch trajectory optimization problem, where we realize waypoints in a set of trajectories as optimizing points. Due to Sinkhorn Step's properties, MPOT does not require gradients propagated from cost functions over long kinematics chains. It optimizes trajectories by solving a sequence of strictly convex linear programs with a maximum entropy objective (ef. Definition 2), smoothly transporting the waypoints according to the local polytope structure. To promote smoothness and dynamically feasible trajectories, incorporate the GP prior
via the lranning-as-infeeenn perspective.
4.1 Planning As Inference With Empirical Waypoint Distribution
Let us consider general discrete-time dynamics X = Fxx, U), where X = [xo....., Xy] denotes the states sequence, U [uo, ..., UT] is the control sequence, and xo is the start state. The target distribution over control trajectories U can be defined as the posterior [33]
(U)) = z exp exp (-)) (U),
with E(U) the energy function representing control cost, qo (U) = NO Σ) a zero-mean normal prior, 1) scaling term (temperature), and Z the normalizing scalar.
Assuming a first-order trajectory optimization', the control sequence can be defined as a time- derívative of the states U = [No. .... Xr]. The larget posterior distribution over both state-trajectories and their derivatives T [X, U) = {z E Rdd 2t = [x,, 81177 is defined as
q" (T) = 7 exp ( (c((()
We describe first-order formulation for simplicity. However, this work can be extended to second-order systems similar to [8].
5

---

which is similar to Eq. (6) with the energy function E = co co U) being the composition of the cost c over T and the dynamics F. The dynamics is also now integrated into the prior distribution (r(r). The absorption of the dynamics into the prior becomes evident when we represent the prior as a 7ero-mean vonstant-velocity GP prior gr(T) = (0, K), with constant time-discretization At and the time-correlated trajectory covariance K, as described in Appendix B.
Now, to apply Sinkhorn Step, consider the trajectory we want to optimize T { ) we can define the proposal trajectory distribution as a waypoint empirical distribution
p(æ; T) Σ [(tt)p(z1t)
Σ nióx, (x),
(8)
with the histogram n [m1..... nel, p(() = n4 = 1/T, and 6z, the Dirac on waypoints 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., kinematies) correlation between waypoints. This assumption is also seen in [7, 9] and can be applied in 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 deferred for future work.
Following the planning-as- as-fferrnce perspective, the motion planning problem can be formulated as the minimization of a Kullback-Leible (KL) divergence between the proposal trajectory distribution p(æ; 7) and the target posterior distribution Eq. (7) (i.e., the I-projection)
r" = argmin {KL (p(æ; T) 11 (T) = Ep [log (or) - Hp))
1
argmin E.p
ne(() +
| 12x - -log Z
2
T-1
argmin T
11 c (1)
o 1.1+1 Ix+1 1
t= stale cust
transition model cost
with Pr,--1 the state transition matrix, and Qt.++1 the covariance between time steps and t + 1 originated from the GP prior (cf. Appendix B), and the normalizing constant of the tar- get posterior Z is absorbed. Note that the entropy of the empirical distribution is constant (p) = - S # Σt= 6e, (xx) log p(æ;T) = 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 general graphical model objective [34] is vital for future work. We apply Sinkhorn Step to Eq. (9) by realizing the trajectory as batch of optimizing points T E Rxx 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-pre projetttoooection 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 imple- mentation for optimizing a trajectory T. First, we define 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 outwards till reaching a probe radius 9 2 ag, resulting the probe set H² with its matrix form H" E Rmxhxd with h probe points for each direction (cf. Fig. 2). Second, we add stochasticity the search directions by applying a random d-dimensional rotation R E (O( to the polytopes to promote local exploration (computation of R E SO(d) is discussed in Appendix G). Third, 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 DP E RTxxxxd Consequently, the probe set is also constructed in batch for every waypoint Hº RTxxxxxdd The Sinkhorn Step is computed with the einsum operation along the second dimension (i.e., the m-dimension) of Dd and H 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 way-oott and -eh-searhh directions in the OT cost matrix C' R is the mean of probe point evaluation along a search direction (i.e., cost-to-go)
1 C₁5 T Ei= ne(wr + + 2ll P1,41 (ai+1 + 31+1,55)
w....) +
(10)
with the probe point Vt,i.j E H.. Then, we ensure the cost matrix positiveness for numerical stability by subtracting its minimum value. With uniform prior histograms n 1r/TT m 1m 1m/m, the problem W = argmin (n, m) is instantiated and solved with the log domain stabilization version [35, 36] of the Sinkhorn algorithm. By setting moderately small À 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 Step defines an explicit trust region, which bounds the update inside the polytope convex hull. More discussions of log-dmmain stabilization and trust region properties are in Appendix E and Appendix D. In the trajectory optimization experiments, we typ- ically do not assume any cost structure (e.g., non- smooth, non-convex). In MPOT, Assumption 2 is usually violated with T > 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 À increase the algorithm's 's robustness against practical condi- tions. Note that these implementation technicalities do not incur much overhead due to the efficient batch computation of modern GPU.
inside the polytope convex hull. In fact, the Sinkhorn
Figure 2: Graphical illustration of Sinkhorn Step with practical considerations. In this point mast ex- ample, we zoom-in one part of the discretized tra- jectory. The search-direction sets are constructed from randomly rotated 2-cube vertices at each it- eration, 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 occu- pancy obstacle cost. The red arrows describe the updates that transport the waypoints gradually out of the obstacles, depending on the (solid inner) polytope circumcircle ag and (dotted outer) probe circle /3k-
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 prob- lems. Specifically, we implement MPOT using Py- Torch [37) for vectorization across different motion plans, randomly rotated polytope constructions, and probe set cost evaluations. For a problem instance, we consider N₂ trajectories of horizon T, and thus, the trajectory set T {TI,.... JN2 J is the param- eter to be optimized. We can flatten the trajectories into the set of N N x T waypoints. Now, the tensors of search directions and probe set D E R xxmxd H Rg-xxxxxx can be efficiently constructed and evaluated by the state cost function c(-), provided that the cost function is imple- mented 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 ag and probe radius Sr. 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 T ~ N( Ko). where Mo is constant-velocity, straight-line trajectory from start-to-goal state, and Ko RT xX)xx(xxx xd) is large GP covariance matrix for exploratory initialization [38, 39] (cf. Appendix B). In execution, we select the lowest cost trajectory T' E T", For collecting a trajectory dataset, all collision-free trajectories T are stored along with contextual data, such as occupancy map, goal state,
7

---

Algorithm 1: Motion Planning via Optimal Transport
T ~ N(po Ko) and nt = 1N/ /N, m 1m/m while termination criteria not met do
(Optional) a <- (1 -))x,, 3 4- (1 1-c -c)3
// Epsilon Annealing for Sinkhorn Step
Construct randomly rotated D" H² and compute the cost matrix C' as in Eq. (10)
Perform Sinkhorn Step T <- T +
end
etc. See Algorithm for an overview of MPOT. Further discussions on batch trajectory optimization in Appendix C.
Experiments
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- nigh-dimensional mobile manipulation tasks with TIAGo+ Additional ablation studies on the design choices of MPOT, and gradient approximation capability on smooth function of Sinkhorn Step w.r.t. different hyperparameter settings are in the Appendix
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 x T' x 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 radius or width of (cf. Fig. 1). We generate 100 cavironment-seeds. and for each environment-seed we randomly sample 10 collision- free pairs of start and goal states, resulting 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 radius of 10cm inside the x-y-z limits of 0.7,,, 0.7], 1.]] ensuring that the Panda's initial configuration has no collisions (cf. Appendix 1). Then, we sample 5 random collision-free (including self-collision-ffee) target configurations, resulting in 500 planning tasks, and plan parallel 10 trajectories containing 64 timesteps.
In the last experiment, we design realistic high- digeesinnni 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 the SE(3) goal, obstacle, self-collision, and 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, 14-othorplex for Panda, and a 36-othorplex 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 fair comparison (even if the vectorization is not mentioned 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.
TTo the best of our knowledge, vectorization of RRT" is non-trivial and still an open problem.
8

---

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 Expesiment
Panda Experiment
SUC|R]
(OOD|5]
PL
Tl:
SUC|R]
GOOD
RRT
48.2
15.2 +-0.
0.43
258
186.9
164.2
100 +-0
26.7
0.17 0.05
7. +229
IRRT"
43.8 13.8
1.0.
0.
1.0.1
184.2 106.0
1.0.
7.6 1.32
STUMP
22 2:01
31.4 13.9
10.5 26.7
0.01 +0001
17.0 #14
4.3 #:0.1
tau 25-3
35.3 42.0
0.01 =1.11
4.5 4.8
SCIPMP
6.5
08.6 + 4.5
74.9 28.9
0.0 +0001
18.3 2.0
6.0 +.0.2
67.8 23.5
08.1
0.01 0.0
4.5
CROMP
1:03
25.5
46.2
46 16.108
GPMP2
2.8
0. #02
25.2
0.01
4.9
MPOT
0.4 1 0.0
09.2 3.1
73.6 26.7
0.06 0.00
19.3 2.3
0.8 0.1
71.6 1 20.2
60.2 61.4
0.01 1.001
0.9
Eoi: Utpda Nora
120
Dpplicener frm
Saiil Siren
0.3
90
10
20 130 150
291
(a)
(b)
Figure 3: Convergence analysis of MPOT in Panda benchmark. The plots show the cost convergence when applying step radius annealing e = 0.035 and without. The plots imply that by following the Sinkhorn Steps, even without anncaling. the cost converges exponentially (w.rt. 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 set 10 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.
Comparing various aspects among different types of motion planners is challenging. We aim benchmark the capability of planners to parallelize trajectory optimization under dense environment settings. The metrics are T[s] planning time until convergence; SUC[%] ssuccess rale over tasks an environment-seedt where success means there is at least one successful trajectory found each task; GOOD[%] success percentage of total parallelized plans in each environment-seed reflecting the parallelization quality; smootthess measured by the norm of the finite difference of trajectory velocities, averaged over all trajectories and horizons; PL path length.
Metrics.
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 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- frddeent-fre methods. However, the sampling variance of STOMP 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 mar- gins, 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-basee based planners in parallelization settings. Thanks to the explicit trust region property (cf. Appendix D), it is easier to tune the stepsize since 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 two Sinkhorn iterations.

---

5.3 Mobile manipulation experiment
We design long-horizon high-din sigh-dimensional whole- body mobile manipulation planning task to tress--est 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 (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 very high time budget of 1000 seconds, signifying the environment's ' difficulty. These fac- tors 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 mech- anism is restrictive, and could not tune it to work in this environment. MPOT achieves much better planning times by avoiding the propagation of gradi- ents in a long computational chain while retaining the performance with the efficient Sinkhorn Step, facili- tating individual waypoint exploration. However, due to the sparsity of the 36-othorpeex (m = 72) defining
Figure 4: A TIAGo++ robot has to fetch a cup from table in a room, then put the cup back on the red shelf while avoiding collisions with the chairs.
Table 2:
Mobile fetch & place. TF[s] depicts the planning time for achieving first successful solu- tion. The average S and PL are evaluated suc- cessful trajectories only. RRT" fails recover a solution with a very high time budget of 1000 sec- onds, signifying the environment difficulty.
TF
SUC
RRT
1000
0.00
ERRT"
1000 a 0.00
STOMP
SGPMP 27.75 1 0.20
25
0.010 1 0.001
6.69 0.38
CHOMP 16.74 4 0.21
40
0.015
1: 0.0001
8.00 0.73
GPMP2 40.11
0.08
40
0.012
0.015
8.63 + 0.53
MPOT 1.49 0.02
0,022
0.00
10.03=
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 the parallelization setting while optimizing reasonably smooth tra- jectories. Our experiment does not imply that MPOT should replace prior methods. MPOT has limitations in some aspects. First, the entropii-regularizeed 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 À 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 À >> O, 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
We presented MPOT- gradient-free and efficient batch motion planner that optimizes multiple nigh-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 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 sampling methods or variational inference.
10

---

Acknowledgments and Disclosure of Funding
An T. Le was supported by the German Research Foundation project METRICAIMITATION (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 Julen Urain for the insightful and fruitful discussions; Snehal Jauhri, Puze Liu, and João Carvalho for their help in setting up the TIAGo+ environments.
References
[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 Intelligent Trans- portation Systems, 2 (22))1882-1848, 2019.
[3] Dario Iz70 and Lorenzo Pettazzi. Autonomous and distributed motion planning for satellite swarm, Journal of Guidance, Control, and Dynamics, 30(2):449 2007.
[4] Ibrahim Al-Bluwi, Thierry Siméon, and Juan Cortés. Motion planning algorithms for molecular simulations: A survey. Computer Science Review, 1(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 Kuffner and Steven M Valaee.. 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-8)) 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 Dragan, Nathan Ratliff, and Siddhartha 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] Julen Urain, An T Le, Alexander Lambert, Georgia Chalvatzaki, Byron Boots, and Jan Peters. Learning implicit priors for motion optimization. In 2022 IEEE/RSI 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-33 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 Peyré, Marco Cuturi, et al. Computational optimal transport: With applications to data science. Foundations and Trendy in Machine Learning, 11(5-6)3355-67, 2019.

---

[18] Alessio Figalli and Federico Glaudo. An invitation to optimal transport, Wasserstein distances, and gradient flows. EMS Press, 2021.
[19] Marco Cuturi. Sinkhorn distances: Lightspeed computation of optimal transport. Advances in neural information processing systems, 26, 2013.
[20] Jason Altschuler, Jonathan Weed, and Philippe Rigollet. Near-linear time approxima- tion algorithms for optimal transport via sinkhorn iteration. Advances in neural information processing systems, 30, 2017.
[21] Mare Toussaint. Newton methods for k-order markov constrained motion problems. arXiv preprint arXiv:1407.0414: 2014.
[22] Takayuki Osa. Multimodal trajectory optimization for motion planning. The International Journal of Roboties Research, 39(8):983-1000 2020.
[23] Alexander Lambert, Adam Fishman, Dieter Fox, Byron Boots, and Fabio Ramos. Stein variational model predictive control. arXiv preprint 1.074120 2020.
[24] Joao Carvalho, An T Le, Mark Baierl, Dorothea Koert, and Jan Peters. Motion planning diffusion: Learning and planning of robot motions with diffusion models. arXiv preprint arXiv:2308.01557 2023.
[25] Marco Cuturi and Arnaud Doucet. Fast computation of wasserstein barycenters. In International conference on machine learning, pages 685-693. PMLR, 2014.
[26] Robert Hooke and Terry A Jeeves. "direct search"solution of numerical and statistical problems. Journal of the ACM (JACM), 8(2):212-22, 1961.
[27] Jeffrey Larson, Matt Menickelly, and Stefan M Wild. Derivative-free optimization methods. Acta Numerica, 28:287 284287-404, 2019.
[28] Rommel Regis. On the properties of positive spanning sets and positive bases. Optimization and Engineering, 17(1):229-266 2016.
[29] Harold Scott Macdonald Coxeter. Regular polytopes. Courier Corporation, 1973.
[30] Volker Kaibel and Marc E Pfetsch. Some algorithmic problems in polytope theory. In Algebra, geometry and software systems, pages 23-47, Springer, 2003.
[31] James R Munkres. Elements of algebraic topology. CRC press, 2018.
[32] Guillaume Garrigos and Robert M Gower. Handbook of convergence theorems for (stochastic) gradient methods. arXiv preprint 1rXiv:2301.1125,, 2023.
[33] Grady Williams, Nolan Wagener, Brian Goldfain, Paul Drews, James M Rehg, Byron Boots, and Evangelos A Theodorou. Information theoretic mpe for model-based reinforcement learning. In 2017 IEEE International Conference on Robotics and Automation (ICRA), pages 1714-1721. IEEE, 2017.
[34] Martin J Wainwright, Michael Jordan, et al. Graphical models, exponential families, and variational inference. Foundations and Trends@ in Machine Learning, 1(1-2):1-305, 2008
[35] Lenaic Chizat, Gabriel Peyré, Bernhard Schmitzer, and François-Xaviis Xavier Vialard. Scaling algorithms for unbalanced optimal transport problems. Mathematics of Computation, 87(314): 2563-2660 2609, 2018
[36] Bernhard Schmitzer. Stabilized sparse scaling algorithms for entropy regularized transport problems. SIAM Journal on Scientific Computing, 41(3):11433-11481, 2019.
[37] Adam Paszke, Sam Gross, Francisco Massa, Adam Lerer, James Bradbury, Gregory Chanan, Trevor Killeen, Zeming Lin, Natalia Gimelshein, Luca Antiga, Alban Desmaison, Andreas Köpf, Edward Yang. Zach DeVito, Martin Raison, Alykhan Tejani, Sasank Chilamkurthy, Benoit Steiner, Lu Fang, Junjie Bai, and Soumith Chintala, Pytorch: An imperative style, reghhperformance deep learning library, 2019.
12

---

[38] Jan Peters, Katharina Mulling, and Yasemin Altun. Relative entropy policy search. In Twenty- Fourth AAAI Conference on Artificial Intelligence, 2010.
[39] John Schulman, Sergey Levine, Pieter Abbeel, Michael Jordan, and Philipp Moritz. Trust region policy optimization. In International conference machine learning, pages 1889-1897. PMLR, 2015.
[40] Jonathan D Gammell, Siddhartha S Srinivasa, and Timothy D Barfoot. Informed rr: Optimal sampling-bel path planning focused via direct sampling of admissible ellipsoidal heuristic. In 2014 IEEE/RS/ International Conference on Intelligent Robots and Systems, pages 2997- 3004. IEEE, 2014.
[41] Matt Zucker, Nathan Ratliff, Anca D Dragan, Mihail Pivtoraiko, Matthew Klingensmith, Christopher M Dellin, J Andrew Bagnell, and Siddhartha S Srinivasa. Chomp: Covariant hamiltonian optimization for motion planning. The International Journal of Robotics Research, 1193, 2013.
[42] Jakub Konečný and Peter Richtárik. Simple complexity analysis of simplified direct search. arXiv preprini arXiv: 2014.
[43] Garrett Birkhoff. Tres observaciones sobre el algebra lineal. Univ. Nac. Tucuman, Ser. A, 5: 147-154, 1946.
[44] Dimitris Bertsimas and John N Tsitsiklis. Introduction to linear optimization, volume 6. Athena scientific Belmont, MA, 1997.
[45] Luís Nunes Vicente. Worst case complexity of direct search. EURO Journal on Computational Optimization, 1(1-2):143-153, 2013.
[46] Carl Edward Rasmussen. Gaussian processes in machine learning. In Summer school on machine learming, pages 63-71. Springer, 2003.
[47] Tim D Barfoot, Chi Hay Tong, and Simo Särkkä. Batch continuous- trajectory estimation as exactly sparse gaussian process regression. In Robotics: Science and Systems, volume 10, pages 1-10. Citeseer, 2014.
[48] Simo Särkkä, Arno Solin, and Jouni Hartikainen. Spatiotemporal learning via infinite- dimensional bayesian filtering and smoothing: A look at gaussian process regression through kalman filtering. IEEE Signal Processing Magazine, 30(4):51-61, 2013.
[49] Qiang Liu and Dilin Wang. Stein variational gradient descent: A general purpose bayesian inference algorithm. Advances in neural information processing systems, 29, 2016.
[50] Zita Marinho, Anca Dragan, Arun Byravan, Byron Boots, Siddhartha Srinivasa, and Geoffrey Gordon. Functional gradient motion planning in reproducing kernel hilbert spaces. arXiv preprint arXiv: 1601::1601.03644 2016.
[51] Stephen Boyd. Stephen P Boyd, and Lieven Vandenberghe. Convex optimization. Cambridge university press, 2004.
[52] Grady Williams, Andrew Aldrich, and Evangelos A Theodorou. Model predictive path integral control: From theory to parallel computation. Journal of Guidance, Control, and Dynamics, 40 (2):344-357, 2017.
[53] Richard Sinkhorn. Diagonal equivalence to matrices with prescribed row and column sums. The American Mathematical Monthly, (444):422405 1967.
[54] Egon Schulte. Symmetry of polytopes and polyhe dra. In Handbook of discrete and computa- tional geometry, pages 477-503, Chapman and Hall/CRC, 2017.
[55] Andrew J Hanson. 4 rotations for n-dimensional graphics. In Graphics Gems V, pages 55-64. Elsevier, 1995.
[56] John Frank Adams, Lectures on Lie groups. University of Chicago Press, 1982.
13

---

[57] Gilbert W Stewart. The efficient generation of random orthogonal matrices with an application condition estimators. SIAM Journal on Numerical Analysis, 17(3):003-409 1980.
[58] Keliang He, Elizabeth Martin, and Matt Zucker. Multigrid chomp with local smoothing. In IEEE-RAS 13th Humanoids, 2013.
[59] Arunkumar Byravan, Byron Boots, Siddhartha S Srinivasa, and Dieter Fox. Space-time func- tional gradient optimization for motion planning. In IEEE ICRA, 2014.
[60] Mohak Bhardwaj, Balakumar Sundaralingam, Arsalan Mousavian, Nathan D Ratliff, Dieter Fox, Fabio Ramos, and Byron Boots. Storm: An integrated framework for fast joint-space model-predictive control for reactive manipulation. In CoRL. PMLR, 2022.
[61] John Schulman, Yan Duan, Jonathan Ho, Alex Lee, Ibrahim Awwal, Henry Bradlow, Jía Pan, Sachin Patil, Ken Goldberg, and Pieter Abbeel. Motion planning with sequential convex optimization and convex collision checking. IJRR, 2014.
[62] Charles R Hargraves and Stephen W Paris. Direct trajectory optimization using nonlinear programming and collocation. Journal of guidance. control, and dynamics, 10(4):338-34 1987.
[63] Daisuke Inoue, Yuji Ito, and Hiroaki Yoshida. Optimal transport-based coverage control for swarm robot systems: Generalization of the voronoi tessellation- based method. In 2021 American Control Conference (ACC), pages 3032-3037. JEEE, 2021.
[64] Vishaal Krishnan and Sonia Martínez. Distributed optimal transport for the deployment of swarms. In 2018 IEEE Conference on Decision and Control (CDC), pages 4583-4588. IEEE, 2018.
[65] Saptarshi Bandyopadhyay. Soon-Jo Chung, and Fred Y Hadaegh. Probabilistic swarm guidance using optimal transport. In 2014 IEEE Conference Control Applications (CCA), pages 198-505. IEEE, 2014.
[66] Rabiul Hasan Kabir and Kooktae Lee. Efficient, decentralized, and collaborative multi-robot exploration using optimal transport theory. In 2021 American Control Conference (ACC), pages 4203-4208, 2021. doi: 10.239 19/CC50 2.211.94882222
[67] Christina Frederick, Magnus Egerstedt, and Haomin Zhou. Collective motion planning for group of robots using intermittent diffusion. Journal of Scientific Computing. 90(1): 2022.
[68] Rabiul Hasan Kabir and Kooktae Lec. sereding-horizonn ergodic exploration planning using optimal transport theory. In 2020 American Control Conference (ACC), pages 1447-1452. IEEE, 2020.
[69] Pascal Klink, Haoyi Yang, Carlo D' D'Eramo, Jan Peters, and Joni Pajarinen. Curriculum rein- forcement learning via constrained optimal transport. In International Conference on Machine Learning, pages 11348. PMLR, 2022.
[70] Yongxin Chen, Tryphon T Georgiou, and Michele Pavon. Optimal transport in systems and control. Annual Review of Control, Robotics, and Autonomous Systems, 4(1), 2021.
[71] An Thai Le, Kay Hansel, Jan Peters, and Georgia Chalvatzaki. Hierarchical policy blending as optimal transport. In Learning for Dynamics and Control Conference, pages 797-812. PMLR, 2023.
[72] Kay Hansel, Julen Urain, Jan Peters, and Georgia Chalvatzaki. Hierarchical policy blending as inference for reactive robot control. In 2023 IEEE International Conference Robotics and Automation (ICRA), pages 10181-10188. IEEE, 2023.
[73] Joan Sola, Jeremie Deray, and Dinesh Atchuthan. A micro lie theory for state estimation in robotics. arXiv preprint
2018.
[74] George Marsaglia. Choosing a point from the surface of sphere. The Annals of Mathematical Statistics, 43(2):645-646, 1972.
14

---

[75] S. Surjanovic and D. Bingham. Virtual library of simulation experiments: Test functions and datasets. Retrieved October 26, 2023, from http::/ww..sttt..eee sfw.. calcccaa" jsurjano, 2013.
[76] Hicham Janati, Marco Cuturi, and Alexandre Gramfort. Debiased sinkhorn barycenters. In International Conference on Machine Learning, pages 4692-4701, PMLR, 2020.
15

---

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 and Assumption 2 stated in Section 3.
First, we state proof sketch of Proposition 1.
Proposition 1. vP P, DP forms positive spanning ser.
Proof. Observe that by construction of d-dimensional regular polytope P E P, the convex hull of its vertex set y
conv(V!) Σ 1asi VP, Σ w; =1 wi > Vi
has dim(conv(V")) = d dimensions. Hence, trivially, the conic hull of DP positively spans R..
Now, we can investigate the quality of D in the following lemma.
Lemma 1. For any E IRd a + O, 3E E D such that
(a, d) up ||al| 0 Up
where up up= 1/ 1/ f) P d-simplex, up 1/vd P d-orthoplex, and up 1/v2 for = d-cube.
Proof. From Proposition 1, DP is positive spanning set, then for any a E R2, 3d E DP such that (a, d) (Theorem 2.6, [28)). This property results in the positive cosine measure of DP (Proposition 7, [42])
(a, d)
1 uP min Of
0
(11)
DED"
|a |
Equivalently, pp is the largest scalar such that (a, d) 2 Ap ||a|| ||]]| = =p ||all, de DP.
Next, due to the symmetry of the regular polytope family P. there exists inscribing hypersphere S with radius for any P [29]. For 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) these centroid vectors having the value
Pp R
with R = 1 is the radius of the circumscribed unit hypersphere. The inradius r for d-simplex, d-orthoplex, and d-cube are Id 1/v2, respectively [29].
This lemma has a straightforward geometric implication for every v of O, U E Rd, there exists a search direction d D such that the cosine angle between these vectors is acute (i.e., μP > 0). Then, if we consider the negative gradient vector, which is unknown, there exists a direction D that approximates it well with μı being the quality metric (i.e., larger (ap is better). The values of upp for each polytope type also confirm the intuition that, for d-cube with an exponential number of vertices
2d has constant cosine measure, while the cosine measure of d-simplex having m = d 1 vertices scales (11d) with dimension. Now, we state the key lemma used to prove the main property of Sinkhorn Step.
Lemma (Key lemma). If Assumption I and Assumption 2 holds, then Vark E X Vk
f(x+11) S f(xx) ppax [f((x))] ax
(12)
16

---

Proof. If the Assumption 2 holds, by Proposition 4.1 in [17], the OT solution W Wo converges the optimal solution with maximum entropy the set of solutions of the original problem
min (Lt(Lm/m...//))
(W, C).
Moreover, Birkhoff doubly stochastic matrix theorem [43] states that the of extremal points of U(1,/n, In/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 a linear objective a finite non-empty polytope is reached at vertex or face of the polytope (i.e., the feasible space of the linear program), leading to the following two cases.
Case I: Wa/n + Wo/n converges to permutation matrix representing the bijective mapping between the optimizing points and the polytope vertices. There exists vertex evaluation permutation forming the cost matrix such that, the update step 8y, is a descending step for each optimizing point
Vay E Xx. 8y = aq - D"
argmin + axd))
(13)
dEDP
with w as row in Wo. then w / a one-hot vector. Let a = (x())) 8y is a descend- ing step fxx 8k) f (xxx), then, by Lemma (Vf(xx), 81.) -aufr [ (zx)]l.
Case 2: Wa/n + Wo/n converges to a linear interpolation between the permutation matrices defining the neighboring vertices of the polytope. In this case, there are infinite solutions the linear interpolation between the two bijective maps. There still exists vertex evaluation permutation forming the cost matrix such that, the update step 8y is the linear interpolation of multiple tied descending steps for each optimizing point, with 8 E, b,d,, Σ; b; = 1, b 2 O, d, argmin [f( + ogd)). Following the argument of Case 1, since 85 is the linear interpolation of descending steps, also conclude that (V 8x) = >; by
d:) I Σ, boggpp p [[x{) = oopp [
Finally, starting the L-smooth property of f, we can write
L ||
Vo E X, Ak 0, f(x+11) f(xx Bk) S f(xx)) (V)(( $4))
2
(14)
L 2
$ f(²)) ppak [f(x))]|
2 ak
recalling that ||]] 1, Vd D".
If the sufficient decrease condition does not hold f(xx) - fx < ca with some > 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 scalar multiplied with the stepsize. We can see this by rewriting Eq. (12) as
1
f(xx) f(xx+11
L + an
[Vf((x)]| <
μP
Okk
L
C
ag..
μP
We can implement check if the sufficient decrease condition holds for ensuring monotonicity in each iteration, variant of the Sinkhorn Step.
Lemma also enables analyzing each optimizing point separately, and hence we can state the following main theorem separately for each æ E Xx.
Theorem 1 (Main result). If Assumption I and Assumption holds at each iteration and the stepsize is sufficiently small a₁ with 0 < < 2pre/ / L, then with sufficient number of iteration
K > k(e) = f(x0) - ff
(upe Lg )a
La
we have mino<Ask [ff(xx)]] S Vox E Xx.
17

---

Proof. We attempt the proof by contradiction, thus we assume [|V((= e for all & S k(c).
From Lemma 2. we have VIz E XR. Ak
f(*++1) S f(xx)) pra [VV + L a²
From Assumption 1, the objective is bounded below f. S / (a:). Hence, we can write
ʃ. S f(0K+1) < [(xx) ppa [V((=))]| + -a²
- f( ppa [Xp([[/((X[] + | ( + 22 aa²
K
[V[())]] + (K + 1) L
202
< (10) - pPa
0=0
(15)
S f(x)) (K + 1)upae + (K + 1) L
2a²
S f(w)
(K + 1K+ppcp az)
S (ma)
(f(20)- - -f.)
= f.
by applying recursively Lemma 2 and the iteration lower bound at the second last line, which is contradiction f- f.. Hence, [+xx) S for some 1: S k(c).
If L is known, we can compute the optimal stepsize a = pupe/L. L. Then, the complexity bound is k(e) = 2L([(ea)=f=> - 1. Note that Theorem 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(æx) f(*++1) 2 ca then f(zk) S f(z,), f S 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 I and Assumption 2, by substituting pr according to Lemma 1.
Remark 1. By Theorem 1, with the optimal stepsize a = pppe d-orthoplex and d-cube are (f (e),, O(d/e), ), and O(1/e)) respectively.
/ L, the complexity bounds for d-simplex,
The optimal stepsize with d-simplex reports the same complexity O(d//e² as the best-known bound for directional-diree search [45]. Within the directional-direct search scope, d-cube reports the new best-known complexity bound ((1/²²,, 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 2d.
B Gaussian Process Trajectory Prior
To provide a trajectory prior with tunable time-correlated covariance for trajectory optimization, either as initialization prior as cost, we introduce a prior for continuous-time trajectories using a GP [46, 47, 8]: + ~ (P(((t)) K(t,t") t'),, with mean function l and covariance function K. As described in [47, 48, 13], GP prior can be constructed from linear time-varying stochastic differential equation
Φ = A(t)x(tt) + (tt) + (())
(16)
with utt) the control input, A(t) and F()) the time-varying system matrices, and w(t) a disturbance following the white-noise process w(t) (O, Q t')), where Qe > is the power-spectral density matrix. With a chosen discretization time At. the continuous-thn GP can be parameter- ized by mean vector of Markovian support states μ [u(0),..., [ and covariance matrix K (iii))lossssss: (i, dxd resulting in multivariate Gaussian q(T) N K).
18

---

The inverse of the covariance matrix has sparse structure K- = DTQ D (Theorem 1 in [47]) with
I
D
(17)
I
0
PTT--
0
I
and the block diagonal time-correlated noise matrix Q = - ( Qo.1.... Q-- Σ 1 )
Here, P1,141 is the state transition matrix, Qt,++1 the covariance between time step and t and Σ,, Eg are the chosen covariance of the start and goal states. In this work, we mainly consider the constant-velocity prior (i.e., white-noise-ong on---celleaationn model x(t) w())), which can approximately represent wide range systems such as point-mass dynamics, gravity-comeee compensstee 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 æ = [x E TR and
A(t
0
d/2
O, F(t)
(18)
0
[d/2
Then, following [47, 8], the state transition and covariance matrix are
L6/2
Alla/2
L 1 Q
AA
Qr,t+1
(19)
0
Ia/2
AtQ.
with the inverse
12at At Q
-6 -
Q2,
2 Q-1
(20)
which are used to compute K.
Consider priors on start state qs (x)) = (N,,, E.) and goal state (z(²) = NN Eg), the GP prior
for discretized trajectory can be factored follows [47, 8]
(r(T) exp (
11l 1)
T-1
(21)
go (x)) g (x()))
II (x41)
where each binary GP-factor is defined
ge (m,, x1+1) = exp { ブタ 01 - 14)) (me+1 M4+1 11
(22)
In the main paper, we use this constant- vellocity GP formulation to sample initial trajectories. The initialization GP is parameterized by the constant-velocity straight line μu connecting a start config- uration μ4 to a goal configuration Mg. having moderately high covariance Kg. For using this GP the cost, set the zero-mean la 0 to describe the uncontrolled trajectory distribution. The conditioning Qg (Er) of the final waypoint to the goal configuration f4g is optional (e.g.. when the goal configuration solution from inverse kinematics is sub-optimal), and typically use the SE(() goal cost.
C Additional Discussions Of Batch Trajectory Optimization
Direct implications of batch trajectory optimization. MPOT can be used as 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 select the best solution according some criteria, e.g., collision avoidance, smoothness, model consistency, etc.
Solution diversity of MPOT. Batch trajectory optimization can serve 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
19

---

the step radius ag annealing scheme,
the variances of GP prior initialization,
the number of plans in 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.
Extension 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 batch of different horizon trajectories, we address this case by setting maximum horizon Taxx and padding with zeros for those trajectories having < Tmax. Then, we also set zeros for all rows corresponding to these padded points in the cost matrix Cf 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 this index list. Finally, the trajectorics 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 F(.): R xd + R to be the planning cost functional, prior works [7, 50, 8] apply first-order Taylor expansion the current parameter Tk, while adding regularization norm
AT argmin { (T(m)) + VF(T)) - L SAATIIA
(23)
resulting in the following update rule by differentiating the right-hand side w.r.t. AT and setting it to zero
Tk+1 + -
(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 AT 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 novel way explicitly constrain the parameter updates inside 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
Tk+1 - Tall = [|addiag(n)) (amn)-1wp -W D
T
Ok w DP n
(25)
t=
|| < To
20

---

Algorithm 2: Stabilized Sinkhorn Algorithm
(a", 6") - (Or, Om), (ū", .6") + (1r, 1m), M = 10 Compute stabilized kernel p using Eq. (29). while termination criteria met do
// Sinkhorn iteration
ū n = =
// Check for numerical instabilities
if +||0" < II < M then Absorption.
(a' (a', (a' Alo((")), b' A Alog(o')).
Compute stabilized kernel P using Eq. (29). (u', 0') (1r,1., 1m).
end end
Return W diag( P diag((")
resulting from D being a regular polytope inscribing the (d - 1)-u hypersphere. In practice, one could scale the polytope in different directions by multiplying with M induced by priors, and, hence, shape the trust region similar fashion. Note that the bound in Eq. (25) does not depend on the local cost information.
For completeness of discussion, in sampling-base 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 sampling distribution with low variance the tails and high variance 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 X, the approximate solution from the entropic-regularizze OT problem
W = argmin X m))
approaches the true optimal plan
W argmin (C, W).
WEU(n.m)
However, small incurs numerical instability for a high-dimensional cost matrix, which is usually the case for our case of batch trajectory optimization. Too high X, which leads to "blurry" plans, also harms the MPOT performance. Hence, we utilize the log-domain stabilization for the Sinkhorn algorithm.
We provide brief discussion of this log-domain stabilization. For a full treatment of the theoretical derivations, we refer to [35, 36). First, with the marginals n E Σr, m E Em and the exponentiated the Sinkhorn algorithm aims to find a pair of scaling factors
kernel matrix P
T a E R. such that
u + Po = v PTu = m.
(26)
where 3 the element-wist multiplication (i.e., the Hadamard product). From a typical one vector initialization d = 1m, the Sinkhorn algorithm performs a sequence of (primal) update rules
(27)
Pet
leading to convergence of the scaling factors u" v² [53]. Then, the optimal transport plan can be computed by W diag(u")
(aaaguu"PPPaalCC (Pdiag(v").
For small values of X, 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
21

---

issue, at an iteration i, Chizat et al. [35] suggests redundant parameterization of the scaling factors
u'
exp(a'/>) v'= exp(b'/ /A),
(28)
with the purpose of keeping ú, i bounded, while absorbing extreme values of into the log- domain via redundant vectors a, The kernel matrix p is also stabilized, having elements being modified as
Pij = exp ((a) + -c,)/A),
(29)
such that large values in a, and C cancel out before the exponentiation, which erucial for small X. With these ingredients, state the log-domain stabilization Sinkhorn algorithm in Algorithm 2. Note that Algorithm 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 due to the exponentiation of the kernel matrix P. Moreover, we only check for numerical issues once per iteration for efficiency. Note that multiple numerical issue checks can be done in iteration as trade-off between computational overhead and stability. Hence, tuning for the cost matrix C magnitudes and A, for the values inside the exp function not become too extreme, is still required for numerical stability.
F Uniform And Regular Polytopes
We provide brief discussion on the d-dimensional uniform and regular polytope families used in the paper (cf. Section 3). For comprehensive introduction, refer [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 < S d) also called cells, faces, edges, and vertices being transitive and also regular sub- polytopes dimension S [29]. Specifically, the polytope's 's facets are pairwise congruent: there exists an isometry that maps any facet to any other facet.
To compactly identify regular polytopes, Schlüffii symbol is defined the form {a, c, =) with regular facets as {a, b, c...,, v). y. and regular vertex figures as {b, c..... 2). For example,
polygon having n edges is denoted {n} (c.g., square is denoted {4]).
regular polyhedron having {n) faces with p faces joining around vertex denoted as p) (e.g., a cube is denoted as {4, (4) and {p} is its vertex figure (i.e., a figure of an exposed polytope when vertex is "sliced off"),
regular 4-polytope having cells {n, with q cells joining around an edge is denoted as (n.P.g] having vertex figure {p. 4) and so on.
A d-dimensional uniform polytope is generalization of regular polytope only retaining the vertex- transitiveness (i.c., only vertices are pairwise congruent), and is bounded by its uniform facets. In fact, nearly every uniform polytope truncation, and alternation from either regular polytopes or other uniform polytopes [54]. This implies vast number of possible choices of vertex nartex-transttiee uniform polytopes that can be applied to the Sinkhorn Step. Further research in this direction is interesting.
be constructed by Wythoff constructions, such as rectification,
Figure 5: Examples of (left to right) 3-simplex, 3-orthorplex, 3-cube.
22

---

Table 3: Regular and uniform polytope families.
Dimension
Simplices
Orthoplexes
Hypercubes
d = 2
regular trigon {3} regular tetrahedron [3,3] regular octahedron {3, 43 d-simplex {3d-1}
square {4}
square {4}
d = 3 Any
cube {4, 3]
d-orthoplex {3d-2 4)
d-cube {4d
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 = for a d-simplex, n 2d for a d-orthoplex, and n 24 for a d-cube.
Construction. We briefly discuss the vertex coordinate construction of d-regular polytopes P inscribing (d - -uit hypersphere with its centroid at the origin. Note that these constructions are GPU vectorizable. First, we denote the standard basis vectors e Cd for Rdd
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 A = =EE= wye; Rd Σ W; = w; > 0, for i = 1,...,.... dd. Now, already got d vertices with the pairwise distance of v2. Next, the final vertex lies on the line perpendicular to the barycenter the standard simplex, so it has the form (a/d..... a/d) Rd for some scalar a. For the final vertex to form regular d d-simplex, its distances to any other vertices have to be V2. Hence, we arrive at two choices of the final vertex coordinate 2(1 + v1 + d) Finally, we shift the regular d-simplex centroid to zero and rescale the coordinate such that its circumradius resulting in two sets + 1 coordinates
1 (1 Vd 1)1a V for 1 S i d, and (V++ dvd
1 Vd
1a
(30)
Note that we either choose two coordinate sets by choosing + in the computation.
For regular d-orthoplex, the construction is trivial. The vertex coordinates are the positive-negative pair of the standard basis vectors, resulting in 2d coordinates
E₁ - -1
Cdi -ed
(31)
For 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 24 vertex coordinates.
G d-Dimensional Random Rotation Operator
We describe the random d-dimensional rotation operator applied on polytopes mentioned in Section Focusing on the computational perspective, we describe the rotation in any dimension through the lens of matrix eigenvalues. For any d' d-dimensional rotation, (proper) rotation matrix R E dxd acting on Rd is an orthogonal matrix R R leading to det( R) = 1. Roughly speaking, R does not apply contraction or expansion to the polytope convex hull vol DP) vol(D* (o(DDR) R).
For even dimension d = 2m, there exist eigenvalues having unit magnitudes &p e030 = I,..., m. There is no dedicated fixed eigenvalue & = 1 depicting the axis of rotation, and thus no axis of rotation exists for even-dimensionaa spaces. For odd dimensions d = 2m + 1, there exists at least one fixed eigenvalue 8 = 1, and the axis of rotation is an aad-dimensional subspace. To see this, set o 1 in det( R pII follows
det( R r) = det( R det R - 1) = det R R - R.)
(32)
= det(T R) = det) I)
-det Mee(RR R I)2
with (-1)²= = for odd dimensions. Hence, det R I) = 0. This implies that the corresponding eigenvector of و = is a fixed axis of rotation Rr = r. When there are some null rotations in the even-dimensional subspace orthogonal to r, i.e., when fixing some 0: = 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 rotation is parameterized by the same number of rotation
23

---

angles as in the 2m-dimensional rotation. As a remark, in d 2 4, there exist pairwise orthogonal planes of rotations, each parameterized by a rotation angle 0. Interestingl, we smoothly rotate a 4-dimensional object from a starting orientation and choose rotation angle rates such that 01 = w02 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 x 2 matrices
R = [oss(O) (0:) sin sin(O:)
sin (0:)] (O)
(33)
describing a rotation associated with the rotation angle O: and the pairs of eigenvalues e 1...., m. In fact, this construction constitutes maximal torus in the special orthogonal group S0(2m) represented as T(m) = {diag(e" e ",,,, VI, 0g E IR,, 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 ood-dimensional rotations S0(2m + 1), where the group action fixes the remaining direction. For instance, the maximal tori in (3) are given by rotations about a fixed axis of rotation, parameterized by a single rotation angle. Hence, we construct a random d x d rotation matrix by first uniformly sampling the angle vector E [0, 2T] then computing in batch the 2 x 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 -innddimnsiooall state space. Hence, we do not need to specifyy the axis of rotation. For general construction of a uniformly random rotation matrix in any dimension d 2 2, readers can refer to the Stewart method [57] and our implementation of Steward method at https: /taps:://// //gtthub. com/ att/nannnnde/eeee/eeeeee ssax/ blob/main/ wwwax//////taateeeeeeeeeeeeeeeee
py#L388
H Related Works
Motion optimization. While sampling-bass 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 tradient-based methods was proposed [7, 21, 8] for finding locally optimal solutions. These methods require differentiable cost functions, effectively requiring crafting 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, c.g., STOMP [9] sample candidate trajectories from proposal distributions, evaluate their cost, and weigh them for performing updates [52, 13J. Although gradient- ffee 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 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 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 control is available in [70]. Recently, Le et al. [71] proposed method for re-weighting Riemannian motion policies [72]
24

---

Figure 6: (Left) An example of the Panda plan execution for three simulation frames. The green line denotes a (E(3) goal. (Right) An example of collision spheres attached to TIAGo+ mesh at 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 PyBuleet is shown in Fig. 6.
For fair comparison, we construct the initialization GP prior N(po: Ko) 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, reimplement all baselines in PyTorch with vectorization design (beside RRT*) and fine-tune them with the parallelization setting, which is unavailable the original codebases.
Notably, we use RRT*/I- as feasibility indicator of the environments since they enjoy probabilistic completeness, i.e., an infinite time budget if a solution exists these search-based methods will find the plan. Optimization-basee motion planners, like MPOT, GPMP2, CHOMP, and STOMP are only local optimizers. Therefore, solution cannot be found by RRT*//-RRT* then not possible that optimization-baset approaches can recover solution.
L1 MPOT experiment settings
For MPOT, we apply e-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 presented in Table 4.
I.2 Environments
For the point-mass environment, populate 15 square and circle obstacles randomly and uniformly inside x-y limits of [010, 10), with each obstacle having radius or width of 2 (cf. Fig. 1). We generate 100 anvironment-seeds and for each environment-seed. we randomly sample 10 collision- free pairs 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 Fach environment-seed contains randomly sampled 15 obstacle-spheres having radius of 10cm inside the x-y-z limits of
25

---

Table 4: Experiment hyperparameters of MPOT. ao, &o the initial stepsize and probe radius. is the number of probe points per search direction, eps is the annealing rate, P is the polytope type, and À the entropic scaling of OT problem.
Point-mass
Panda
TIAGo+
Og
0.38
0.03
0.03
0.5
0.15
0.1
10
0.032
0.035
0.05
P d-cube
d-orthoplex 0.01
d-orthoplex
X
0.01
0.01
(1_0.7,0.7,,(-0..,, 1.]] (cf. Fig. 6), ensuring that the Panda's initial configuration has no collisions. Then, we sample random collision-free free (including self-collision-fiie 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, first solve inverse kinematics (IK) for the (3) goal in PyBullet, and then create a constant-velocing straight line 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 realistic nigh-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 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 's base at the front side of the table or the shelf and solve IK using PyBullet. We designed holonomic base for this experiment. A successful trajectory finds collision-fre plans, successfully grasping the cup and placing it on the shelf.
1.3 Metries
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₂ environment-seeds and N₂ tasks for each environment-seee. For each task, we optimize Np 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 T[s] of trajectory optimizers until convergence or till maximum iteration is reached. T[s] is averaged over N₂ x N, tasks.
We measure the success rate of task executions over environment-seett Specifically, SUC[%] Nst/N, 100 with Nst being the number of successful task executions (i.e., having least a successful trajectory in a batch). The success rate is averaged over N, environment-seeds.
Success Rate.
Parallelization Quality. We measure the parallelization quality, reflecting the success rate of trajectories a single task. Specifically, GOOD[% [%] = Nap/N x 100 with Nap being the number of successful trajectories in task, and it is averaged over N, N; 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, S 1 ET-1 -1
||x & S is averaged over successful trajectories in
N. X N₂ tasks.
Path Length. We measure the trajectory length, reflecting natural execution and also smoothness. Specifically, PL ET- ||x++1 xx].. PL is averaged over successful trajectories in N, N, tasks.
26

---

1.4 Motion planning costs
For the obstacle costs, we use an occupancy map having binary values for gradient- 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 1.2 cost for joint violations. Besides the point-mass experiment, all collisions are checked by PyBullet. The differentiable forward kinematics implemented in Py PyTorh is used for all planners.
Goal Costs. For the (3) goal cost, given two points Ti = [Ri, Pi] and T3 = R2, Pal in SE(3), we decompose translational and rotational part, and choose the following distance as cost dsv(3) [T, T2) [p Pzll + (aooMMap RJ R2)) where LogMap(-) is the operator that maps an element of SO(3) to its tangent space [73].
Collision Costs. Similar CHOMP and GPMP2, 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
K
Cobs (q)
1 K c(a(qq
Σ
(34)
with æ0g, Sj) is the forward kinematics position of the -olcoolisoon sphere, which is computed in batch. For gradient-based motion optimizers, we design the cost using the signed-distance function d(-) from the sphere center the closest obstacle surface (plus the sphere radius) in the task space with a e >> 0 margin
c(xx) [d(x)) +e if d() -- if d( <
(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, 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 cost, with
margin each dimension
|| gmii + qell if q:
Qmin +
(g:)
0
if qmin e qs S Geax
(36)
119max c qill
else
J
Ablation Study
In this section, we study different algorithmic aspects, horizons and number of paralleled plans, and also provide ablation 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 random rotation applied on the polytopes, and MPOT-NoAmnealing annealing option is disabled. This ablation study 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 the fixed probe set HP 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.
27

---

MPOT
MPOT-NoRot
100
NoAnnellig
80-
Parallelancies Quality Rate
60-
4-
2
0-
0-
-20
0
20
40
100
100
Sinkhorn Steps
Sinkhorn Steps
Figure 7: Ablation study algorithmic choices in the point-mass environment. All planners terminated 100 Sinkhorn Steps. All statistics are evaluated 1000 tasks as described Scction 5.2.
0.47
2.76
5.68
2.65
5.14
20.90
0.23
0.43
1.11
2.82
5.48
10.84
0.41
0.85
1.54
2.74
5.33
10.41
Num Trajectories
Trajentoriss
128
0.17
0.21
0.48
1.21
2.77
5.34
0.26
0.39
0.70
1.34
2.64
5.46
64
0.10
0.13
0.21
0.19
1.15
2.72
16
0.21
0.26
0.38
0.72
1.41
2.69
32
0.26
0.50
1.07
0.23
0.26
0.41
16
16
32
64
128
256
512
8
16
32
64
128
Horizons
Horizons
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 context. MPOT has faster convergence rate due to annealing the step and probe radius, which leads to better approximation of local minima. However, requires careful tuning of the annealing rate to avoid premature convergence and missing better local minima. APOT-NoAnnealinn converges slower and thus takes more time, but eventually discovers more successful local minima (nearly 80%) than MPOT with annealing (cf. Table 1). This 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 Np. For each (T, N₁)) combination, we tune MPOT achieve 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 a nearly symmetric pattern. Despite long horizons and large batch trajectories, the planning time remains reasonable
28

---

Table 5: Polytope ablation study the Panda environment. All statistics evaluated on 500 tasks described 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 1 23.2
60.2 1 44.4
0.01
- 0.01
4.6 - 0.9
(under a minute) and 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-Orthopice (i.e., MPOT in the Panda experiments) with its variants: MPOT-Simplex using the d-simplex vertices as search direction set DP, and MPOT- Random, ie., 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 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 more sparse approximation. MPOT-Random, while achieving 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- digeesimensi settings.
J.4 Smooth gradient approximation ablations
We conduct an ablation on the gradient approximation of Sinkhorn Step w.r.t. different important hy- perparameter settings for sanity check of Sinkhorn Step's optimization behavior on smooth objective function. We choose the Styblinski-Tand due to its variable dimension and multi-modality for non-convex optimization benchmark [75]. We target the most important hyperparameters of polytope rype P, and entropic regularization scalar X. These parameters sensitively affect the Sinkhorn Step's s optimization performance. We the other important hyperparameters of step size and probe size a = B = 0.1 to be constant, the number of probing points per vertices be and turn off the annealing option for all optimization runs. The cosine similarity is defined for each particle a; E X as follows:
function (cf. Fig. 9) in 10D as the smooth objective function
s(x;) (-V CS, [| Vfx=)l
(37)
Regarding this smooth objective, we observe the gradient approximation quality is consistent with Lemma with increasing cosine similarities for all curves from left right column (cf. Fig. 10). However, regarding entropic regularization scalar X, we observe higher cosine similarity and lower curve variance for larger A. Interestingly, this means higher induces both computational benefit solving entropic OT [19] and higher entropic smoothing bias [76], where the latter regularizes the gradient approximation directions, while 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 X = 0.5 (red curve) keeps high cosine similarity toward fixed points (cf. Fig. 10), while the lower/sharper exhibits degradation due to noisy random rotated 10-cube with constant-size having 2 vertices.
Note that the conclusion drawn from this ablation may not apply 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) -gaddint approximation in various objective function conditions is very interesting for future work.
29

---

R2r5
Kert 25
St
Aurs
Aers
Figure 9: An example optimization run of 1000 points on the Styblinski- Tang function with Sinkhorn Step. The points uniformly sampled at the start of optimization. This plot shows the projected optimization in the first two dimensions.
Figure 10: Ablation study gradient approximation with cosine similarity between Sinkhorn Step directions and true gradients. We choose the Styblinski Tang function the test objective function. Each curve represents an optimization run of 1000 points w.r.t to entropic regularization scalar 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.
30


Reviews:
[{"review_id": "diNpca985L", "reviewer": "Reviewer_caUS", "summary": "The paper proposes a trajectory optimization method using Sinkhorn Step which is able to perform efficient gradient-free batch optimization with non-linear objectives.", "questions": "See weaknesses.", "limitations": "Limitations are well-discussed in the paper.", "rating": 7, "confidence": 2, "soundness": 3, "presentation": 3, "contribution": 3, "strengths": "Gradient-free motion optimization is an important area with broad potential applications. \n\nThe contribution of the paper is novel. An original update rule is proposed, which significantly improves the converging speed and trajectory quality.\n\nThe paper is well-structured. The experiment results are adequate and easy to understand.", "weaknesses": "It is unclear how the proposed method is compared to learning-based trajectory planning approaches, and also how large is the improvement of the proposed method compared to existing optimization methods when using learning-based trajectory predictions as initializations.", "quality": null, "clarity": null, "significance": null, "originality": null, "strengths_and_weaknesses": "", "was_revised": false, "final_justification": "", "rebuttal": "We thank the reviewer for appreciating our paper contributions.\n\nLearning-based motion planning methods [1] typically utilize a dataset from previously successfully generated plans to learn generative priors for generating plans directly [2] or using rollout samples as initialization for motion planners [3]. Differently from the learning-based setting, we propose an optimization-based planner without any learning elements, and we conduct comparison experiments with other widely-used motion planners to support the paper's claims.\n\nPer the reviewer's suggestion, a learning-based setting can naturally complement MPOT to provide even better initializations, as currently, we only use GP priors to provide random initial smooth trajectories. However, this is considered for future work. In the current research, we focus on the proposition of the novel operator, the Sinkhorn Step, that allows us to tackle motion planning problems, resulting in an efficient trajectory optimization method producing smooth trajectories. This opens up exciting future directions for learning to plan applications, e.g., using MPOT as an expert for learning to plan or using implicit or explicit learned policies to improve MPOT further.\n\n[1] J. Wang et al., \"A survey of learning-based robot motion planning,\" IET Cyber-Systems and Robotics, vol. 3, no. 4, pp. 302–314, 2021.\n\n[2] A. H. Qureshi, A. Simeonov, M. J. Bency, and M. C. Yip, \"Motion planning networks,\" in IEEE ICRA, 2019.\n\n[3] J. Urain, A. Le, A. Lambert, G. Chalvatzaki, B. Boots, and J. Peters, \"Learning implicit priors for motion optimization,\" in IEEE/RSJ International Conference on Intelligent Robots and Systems, 2022."}, {"review_id": "48y4UteZv1", "reviewer": "Reviewer_MARJ", "summary": "The paper focuses on the optimization of motion planning problem, by introducing a gradient-free method that is parallelizable and smooth. The method first probes the costs at several vertices, then decides the optimization direction by aligning the weight matrix with the cost matrix. Furthermore, to enforce the weight matrix as a joint distribution, the method introduces Sinkhorn Step, which is based on an acceleration technique for optimal transport optimization. Results show that the method has significantly low planning time and a high success rate.", "questions": "1. How is D^P initialized at the beginning of the algorithm? I guess it is not that important since you will rotate it for each step, but I still would love to know.", "limitations": "Limitations are adequately addressed.\n\nI do not see any significant potential negative societal impact.", "rating": 7, "confidence": 2, "soundness": 3, "presentation": 3, "contribution": 3, "strengths": "1. Though including some technical mathematical concepts, the paper has good writing and is easy to understand.\n2. Results show the method has significant empirical performance.\n3. Technical tricks and limitations are well discussed, for example, Section 4.2 and Section 5.3.\n4. Implementing most baselines in PyTorch seems to be a lot of work. This is also a contribution, since it provides a platform to compare fairly with previous methods.", "weaknesses": "1. To me, 7 DoF is still not high-dimensional tasks (TIAGo++ should be if you are not using action primitives). So since the author claim that the approach could work better than the baselines in high-dimensional tasks, I would appreciate a systematic comparison to other baselines in more high-dimensional tasks, probably in addition to TIAGo++.", "quality": null, "clarity": null, "significance": null, "originality": null, "strengths_and_weaknesses": "", "was_revised": false, "final_justification": "", "rebuttal": "We appreciate the positive feedback on our contributions.\n\nRegarding the reviewer's comments and questions:\n\n- **On the dimensionality of trajectory optimization problem**: We do not consider any movement primitives in our experiments. In all cases, we consider optimizing the full-state (the concatenation of position and velocity) trajectories batch-wise. As mentioned in the experiment description (Sec. 5.1), 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, and their velocities) for the mobile manipulation experiment. Note that the typical optimizing variable would be the whole trajectory, leading to variable dimension $T \\times d$. In our vectorization setting, it would be batched $b \\times T \\times d$. Hence, even for the Panda case, typical full-state motion planning with multiple objectives, such as obstacle avoidance, self-collision avoidance, smoothness, goal-reaching, joint limit handling, etc., is inherently challenging due to many local minima and high computational cost due to many objectives. Notably, we design the TIAGo++ mobile manipulation experiment as a stress test, reflecting even more performance differences between MPOT and the baselines.\n\n- **The importance of full-state trajectory optimization**: Optimizing a full-state (position & velocity) trajectory is vital for many robotics tasks, e.g., obstacle avoidance or human-robot collaboration, where smooth motion is desirable. Full-state reference trajectory generally allows for the low-level controller to track more smoothly. In contrast, if the trajectory is position-only, it is typically challenging to tune the gain of the position controller to track the reference trajectory correctly with large jerks or lower jerks but inaccurate tracking.\n\n- **Additional baseline comparisons on TIAGo++ environment**: We conducted additional comparative experiments on the TIAGo++ environment to better support our claims, and according to the reviewer's request, the results are available in the rebuttal attachment. Apparently, CHOMP performs worse than GPMP2 and takes more iterations in a cluttered environment. However, according to the new table result, CHOMP beats GPMP2 in run time complexity in TIAGo++ environment due to its simpler update rule. Regarding SGPMP, we could not tune SGPMP to surpass the success rate of 5/20; hence we opted for the minimum iterations to achieve the mentioned success rate. The problem lies in how SGPMP explores with constant GP variance in a high dimensional setting; thus, a possible future extension of SGPMP with adaptive explorative variance could mitigate the problem. Similarly, STOMP's exploration mechanism is even more restrictive, and we could not tune it to work in this environment. Overall, with the efficient Sinkhorn Step facilitating individual waypoint exploration, MPOT outperforms the baselines in terms of run time complexity by a considerable margin in this case while maintaining reasonable smoothness and task performance.\n\n- **How is $D^P$ initialized at the beginning of the algorithm?** In this paper, we adopt the common regular polytope (i.e., simplex, orthoplex, and hypercube) known to exist for any dimension. Their vertex coordinate computations are well-known in Geometry literature. Hence, for completeness, we provide the construction description of polytope vertices in Appendix F. For actual implementation, we also provide the example code in the supplementary material. The vertex construction functions are found in `mpot/mpot/utils/polytopes.py`. Interestingly, we conducted an ablation study and showed that polytope structure and random rotation are essential for sample efficiency in terms of search directions compared to just sampling random search directions on an $d$-dimensional sphere, as reflected in Table 5 in Appendix J3."}, {"review_id": "IuWSTMtyuo", "reviewer": "Reviewer_2eXt", "summary": "This paper proposes MPOT, a gradient-free method that optimizes a batch of smooth trajectories with nonlinear costs even for high dimensional tasks. In especial, a zero-order and highly-parallelizable update rule called Sinkhorn Step is proposed to facilitate the optimization process. MPOT outperforms the baseline methods with respect to the planning speed and success rate, across various tasks.", "questions": "1. Please refer to the weakness. How do you process the sequences with different waypoint numbers?\n2. How do you evaluate the planning time of other baselines, such as STOMP and GPMP2? Are other baseline methods also executed in batch? \n3. If other baselines are not performed in batch, how do you justify the motivation of performing trajectory smooth in batch? ", "limitations": "Yes, limitations are addressed by the authors. I see no negative societal impact.", "rating": 7, "confidence": 3, "soundness": 3, "presentation": 3, "contribution": 3, "strengths": "1. This paper addresses a fundamental problem - trajectory smoothing in motion planning, and proposes a solid method.\n2. The paper is self-contained and provide very detailed introduction of the method. It is well organized and easy to read.", "weaknesses": "1. To enable the batch optimization, it seems that each sequence of optimizing points in the same batch are asked to share the same length, thereby forming a matrix $X \\in R^{n\\times d}$. Such operation kind of hinders the flexibility of the algorithm to process trajectories with quite different number of waypoints in the batch-wise manner.", "quality": null, "clarity": null, "significance": null, "originality": null, "strengths_and_weaknesses": "", "was_revised": false, "final_justification": "", "rebuttal": "We thank the reviewer for their positive evaluation of our work.\n\n1. **How do you process the sequences with different waypoint numbers?** Currently, for vectorizing the update of all waypoints across the batch of trajectories, we flatten the batch and horizon dimensions and apply Sinkhorn Step, then we reshape the tensor to the original shape. Notice that what really glues the waypoints in the same trajectory together after optimization is the log of the Gaussian Process as cost model, 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_{\\textrm{max}}$ and padding with zeros for those trajectories having $T < T_{\\textrm{max}}$. Then, we also set zeros for all rows corresponding to these padded points in the cost matrix $\\mathbf{C}^{T_{\\textrm{max}} \\times n}$. By this way, the padded points are ignored in the barycentric projection. Intuitively, we just need to manipulate cost entries to dictate the behavior of waypoints.\n\n2. **How do you evaluate the planning time of other baselines, such as STOMP and GPMP2? Are other baseline methods also executed in batch?** We further describe our experiment settings in Appendix I. We tune all baselines for each experiment and then measure the planning time T$[s]$ of baselines until convergence or till maximum iteration is reached. T$[s]$ is averaged over number of environment-seeds and number of tasks. \n    - Striving for a fair comparison, in all cases, besides RRT*/I-RRT*, we reimplemented all baselines in PyTorch and fine-tuned them with the vectorization option. To the best of our knowledge, the baselines are not explicitly designed for plan vectorization. Thus, their associated public codebases are implemented for single-plan querying. \n    - Regarding the vectorization of RRT and its variants, we found some works [1, 2] that focus on parallelizing specific algorithmic components of RRT*, such as graph operations or collision checking in a single planning instance. They resort to special hardware design (e.g., FPGA) or state-space grid discretization, limiting their application to general settings. Thus, vectorizing the whole RRT* algorithm pipeline is non-trivial and still an open question. We opted for serial computation of RRT* to affirm the environments' solvability due to its completeness property, while reflecting the performance gap between GPU-vectorization optimization-based algorithms and serial classical sampling-based algorithms.\n\n3. **If other baselines are not performed in batch, how do you justify the motivation of performing trajectory smooth in batch?** As explained above, all baseline comparisons (except RRT*/I-RRT*) are performed in batch. On a further note, there are three interplaying factors that contribute to the solution diversity and, hence discovering better modes: \n    - the step radius of Sinkhorn Step\n    - the moderately-high initialization variances\n    - the number of plans in a batch\n\n    -> For using MPOT as an oracle for collecting datasets, these factors contributing to the solution diversity covering various modes, capturing homotopy classes of the tasks and their associated contexts. For direct execution, abundance of solutions vastly increases the probability of finding good local minima, which we can select the best solution according to some criteria, e.g., collision avoidance, smoothness, model consistency, etc. We will add such a discussion in the final version of the paper to better motivate the need of batch-wise trajectory optimization.\n\n[1] Bialkowski, Joshua, Sertac Karaman, and Emilio Frazzoli. \"Massively parallelizing the RRT and the RRT.\" 2011 IEEE/RSJ International Conference on Intelligent Robots and Systems. IEEE, 2011.\n\n[2] Xiao, Size, Neil Bergmann, and Adam Postula. \"Parallel RRT* architecture design for motion planning.\" 2017 27th International Conference on Field Programmable Logic and Applications (FPL). IEEE, 2017."}, {"review_id": "M5hnce75W0", "reviewer": "Reviewer_hcju", "summary": "With the aim of improving efficiency in motion planning, this paper proposes an efficient, gradient-free optimization method, MPOT. This is enabled by the introducing the Sinkhorn step, a zero-order parallelizable update rule that is guaranteed to converge under smoothness and boundedness assumptions. The authors perform empirical studies of the effectiveness of their method in 3 benchmarks.", "questions": "- On what hardware is the time comparison from Table 1 being drawn? RRT*/I-RRT* will benefit from stronger CPUs, whereas the other methods have GPU dependencies, so a hardware comparison is important to understand the run time benchmark.\n- How does MPOT do in other benchmarks where RRT* fails to reach a solution?\n- Why is MPOT only compared to GPMP2 in the mobile manipulation experiment, instead of considering similar baselines to the previous cases?\n- Are there other known applications for the Sinkhorn step? The authors comment “sampling methods or variational inference”, but a brief slightly more detailed discussion of this in the paper could improve the contributions greatly.", "limitations": "The authors have addressed limitations of their work to a good extent in Section 5.", "rating": 6, "confidence": 3, "soundness": 3, "presentation": 3, "contribution": 2, "strengths": "- Motion planning is an important task that must be solved quickly while outputting smooth plans to enable the deployment of robotic systems in the real world. As such, this problem is a relevant one.\n- Both the Sinkhorn step as MPOT are described in detail and, to the best of my knowledge, are novel contributions to the field.\n- The paper is well written, well organized and clear.", "weaknesses": "- The benchmarking results presented in Section 5.2 correspond to two settings in cluttered environments where RRT*/I-RRT* do very well despite the running time cost incurred. Including benchmarks where RRT* fails would be interesting to explore further other potential advantages of this method besides run time complexity.\n- In Section 5.3, the authors only report results on MPOT and GPMP2, despite the fact that methods like SGPMP solve similar-sized problems in their experiments. A discussion on the limitations of application, or extension of the benchmark would make the case stronger within this setting. \n\n**Minor comments**:\n- In line 386, it should read “At last”.", "quality": null, "clarity": null, "significance": null, "originality": null, "strengths_and_weaknesses": "", "was_revised": false, "final_justification": "", "rebuttal": "Thank you very much for your insightful comments and constructive feedback. \n\n**Regarding the comment about RRT\\*/I-RRT\\*:** Initially, our intention was to use RRT*/I-RRT* as an indicator of feasibility of the tested environments since they enjoy probabilistic completeness, i.e., at 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 optimization-based approaches can recover a solution. Please note that key issues with RRT*/I-RRT* are the computational complexity and the lack of smoothness (e.g., see the answer for question 2, about the performance of RRT*/I-RRT* for the TIAGo++ environment). Utilizing a motion planning strategy like MPOT that plans very high frequencies, we can potentially optimize in real-time and be more efficient in re-planning in dynamic environments, while the induced smoothness of MPOT makes the trajectory tracking from the low-level controller much easier. We plan to investigate reactive motion planning with MPOT in future work.\n\n1. **What hardware is the time comparison from Table 1 being drawn?** 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 other baselines.\n\n2. **How does MPOT do in other benchmarks where RRT\\* fails to reach a solution?** In our experiments, RRT*/I-RRT* only fails to find solutions in TIAGo++ environment due to hitting the time-limit budget. We tried a very high time limit (1000 seconds); however, the dimensionality of TIAGo++ environment is too high for RRT* to explore to reach the narrow grasp point. MPOT manages to find a reasonable solution in a few seconds in this case due to being a batch gradient-free optimizer leveraging the effective Sinkhorn algorithm.\n\n3. **Why is MPOT only compared to GPMP2 in the mobile manipulation experiment, instead of considering similar baselines to the previous cases?** We conducted additional comparative experiments on the TIAGo++ environment to better support our claims, and according to the reviewer's request, the results are available in the rebuttal attachment. Apparently, CHOMP performs worse than GPMP2 and takes more iterations in a cluttered environment. However, according to the new table result, CHOMP beats GPMP2 in run time complexity in TIAGo++ environment due to its simpler update rule. Following your suggestion, we tried SGPMP and observed that we could not tune SGPMP to surpass the success rate 5/20; hence we opted for the minimum iterations to achieve the mentioned success rate. The problem lies in how SGPMP explores with constant GP variance in a high dimensional setting; thus, a possible future extension of SGPMP with adaptive explorative variance could mitigate the problem. Similarly, STOMP's exploration mechanism is even more restrictive, and we could not tune it to work in this environment. Overall, with the efficient Sinkhorn Step facilitating individual waypoint exploration, MPOT outperforms the baselines in terms of run time complexity by a considerable margin in this case while maintaining reasonable smoothness and task performance. \n\n4. **Are there other known applications for the Sinkhorn step?** To the best of our knowledge, we are the first to propose Sinkhorn Step - an optimization operator that utilizes the Sinkhorn algorithm, that connects zero-order optimization in non-convex objectives with Optimal Transport theories. We briefly mention our vision to apply Sinkhorn Step in other Machine Learning fields. For example, Sinkhorn Step can approximate gradients in SVGD [1] with a suitable choice of the polytope family. Another example in variational inference is to apply Sinkhorn Step to update a Gaussian Mixture Model as the proposal distribution to match the unormalized target distribution.\n\n[1] Liu, Qiang, and Dilin Wang. \"Stein variational gradient descent: A general purpose bayesian inference algorithm.\" Advances in neural information processing systems 29 (2016)."}]

Author rebuttal:
We thank the reviewers for their time and effort in reviewing our paper, and their constructive and positive evaluations of our work. Below are the main questions and concerns from the reviewers that we have addressed. We briefly state the discussions as follows (full answers can be found in the reviewers' rebuttal boxes): 

- **Comparison with learning-based motion planning methods**

     Learning-based motion planning methods typically utilize a dataset from previously successfully generated plans to learn generative priors for generating plans directly or using rollout samples as initialization for motion planners. In fact, the learning-based methods can naturally complement MPOT to provide even better initializations, as currently, we only use GP priors to provide random initial smooth trajectories. However, this is considered for future work. In the current scope, we focus on the proposition of the novel operator, the Sinkhorn Step, that allows us to tackle motion planning problems, resulting in an efficient trajectory optimization method producing smooth trajectories.


- **On the dimensionality of trajectory optimization problem** 

    We do not consider any movement primitives in our experiments. In all cases, we consider optimizing the full-state (the concatenation of position and velocity) trajectories batch-wise. As mentioned in the experiment description (Sec. 5.1), 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, and their velocities) for the mobile manipulation experiment. Note that the typical optimizing variable would be the whole trajectory, leading to variable dimension $T \times d$. In our vectorization setting, it would be batched $b \times T \times d$. Hence, even for the Panda case, typical full-state motion planning with multiple objectives, such as obstacle avoidance, self-collision avoidance, smoothness, goal-reaching, joint limit handling, etc., is inherently challenging due to many local minima and high computational costs for many objectives.
    

- **Baseline implementation considerations**

    We further describe our experiment settings in Appendix I. Striving for a fair comparison, in all cases, besides RRT*/I-RRT*, we reimplement all baselines in PyTorch and fine-tuned them with the vectorization option. To the best of our knowledge, the baselines are not explicitly designed for plan vectorization. Thus, their associated public codebases are implemented for single-plan querying. 


- **Additional baseline comparisons in the TIAGo++ environment**

    We conducted additional comparative experiments on the TIAGo++ environment to better support our claims, and according to the reviewer's request, the results are available in the rebuttal attachment. Following the reviewers' suggestion, we tried SGPMP and observed that we could not tune SGPMP to surpass the success rate of 5/20; hence we opted for the minimum iterations to achieve the mentioned success rate. The problem lies in how SGPMP explores with constant GP variance in a high dimensional setting; thus, a possible future extension of SGPMP with adaptive explorative variance could mitigate the problem. Similarly, STOMP's exploration mechanism is even more restrictive, and we could not tune it to work in this environment. Overall, with the efficient Sinkhorn Step facilitating individual waypoint exploration, MPOT outperforms the baselines in terms of run time complexity by a considerable margin in this case while maintaining reasonable smoothness and task performance.
    
In summary, we propose Sinkhorn Step - a batch gradient-free optimization operator formulated as an Optimal Transport problem, leveraging the efficient Sinkhorn algorithm. We then apply the Sinkhorn Step to trajectory optimization, resulting in Motion Planning via Optimal Transport (MPOT) method. MPOT is inherently a multi-modality motion planner that optimizes a batch of high-dimensional smooth trajectories on multiple non-convex objectives, exhibiting individual waypoint exploration for better local minima escape. Finally, we also preliminarily investigate the Sinkhorn Step theoretical properties under standard assumptions.

Meta-review: The paper proposes an optimal transport-based approach (MPOT) to motion planning capable of producing smooth trajectories without the need for objective gradients as are required by trajectory optimization-based methods. Integral to MPOT is the proposed zero-order parallelizable update rule that enables gradient-free optimization over a batch of trajectories. Smoothness is achieved using a Gaussian Process prior over the costs. Experiments on point-mass navigation and mobile manipulation domains demonstrate that MPOT outperforms contemporary baselines in terms of planning time and success rate.

The paper was well received by all four reviewers, who agree that real-time, gradient-free motion planning/optimization is an important problem. The proposed MPOT algorithm, including the Sinkhorn Step that enables batch gradient-free optimization, is sound and provides a novel contribution, and its effectiveness is empirically demonstrated. The reviewers initially raised a few questions/concerns including the potential limitations of requiring fixed-length sequences for batch optimization, which the authors largely resolved in their detailed responses.
Decision: Accept (poster)