The problem: trajectory optimization gives smooth robot motions, but it needs cost-function gradients that often don't exist (occupancy maps, contacts), and it gets trapped in local minima. The trick (Le et al., NeurIPS 2023): around every waypoint of a batch of trajectories, probe the cost along the vertices of a regular polytope, then let a tiny optimal-transport problem — solved by Sinkhorn's algorithm — "transport" each waypoint toward the cheap directions. One update, no gradients, thousands of waypoints at once on a GPU. This page builds the method from scratch: optimal transport → the Sinkhorn Step update rule → why it converges → how it becomes a motion planner → what the experiments say.
Source: A. Le, G. Chalvatzaki, A. Biess, J. Peters, “Accelerating Motion Planning via Optimal Transport”, NeurIPS 2023 (arXiv:2309.15970). Code name: MPOT; update rule: Sinkhorn Step.
A robot motion is a discrete trajectory \(\tau = (x_0, x_1, \dots, x_T)\) — waypoints in configuration space with \(x_0\) the start. Trajectory optimization searches for the \(\tau\) that minimizes a sum of planning costs:
with an obstacle cost \(c_{\mathrm{obs}}\), a goal cost \(c_g\) pulling the end of the trajectory to \(x_g\), and a smoothness cost \(c_{\mathrm{sm}}\) so the low-level controller can actually track the plan. Every serious motion planner must handle three hard facts about this problem:
The paper's answer is a zero-order update rule — it evaluates costs, never derivatives — that updates every waypoint of every trajectory independently, with a step whose size is bounded by construction, and that is one matrix operation on a GPU. The machinery that makes "evaluate locally, then move" principled instead of heuristic is entropic optimal transport, so we build that first.
Optimal transport (OT) asks: given a pile of mass at some origins and a demand at some destinations, what is the cheapest way to ship it? In the discrete case the pile is a histogram \(n \in \Sigma_n\) — a non-negative vector summing to one, \(\Sigma_d := \{x \in \mathbb{R}^d_{+} : x^\top \mathbf{1}_d = 1\}\) — and likewise \(m \in \Sigma_m\). A transport plan is a matrix \(W \in \mathbb{R}^{n\times m}_{+}\) whose row sums are \(n\), column sums \(m\):
Entry \(W_{ij}\) is "how much mass moves from origin \(i\) to destination \(j\)". With a cost matrix \(C \in \mathbb{R}^{n\times m}_{+}\) (unit shipping cost from \(i\) to \(j\)), classic OT is the linear program
Exact OT is expensive at scale, so Cuturi (2013) added an entropy bonus that keeps the plan spread out and makes the problem strictly convex and solvable by a few matrix–vector products:
Large \(\lambda\) → plan is blurry (entropy wins); \(\lambda \to 0\) → recovers the exact LP.
Problem (1) has a beautiful structure: its optimum has the form \(W^{*}_{ij} = u_i\, P_{ij}\, v_j\) with the Gibbs kernel \(P_{ij} = e^{-C_{ij}/\lambda}\) and two positive vectors \(u, v\). Finding \(u,v\) so that the marginals match is a fixed point of two alternating normalizations — the Sinkhorn (a.k.a. Sinkhorn-Knopp) iterations:
Each pass rescales rows to match \(n\), then columns to match \(m\), then rows again; convergence is geometric. In practice one runs it in the log domain (absorbing huge factors \(u = \tilde{u}\,e^{a/\lambda}\) into log-space offsets \(a\)) because small \(\lambda\) makes \(e^{-C/\lambda}\) underflow — the paper uses exactly this stabilization (its Appendix E).
Take two origins, two destinations, unit marginals \((\tfrac12, \tfrac12)\), and cost matrix \(C = \big(\begin{smallmatrix} 1 & 4 \\ 2 & 1 \end{smallmatrix}\big)\). Origin 1 would rather ship to destination 1 (\(1 < 4\)); origin 2 to destination 2 (\(1 < 2\)). Run the iterations (2):
| \(\lambda\) | \(W^{*}_{\lambda}\) | behavior |
|---|---|---|
| \(0.1\) | \(\big(\begin{smallmatrix} 0.500 & 0.000 \\ 0.000 & 0.500 \end{smallmatrix}\big)\) | nearly the exact LP: a soft assignment — each origin picks its cheap destination |
| \(1.0\) | \(\big(\begin{smallmatrix} 0.440 & 0.060 \\ 0.060 & 0.440 \end{smallmatrix}\big)\) | blurry: every entry gets some mass; the plan is a smoothed compromise |
Two observations matter for what follows. First, at small \(\lambda\) the plan is a soft permutation — OT is secretly an assignment algorithm. Second, the column marginals are hard: each destination receives exactly total mass \(m_j\) no matter what. This "every destination gets served" property will become "every search direction gets used" — the source of the method's unbiasedness. (Both numbers above were verified by an independent Sinkhorn implementation for this page.)
Now switch hats: instead of shipping goods, we are minimizing a non-convex, \(L\)-smooth, bounded-below function \(f:\mathbb{R}^d \to \mathbb{R}\) (Assumption 1 of the paper) over a batch of \(n\) points \(X_k = \{x_1,\dots,x_n\}\), with function values cheap and derivatives expensive or unavailable. We want a batch update rule with three properties:
Center a regular polytope on each point: the \(d\)-simplex (\(d{+}1\) vertices), the \(d\)-orthoplex (\(2d\) vertices, the generalized octahedron), or the \(d\)-cube (\(2^d\) vertices), each inscribed in the unit sphere \(\mathcal{S}^{d-1}\). The unit vertex vectors \(\mathcal{D}_{\mathcal{P}} = \{d_1,\dots,d_m\}\) have two key properties:
The worst-case alignment between any vector \(a\) (in particular \(-\nabla f\)) and the best polytope vertex is quantified by the cosine measure (Lemma 1):
\(\mu_{\mathcal P}\) is a guarantee: even in the unluckiest orientation, one of the \(m\) probes aligns with the negative gradient at least to cosine \(\mu_{\mathcal P}\). Buying a bigger \(\mu_{\mathcal P}\) costs more vertices per iteration: \(d{+}1\), \(2d\), or \(2^d\) cost evaluations.
Now the punchline of the paper (Definition 2). At iteration \(k\), with step size \(\alpha_k\):
Read it from the inside out:
In free-support OT language (Cuturi & Doucet 2014), the new point is the barycentric projection of the old one under the optimal plan: the points are the "mass", the polytope vertices are the "destinations", and the cost matrix decides which destinations are cheap. In directional-search language, it is a soft, batched pattern-search step: classical direct search would take \(\arg\min_j C_{ij}\); entropic OT replaces the argmin with a \(\lambda\)-controlled softmax-like weighting while keeping the marginals hard.
A second gift of the formulation is an explicit trust region. Since each row of \(\operatorname{diag}(n)^{-1}W^{*}_{\lambda}\) is a convex combination of unit vertices, the step lies in the polytope hull: \(\lVert s_i\rVert \le \alpha_k\) — no matter what the costs say (paper's Appendix D). Gradient methods get their trust region from a regularized Taylor expansion; here the geometry of the probe set is the trust region, which is why one shared \(\alpha_k\) is safe for a whole heterogeneous batch. With \(\lambda = 0.01\) (the paper's setting) the plan is nearly a permutation, so points take nearly the full step toward their best vertex, with a small entropy-induced shrinking toward the centroid.
The figure below is a live run of Definition 2 that I implemented for this page: \(f(x) = 0.1x^2 + 1.1\sin(2.2x)\) — non-convex, four wells — a batch of 14 points, the 1-cube directions \(\{-1,+1\}\), \(\alpha = 0.3\), \(\beta = 0.85\), \(h = 6\) probes, \(\lambda = 0.05\). No derivatives anywhere.
Notice what the balanced transport did not do: it did not drag all points to the same global minimum. Points far apart have different cost rows, so the plan assigns them differently; the marginals only enforce that, in total, left-moves and right-moves are used equally. Diversity of the batch survives — which is exactly what the planner will exploit.
The paper connects the Sinkhorn Step to directional direct search (the Hooke–Jeeves/pattern-search family). Under Assumption 2 (uniform histograms with \(n = m\), and \(\lambda \to 0\)), Birkhoff's theorem says the extreme points of \(U(n,m)\) are permutation matrices — the soft assignment hardens into a bijection between points and directions — and each point's step reduces to the classical direct-search step "go to your best vertex". Two cases cover the limit: the plan is a permutation, or a convex combination of tied optimal permutations; in both, the step \(s_k\) is a weighted combination of descending steps.
Combine this with the guarantee that some vertex aligns with the negative gradient to cosine \(\mu_{\mathcal P}\) (Lemma 1), and with the \(L\)-smoothness bound \(f(x+s) \le f(x) + \langle\nabla f, s\rangle + \tfrac{L}{2}\lVert s\rVert^2\) (valid for any step \(s\)). Because \(\lVert s_k\rVert \le \alpha_k\), every point separately obeys the key descent inequality (Lemma 2):
Telescoping (12) over \(K\) steps and using \(f \ge f^{*}\) gives the main theorem (Theorem 1): if the step is small enough, \(\alpha < 2\mu_{\mathcal P}\epsilon/L\), then after
Choosing the optimal step \(\alpha = \mu_{\mathcal P}\epsilon/L\) yields the complexity \(k(\epsilon) = 2L(f(x_0)-f^{*})/(\mu_{\mathcal P}^2\epsilon^2) - 1\), and plugging in each polytope's \(\mu_{\mathcal P}\) (Remark 1):
| polytope | vertices \(m\) | \(\mu_{\mathcal P}\) | iterations to \(\epsilon\)-stationarity | cost evals per iter. |
|---|---|---|---|---|
| \(d\)-simplex | \(d{+}1\) | \(1/\sqrt{d(d{+}1)}\) | \(\mathcal{O}(d^2/\epsilon^2)\) | \(d{+}1\) |
| \(d\)-orthoplex | \(2d\) | \(1/\sqrt{d}\) | \(\mathcal{O}(d/\epsilon^2)\) | \(2d\) |
| \(d\)-cube | \(2^d\) | \(1/\sqrt{2}\) | \(\mathcal{O}(1/\epsilon^2)\) | \(2^d\) |
The \(d\)-cube matches gradient descent's famous \(\mathcal{O}(1/\epsilon^2)\) — but pays \(2^d\) evaluations per step, so it is only practical for small \(d\). The orthoplex is the workhorse for high \(d\): \(\mathcal{O}(d/\epsilon^2)\) iterations at \(2d\) evaluations each. Note the honest caveat the authors attach: the guarantee is that some iterate enters the \(\epsilon\)-stationary set; enforcing monotone descent via a sufficient-decrease check \(f(x_k)-f(x_{k+1}) \ge c\alpha_k^2\) would upgrade "some" to "the last", but wastes iterations in practice — and the experiments show convergence without it.
The Sinkhorn Step updates independent points. A trajectory is not a bag of independent points — its waypoints are glued together by dynamics and smoothness. The paper's bridge is the planning-as-inference view, which turns "smooth, dynamically feasible trajectory" into a cost term that the waypoint-wise update can carry.
Give trajectories a prior — a zero-mean Gaussian process \(q_F(\tau) = \mathcal{N}(0, K)\) built from a constant-velocity stochastic dynamics (white-noise acceleration), discretized at \(\Delta t\). Its precision is sparse: \(K^{-1} = D^\top Q^{-1} D\) with \(D\) a block first-difference matrix, so the prior density factors over pairs of neighboring waypoints (paper's Appendix B):
\(\Phi_{t,t+1}\) is the constant-velocity state transition and \(Q_{t,t+1}\) the integrated process noise. Then define the target posterior as cost-tempered prior:
\(\eta\) is an inverse temperature. Planning as inference means: find the most probable trajectory, i.e. minimize some divergence between a proposal and \(q^{*}\).
The trajectory is represented by its waypoint empirical distribution \(p(x;\tau) = \sum_{t=1}^{T} \frac{1}{T}\,\delta_{x_t}(x)\) (Eq. 8) — a histogram with one atom per waypoint. Projecting it onto \(q^{*}\) in KL (the I-projection, Eq. 9) and dropping constants (\(H(p) = \log T\) is fixed for a fixed \(T\)) yields:
This is exactly the classical trajectory-optimization objective (2) — the GP prior did not add machinery, it became the smoothness cost. That is the whole point of the inference detour: smoothness enters as pairwise costs between neighbors, not as a matrix to invert, and — crucially for us — not as something that needs gradients propagated through a kinematic chain. (Compared to GPMP2's moment projection, the paper's I-projection also removes the importance-sampling step: the model lives in the cost, the update acts directly on waypoint parameters.)
The objective (9) is a sum over waypoints of a state cost plus a term coupling each waypoint to its neighbor. To probe it locally at waypoint \(t\), the paper places \(h\) points \(y_{t,i,j}\) along direction \(i\) out to radius \(\beta_k \ge \alpha_k\), and prices direction \(i\) by the mean cost-to-go over the probes (Eq. 10):
The second term asks: if the neighbor moved along this same direction, how much would the GP link be stretched? It is what keeps the transport smooth — a waypoint's direction is cheap only if it is also compatible with its neighbor moving consistently. Each waypoint gets its own randomly rotated polytope \(R \in SO(d)\), sampled fresh every iteration: this de-biases the probe geometry (a fixed polytope would systematically miss features oriented between vertices) and decorrelates the rows of the cost matrix. All \(T\) waypoints of all \(N_p\) trajectories are flattened into one batch of \(N = N_p \cdot T\) points; one \(\mathbb{R}^{N \times m}\) OT instance updates them all.
And the trust region? Flattening Eq. (25) of the paper: since every row of the plan is a convex combination of unit vertices,
Small print, for fidelity: my demo evaluates the GP link on the probe point of the waypoint itself against both neighbors (the symmetric form of Eq. 10 — one link per row is the paper's asymmetric version, and using both is what keeps the toy batch from zig-zagging), and inflates the occupancy radius the way costmaps are conventionally inflated. Neither changes the algorithmic story; both make a 288-point toy stable.
Assembled end-to-end, the planner is short (paper's Algorithm 1):
Four practical details carry real weight:
The paper benchmarks against gradient-based planners (CHOMP, GPMP2), gradient-free stochastic ones (STOMP, SGPMP), and the sampling-based gold standards RRT*/Informed-RRT*, all reimplemented in PyTorch and tuned per environment. Metrics: planning time \(T[s]\) to convergence; task success \(\mathrm{SUC}[\%]\) (≥1 collision-free plan per task); parallelization quality \(\mathrm{GOOD}[\%]\) (fraction of the batch that succeeds); smoothness \(S\) (mean velocity jump) and path length \(PL\).
| planner | point-mass (2D, 15 obstacles, 1000 tasks) | Panda (7-DoF, 500 tasks) | ||||
|---|---|---|---|---|---|---|
| T [s] | SUC [%] | GOOD [%] | T [s] | SUC [%] | GOOD [%] | |
| RRT* | 43.2 ± 15.2 | 100 | 100 | 186.9 ± 184.2 | 100 | 73.8 |
| I-RRT* | 43.6 ± 13.8 | 100 | 100 | 184.2 ± 166.0 | 100 | 74.6 |
| STOMP | 2.2 ± 0.1 | 31.4 ± 13.9 | 10.5 ± 25.7 | 4.3 ± 0.1 | 50.8 ± 28.3 | 35.3 ± 42.0 |
| SGPMP | 6.5 ± 0.9 | 98.6 ± 4.5 | 74.9 ± 28.9 | 5.0 ± 0.2 | 67.8 ± 23.5 | 58.1 ± 45.8 |
| CHOMP | 0.5 ± 0.1 | 70.9 ± 16.7 | 38.6 ± 40.7 | 3.1 ± 0.3 | 63.0 ± 25.5 | 51.6 ± 46.2 |
| GPMP2 | 2.8 ± 0.1 | 98.3 ± 4.9 | 74.9 ± 32.1 | 3.3 ± 0.2 | 66.0 ± 25.2 | 53.2 ± 42.3 |
| MPOT | 0.4 ± 0.0 | 99.2 ± 3.1 | 73.6 ± 26.7 | 0.8 ± 0.1 | 71.6 ± 23.2 | 60.2 ± 44.4 |
How to read this. RRT\(^*\) always eventually succeeds (probabilistic completeness) but takes 40–190 s — useless for real-time, and its raw paths need post-smoothing. CHOMP is fast but fails in narrow passages: with a whole batch sharing one step size, the size that is stable for the majority overshoots the few trajectories squeezed through gaps. STOMP's Gaussian sampling variance can't produce the sharp turns dense clutter demands. SGPMP matches success rates but pays for its proposal-based updates with 6.5 s. MPOT wins time (0.4 s / 0.8 s — ~4–8× faster than the best competitor) while matching or beating success, because (i) waypoints update independently, so narrow-passage trajectories are not hostage to the batch's shared step size, and (ii) the explicit trust region makes one \(\alpha_k\) safe for everyone.
The TIAGo++ task — navigate, fetch a cup, place it on a shelf, avoid chairs — is one planning problem per phase with 128 timesteps and \(d = 36\) (base + torso + two 7-DoF arms). SDFs are painful to craft here, whole-body IK is unreliable, and RRT\(^*\) fails outright within a 1000 s budget:
| planner | first solution \(T_F\) [s] | SUC [%] | S | PL |
|---|---|---|---|---|
| RRT* / I-RRT* | 1000 (budget) — no solution | 0 | — | — |
| STOMP | — | 0 | — | — |
| SGPMP | 27.75 ± 0.29 | 25 | 0.010 | 6.69 |
| CHOMP | 16.74 ± 0.21 | 40 | 0.015 | 8.60 |
| GPMP2 | 40.11 ± 0.08 | 40 | 0.012 | 8.63 |
| MPOT | 1.49 ± 0.02 | 55 | 0.022 | 10.53 |
An 11–27× speedup at the highest success rate. The one blemish is honest and instructive: MPOT's smoothness \(S\) and path length \(PL\) are the worst of the successful planners here, because in \(d = 36\) the orthoplex's \(m = 72\) directions are a very sparse sample of the sphere — the trust region is boxy, and smoothing must be traded against success when tuning. Sparsity of the direction set is the price of gradient-freedom at high dimension.
Lineage. The Sinkhorn Step is a hybrid of three classical ideas:
Honest limitations (from the paper's own accounting): the entropic OT inner solve can still go numerically unstable for very large \(N\) (log-domain helps; rare divergence remains); the convergence theory assumes \(n = m\) and \(\lambda \to 0\) while the practice runs \(N \gg m\) at \(\lambda = 0.01\) — the analysis is a bridge to the direct-search literature, not a full account of the practical algorithm; and in high dimensions the sparse direction set forces a tune-between-smoothness-and-success compromise. The authors flag natural next steps: learned motion priors as initializers, SVGD-style diversity mechanisms inside the batch, and conditional (per-waypoint) marginals.
When would you reach for this? When costs are natural as evaluations (occupancy grids, simulators, contact checks) but not as derivatives; when you want many diverse, smooth candidate plans in one GPU round (dataset generation for policy learning, multi-modal prediction); or when a shared gradient step size across a batch is the thing breaking your trajectory optimization. If you have clean SDFs and a single well-initialized trajectory, CHOMP/GPMP2 remain excellent.
| symbol | meaning |
|---|---|
| \(f,\; L\) | objective; its Lipschitz constant of \(\nabla f\) |
| \(X_k = \{x_i\}_{i=1}^{n}\) | batch of optimizing points (flattened waypoints) |
| \(\mathcal{D}_{\mathcal P},\; m\) | polytope vertex directions; number of vertices |
| \(C \in \mathbb{R}^{n\times m}\) | cost matrix: probe evaluations |
| \(n \in \Sigma_n,\; m \in \Sigma_m\) | histograms over points / directions (uniform in practice) |
| \(U(n,m)\) | transport plans with marginals \(n, m\) |
| \(\lambda\) | entropic regularization (\(0.01\) in MPOT) |
| \(\alpha_k, \beta_k, h\) | step radius, probe radius, probes per direction |
| \(\mu_{\mathcal P}\) | cosine measure of the polytope (Lemma 1) |
| \(\tau = \{x_t\}_{t=0}^{T}\) | discrete trajectory; \(T\) waypoints |
| \(\Phi_{t,t+1},\, Q_{t,t+1}\) | GP prior transition / integrated noise covariance |
| \(\eta\) | inverse temperature on the planning cost |
| \(N_p,\; N\) | number of parallel trajectories; total waypoints \(N_p \cdot T\) |
All simulation figures on this page were generated by an independent implementation of the Sinkhorn Step following the paper's definitions (solver cross-checked against a reference implementation; marginals verified to \(10^{-8}\)). Figures 2–4 use the demo setups described in their captions; Figure 5 plots the paper's published Table 1 numbers.