SlowGuess/NeurIPS_2023_OCR huggingface dataset:
An T. Le<sup>1</sup>, Georgia Chalvatzaki<sup>1,3,5</sup>, Armin Biess, Jan Peters<sup>1-4</sup>
$^{1}$ Department of Computer Science, Technische Universitat Darmstadt, Germany  $^{2}$ German Research Center for AI (DFKI)  $^{3}$ Hessian.AI  $^{4}$ Centre for Cognitive Science
$^{5}$ Center for Mind, Brain and Behavior, Uni. Marburg and JLU Giessen, Germany
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 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 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.
Motion planning is a fundamental problem for various domains, spanning robotics [1], autonomous driving [2], space-satellite 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 sampling-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 configuration and task space, and the requirement for smooth trajectories that low-level controllers can effectively execute. Sampling-based methods sample the high-dimensional manifold of configurations and use different search techniques to find a 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 (e.g., highly redundant robots and narrow passages) [11]. Optimization-based approaches work on a 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 to the non-convexity of complex objectives. Moreover, in some problem settings, objective gradients are unavailable or expensive to compute. Indeed, trajectory optimization is difficult to tune and is often avoided in favor of sampling-based methods with probabilistic completeness. We refer to Appendix H for an extensive discussion of related works.
Figure 1: 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.google.com/view/ sinkhorn-step/
To address these issues of trajectory optimization, we propose a zero-order, fast, and highly parallelizable 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 optimization 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-inference perspective [15, 13] allows us to impose constraints related to transition dynamics as planning costs, additionally imposing smoothness through a Gaussian Process (GP) prior. Delegating complex constraints to the planning objective allows us 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 of MPOT in a 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 a batch of parameters, formulated as a 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 a batch of trajectories by efficiently solving a sequence of linear programs. It treats every waypoint across trajectories equally, enabling fast batch updates of multiple trajectories-waypoints over multiple goals by solving a single OT instance while retaining smoothness due to integrating the GP prior as cost function.
Entropic-regularized optimal transport. We briefly introduce discrete OT. For a thorough introduction, we refer to [16-18].
Notation. Throughout the paper, we consider the optimization on a  $d$ -dimensional Euclidean space  $\mathbb{R}^d$ , representing the parameter space (e.g., a system state space).  $\mathbf{1}_d$  is the vector of ones in  $\mathbb{R}^d$ . The scalar product for vectors and matrices is  $x,y\in \mathbb{R}^d$ ,  $\langle x,y\rangle = \sum_{i = 1}^{d}x_{i}y_{i}$ ; and  $\mathbf{A},\mathbf{B}\in \mathbb{R}^{d\times d}$ ,  $\langle \mathbf{A},\mathbf{B}\rangle = \sum_{i,j = 1}^{d}\mathbf{A}_{ij}\mathbf{B}_{ij}$ , respectively.  $\| \cdot \|$  is the  $l_2$ -norm, and  $\| \cdot \|_M$  denotes the Mahalanobis norm w.r.t. some positive definite matrix  $\mathbf{M}\succ 0$ . For two histograms  $\mathbf{n}\in \Sigma_n$  and  $\mathbf{m}\in \Sigma_m$  in the simplex  $\Sigma_d\coloneqq \{\pmb {x}\in \mathbb{R}_+^d:\pmb{x}^\intercal \mathbf{1}_d = 1\}$ , we define the set  $U(\pmb {n},\pmb {m})\coloneqq \{\pmb {W}\in \mathbb{R}_+^{n\times m}\mid \pmb {W}\pmb {1}_m = \pmb {n},\pmb {W}^\intercal \pmb {1}_n = \pmb {m}\}$  containing  $n\times m$  matrices with row
and column sums  $\mathbf{n}$  and  $\mathbf{m}$  respectively. Correspondingly, the entropy for  $\mathbf{A} \in U(\mathbf{n},\mathbf{m})$  is defined as  $H(\mathbf{A}) = -\sum_{i,j=1}^{n,m} a_{ij} \log a_{ij}$ .
Let  $C \in \mathbb{R}_+^{n \times m}$  be the positive cost matrix, the OT between  $n$  and  $m$  given cost  $C$  is  $\mathrm{OT}(n, m) \coloneqq \min_{W \in U(n, m)} \langle W, C \rangle$ . Traditionally, OT does not scale well with high dimensions. To address this, Cuturi [19] proposes to regularize its objective with an entropy term, resulting in the entropic-regularized OT
$$
\mathrm {O T} _ {\lambda} (\boldsymbol {n}, \boldsymbol {m}) := \min  _ {\boldsymbol {W} \in U (\boldsymbol {n}, \boldsymbol {m})} \langle \boldsymbol {W}, \boldsymbol {C} \rangle - \lambda H (\boldsymbol {W}). \tag {1}
$$
Solving (1) with Sinkhorn-Knopp [19] has a complexity of  $\tilde{O}(n^2/\epsilon^3)$  [20], where  $\epsilon$  is the approximation error w.r.t. the original OT. Higher  $\lambda$  enables a faster but "blurry" solution, and vice versa.
Trajectory optimization. Given a parameterized trajectory by a discrete set of support states and control inputs  $\pmb{\tau} = [\mathbf{x}_0, \mathbf{u}_0, \dots, \mathbf{x}_{T-1}, \mathbf{u}_{T-1}, \mathbf{x}_T]^{\top}$ , trajectory optimization aims to find the optimal trajectory  $\pmb{\tau}^*$ , which minimizes an objective function  $c(\pmb{\tau})$ , with  $\mathbf{x}_0$  being the start state. Standard motion planning costs, such as goal cost  $c_g$  defined as the distance to a desired goal-state  $\mathbf{x}_g$ , obstacle avoidance cost  $c_{obs}$ , and smoothness cost  $c_{sm}$  can be included in the objective. Hence, trajectory optimization can be expressed as the sum of those costs while obeying the dynamics constraint
$$
\boldsymbol {\tau} ^ {*} = \arg \min  _ {\boldsymbol {\tau}} \left[ c _ {o b s} (\boldsymbol {\tau}) + c _ {g} (\boldsymbol {\tau}, \mathbf {x} _ {g}) + c _ {s m} (\boldsymbol {\tau}) \right] \text {s . t .} \dot {\boldsymbol {x}} = f (\mathbf {x}, \boldsymbol {u}) \text {a n d} \boldsymbol {\tau} (0) = \boldsymbol {x} _ {0}. \tag {2}
$$
For many manipulation tasks with high-DoF robots, this optimization problem is typically highly nonlinear due to many complex objectives and constraints. Besides  $c_{obs}$ ,  $c_{sm}$  is crucial for finding smooth trajectories for better tracking control. Covariant Hamiltonian Optimization for Motion Planning (CHOMP) [7] designs a finite difference matrix  $M$  resulting in the smoothness cost  $c_{sm} = \tau^{\top}M\tau$ . 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-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.
3 Sinkhorn Step
To address the problem of batch trajectory optimization in a 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 a 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 two components: a polytope structure defining the unbiased search-direction bases, and a weighting distribution for evaluating 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-regularized 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-direct 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  _ {X} f (X) = \min  _ {X} \sum_ {i = 1} ^ {n} f \left(\boldsymbol {x} _ {i}\right), \tag {3}
$$
where  $X = \{\pmb{x}_i\}_{i=1}^n$  is a set of  $n$  optimizing points,  $f: \mathbb{R}^d \to \mathbb{R}$  is non-convex, differentiable, bounded below, and has  $L$ -Lipschitz gradients.
Assumption 1. The objective  $f$  is  $L$ -smooth with  $L > 0$
$$
\| \nabla f (\boldsymbol {x}) - \nabla f (\boldsymbol {y}) \| \leq L \| \boldsymbol {x} - \boldsymbol {y} \|, \forall \boldsymbol {x}, \boldsymbol {y} \in \mathbb {R} ^ {d}
$$
and bounded below by  $f(\pmb {x})\geq f_{*}\in \mathbb{R},\forall \pmb {x}\in \mathbb{R}^{d}$
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 a set of initial points  $X_0 \sim \mathcal{D}_0$ , with its matrix form  $\pmb{X}_0 \in \mathbb{R}^{n \times d}$ , from some prior distribution  $\mathcal{D}_0$ . The goal is to compute a 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 search-direction-set  $D$ , ensuring descent with a sufficiently small stepsize. The search-direction-set is typically a vector-set requiring to be a positive spanning set [28], i.e., its conic hull is  $\mathbb{R}^d = \{\sum_i w_i \pmb{d}_i, \pmb{d}_i \in D, w_i \geq 0\}$ , ensuring that every point (including the extrema) in  $\mathbb{R}^d$  is reachable by a sequence of positive steps from any initial point.
Regular Polytope Search-Directions. Consider a  $(d - 1)$ -unit hypersphere  $S^{d - 1} = \{\pmb {x}\in \mathbb{R}^d:$ $\| \pmb {x}\| = 1\}$  with the center at zero.
Definition 1 (Regular Polytope Search-Directions). Let us denote the regular polytope family  $\mathcal{P} = \{\text{simplex, orthoplex, hypercube}\}$ . Consider a  $d$ -dimensional polytope  $P \in \mathcal{P}$  with  $m$  vertices, the search-direction set  $D^{P} = \{\pmb{d}_{i} \mid \| \pmb{d}_{i} \| = 1\}_{i=1}^{m}$  is constructed from the vertex set of the regular polytope  $P$  inscribing  $S^{d-1}$ .
The  $d$ -dimensional regular polytope family  $\mathcal{P}$  has all of its dihedral angles equal and, hence, is an unbiased sparse approximation of the circumscribed  $(d - 1)$ -sphere, i.e.,  $\sum_{i} \boldsymbol{d}_{i} = 0$ ,  $\| \boldsymbol{d}_{i} \| = 1 \forall i$ . There also exist other regular polytope families. However, the regular polytope types in  $\mathcal{P}$  exist in every dimension (cf. [29]). Moreover, the algorithmic construction of general polytope is not trivial [30]. Vertex enumeration for  $\mathcal{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.  $\forall P\in \mathcal{P}$ $D^{P}$  forms a positive spanning set.
This property ensures that any point  $\pmb{x} \in \mathbb{R}^d$ ,  $\pmb{x} = \sum_{i} w_i \pmb{d}_i$ ,  $w_i \geq 0$ ,  $\pmb{d}_i \in D^P$  can be represented by a positively weighted sum of the set of directions defined by the polytopes.
Batch Update Rule. At an iteration  $k$ , given the current optimizing points  $X_{k}$  and their matrix form  $X_{k} \in \mathbb{R}^{n \times d}$ , we first construct the direction set from a chosen polytope  $P$ , and denote the direction set  $D^{P} \in \mathbb{R}^{m \times d}$  in matrix form. Similar to [25], let us define the prior histograms reflecting the importance of optimizing points  $n \in \Sigma_{n}$  and the search directions  $m \in \Sigma_{m}$ , then the constraint space  $U(n, 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 4.11, [17]) that optimizes the free-support barycenter of the optimizing points and the batch polytope vertices
$$
\boldsymbol {X} _ {k + 1} = \boldsymbol {X} _ {k} + \boldsymbol {S} _ {k}, \boldsymbol {S} _ {k} = \alpha_ {k} d i a g (\boldsymbol {n}) ^ {- 1} \boldsymbol {W} _ {\lambda} ^ {*} \boldsymbol {D} ^ {P}
$$
$$
s. t. \boldsymbol {W} _ {\lambda} ^ {*} = \operatorname {a r g m i n} _ {\boldsymbol {W} \in U (\boldsymbol {n}, \boldsymbol {m})} \langle \boldsymbol {W}, \boldsymbol {C} \rangle - \lambda H (\boldsymbol {W}), \tag {4}
$$
with  $\alpha_{k} > 0$  as the stepsize,  $C\in \mathbb{R}^{n\times m}$ ,  $C_{i,j} = f(\pmb {x}_i + \alpha_k\pmb {d}_j)$ ,  $\pmb {x}_i\in X_k,\pmb {d}_j\in D^P$  is the local objective matrix evaluated at the linear-translated polytope vertices.
Observe that the matrix  $\mathrm{diag}(n)^{-1}W_{\lambda}^{*}$  has  $n$  row vectors in the simplex  $\Sigma_{m}$ . The batch update transports  $\mathbf{X}$  to a barycenter shaping by the polytopes with weights defined by the optimal solution  $W_{\lambda}^{*}$ . 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 1,  $D^{P}$  is a positive spanning set, thus,  $W_{\lambda}^{*}$  forms a generalized barycentric coordinate, defined w.r.t. the finite set of polytope vertices. This property implies any point in  $\mathbb{R}^d$  can be reached by a sequence of Sinkhorn Steps. For the  $d$ -simplex case, any point inside the convex hull can be identified with a 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.
Assumption 2. At any iteration  $k > 0$ , the prior histogram on the optimizing points and the search direction set is uniform  $n = m = 1_n / n$ , having the same dimension  $n = m$ . Additionally, the entropic scaling approaches zero  $\lambda \rightarrow 0$ .
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 a conditional Sinkhorn Step, which is interesting to 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  $\pmb{x}_k \in X_k$  separately, given that they follow the Sinkhorn Step rule.
Theorem 1 (Main result). If Assumption 1 and Assumption 2 hold and the stepsize is sufficiently small  $\alpha_{k} = \alpha$  with  $0 < \alpha < 2\mu_{P}\epsilon /L$ , then with a sufficient number of iterations
$$
K \geq k (\epsilon) := \frac {f \left(\boldsymbol {x} _ {0}\right) - f _ {*}}{\left(\mu_ {P} \epsilon - \frac {L \alpha}{2}\right) \alpha} - 1, \tag {5}
$$
we have  $\min_{0\leq k\leq K}\| \nabla f(\pmb {x}_k)\| \leq \epsilon ,\forall \pmb {x}_k\in X_k$
Note that we do not make any additional assumptions on  $f$  besides the smoothness and boundedness, and the analysis is performed on non-convex settings. Theorem 1 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, we implement the sufficient decreasing condition  $f(\pmb{x}_k) - f(\pmb{x}_{k + 1})\geq c\alpha_k^2$ , then  $f(\pmb{x}_K)\leq f(\pmb{x}_i)$ ,  $\| \nabla f(\pmb{x}_i)\| \leq \epsilon$  holds. However, this sufficient decrease check may waste some iterations and worsen the performance. We show in the experiments that the algorithm empirically exhibits convergence behavior without this condition checking. If  $L$  is known, then we can compute the optimal stepsize  $\alpha = \mu_P\epsilon /L$ , leading to the complexity bound  $k(\epsilon) = \frac{2L(f(\pmb{x}_0) - f_*)}{\mu_P^2\epsilon^2} -1$ .
Therefore, the complexity bounds for  $d$ -simplex,  $d$ -orthoplex and  $d$ -cube are  $O(d^2/\epsilon^2)$ ,  $O(d/\epsilon^2)$ , and  $O(1/\epsilon^2)$ , respectively. The  $d$ -cube case shows the same complexity bound  $O(1/\epsilon^2)$  as the well-known gradient descent complexity bound on the L-smooth function [32]. These results are detailed in Appendix A. Generally, we perform a preliminary study on Sinkhorn Step with Assumption 1 and Assumption 2 to connect the well-known directional-direct search literature [26, 27], as many unexplored theoretical properties of Sinkhorn Step remain in practical settings described in Section 4.
4 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 (cf. Definition 2), smoothly transporting the waypoints according to the local polytope structure. To promote smoothness and dynamically feasible trajectories, we incorporate the GP prior as a cost via the planning-as-inference perspective.
4.1 Planning As Inference With Empirical Waypoint Distribution
Let us consider general discrete-time dynamics  $\mathbf{X} = F(\mathbf{x}_0, \mathbf{U})$ , where  $\mathbf{X} = [\mathbf{x}_0, \dots, \mathbf{x}_T]$  denotes the states sequence,  $\mathbf{U} = [u_0, \dots, u_T]$  is the control sequence, and  $\mathbf{x}_0$  is the start state. The target distribution over control trajectories  $\mathbf{U}$  can be defined as the posterior [33]
$$
q (\boldsymbol {U}) = \frac {1}{Z} \exp (- \eta E (\boldsymbol {U})) q _ {0} (\boldsymbol {U}), \tag {6}
$$
with  $E(\mathbf{U})$  the energy function representing control cost,  $q_{0}(\mathbf{U}) = \mathcal{N}(\mathbf{0},\boldsymbol{\Sigma})$  a zero-mean normal prior,  $\eta$  a scaling term (temperature), and  $Z$  the normalizing scalar.
Assuming a first-order trajectory optimization<sup>1</sup>, the control sequence can be defined as a time-derivative of the states  $\mathbf{U} = [\dot{\mathbf{x}}_0, \dots, \dot{\mathbf{x}}_T]$ . The target posterior distribution over both state-trajectories and their derivatives  $\pmb{\tau} = (\pmb{X}, \pmb{U}) = \{\pmb{x}_t \in \mathbb{R}^d : \pmb{x}_t = [\mathbf{x}_t, \dot{\mathbf{x}}_t]\}_{t=0}^T$  is defined as
$$
q ^ {*} (\boldsymbol {\tau}) = \frac {1}{Z} \exp (- \eta c (\boldsymbol {\tau})) q _ {F} (\boldsymbol {\tau}), \tag {7}
$$
<sup>1</sup>We describe first-order formulation for simplicity. However, this work can be extended to second-order systems similar to [8].