Certified Warm Starts for Few-Step Diffusion Policies

Diffusion policies are robot controllers that produce an action sequence by starting from random noise and denoising it step by step. They imitate human demonstrations well, but they are slow, because each decision needs dozens of network passes. The fastest current versions skip most of those steps. They start from a "warm start," a guess that is already close to a good action, and then run only two to five denoising steps. The warm start comes from a small neural predictor or a hand-written heuristic, and neither one knows about obstacles. When the guess is wrong near an obstacle, two to five steps are not enough to repair it.

We propose to produce the warm start with a global polynomial solver instead. The solver is a GPU branch-and-bound method over Bernstein polynomials, which repeatedly splits a small search box and discards pieces that provably cannot contain a feasible optimum. It works in a deliberately tiny space: one to four coordinates of a mid-horizon keypoint of a spline trajectory. In that space it can list every distinct feasible solution, for example "pass left of the obstacle" and "pass right," each with a certificate that the spline clears all obstacles. Each of these goes to the diffusion policy as a separate warm start, alongside the usual learned prediction. After denoising, the same Bernstein bounds check whether the output is still feasible. If it is not, the robot executes the certified spline instead. The polynomial machinery is the mechanism. Few-step diffusion latency is the setting it operates in and the budget it must respect.

Core Claim

Our claim is that, for obstacle-constrained manipulation and navigation, a handful of solver-certified warm starts plus two denoising steps reaches the constraint satisfaction of trajectory-optimization samplers like HardFlow at the latency of STEP. It should also beat equally sized populations of random or learned warm starts, because the solver enumerates the separate feasible regions that random samples rarely cover. The construction follows the logic of TS-roots. TS-roots uses exact 1D Chebyshev rootfinding to build an "exploration set" of guaranteed local optima and pairs it with an "exploitation set" of known good points. It reports that one point from each set usually suffices to find a global optimum. Here the exploration set comes from Bernstein branch-and-bound over the spline keypoint, and the exploitation set is the learned STEP warm start.

Relation to Prior Work

Two papers supply the method. PCBA/RTD* solves receding-horizon planning problems written as polynomials to global optimality within strict time bounds, and it beats fmincon on optimality and on emergency-braking frequency. It is shown only up to 4 dimensions, its patch count explodes beyond that, and it never interacts with learned or multimodal behavior. We keep it inside that 4-D regime on purpose and let the diffusion model carry the high-dimensional, demonstration-shaped detail. TS-roots supplies the exploration/exploitation pairing.

Three papers motivate the proposal by exposing the gap. STEP warm-starts denoising with a Transformer predictor and runs 2–4 steps in about 20 ms. It proves that denoising converges if the start lies inside a local basin around the data, but it cannot check whether a given start does, and it needs ad-hoc "velocity kicks" to escape stalls. BRIDGER bridges from heuristic or variational-autoencoder priors to the expert distribution and reports 0.75 success versus DDIM's 0.17 at 5 steps on Adroit Relocate. Its priors are also blind to constraints. Motion Planning Diffusion shows that diffusion over B-spline control points represents 7-DoF motion well, which justifies our spline interface, but it relies on local gradient guidance.

Two papers constrain the design. HardFlow finds that projecting intermediate noisy samples onto the feasible set harms sample quality. We therefore touch only the starting point and the terminal check, never the intermediate states. HardFlow itself reaches perfect safety through per-step model-predictive optimization, which costs time; we test whether the same safety can be bought once, up front. GDP shows that a population of 2-step candidates scored and selected in parallel already reaches 100% success on Adroit. That makes population selection the method to beat, and the open question is whether where the population starts matters once constraints are present.

Research Program

At each control step, a fixed polynomial program is posed over a keypoint $z \in [-1,1]^m$ with $m \le 4$. Examples are a 2-D mid-horizon position plus a 2-D velocity for navigation, or a 3-D end-effector waypoint for an arm. A cubic Bézier spline passes through the current state, $z$, and the horizon end. Obstacles are written as polynomial inequalities on that spline, and clearance is enforced by Bernstein coefficient bounds over the spline's time interval, so it holds in continuous time. The cost is

$$J(z) = c_{\text{task}}(z) + \lambda \,\lVert z - \hat z \rVert^2,$$

where $c_{\text{task}}$ is a polynomial progress-plus-smoothness cost, $\hat z$ is the keypoint read off the learned STEP warm start, and $\lambda$ controls how far certified starts may stray from demonstration-like behavior. The $\lambda$ term addresses the main risk: a certified but unnatural start may fall outside STEP's convergence basin.

We modify the branch-and-bound so that it returns up to $K$ surviving boxes from disjoint feasible components, rather than a single minimizer. Each returned keypoint becomes a spline, which is converted to an action sequence. That sequence is noised to the late diffusion time and denoised for two steps, batched together with the STEP start. The candidate with the lowest cost that also passes the Bernstein clearance check on a spline fit of its output is executed. If none passes, the best certified spline itself is executed, which gives a fail-safe in the RTD style. The policy network is not retrained. We will additionally measure the per-step solver budget, the fallback rate, and how often the realized output leaves the certified region.

Baselines

The naive reference is the same pretrained policy run with DDIM at 2 and 5 steps from Gaussian noise. BRIDGER's reported Adroit Relocate number shows how badly this can fail: 0.17 versus 0.75. The strongest latency baseline is STEP, run with the official STEP code, the same policy weights, and the same 2 steps. STEP reports about 20 ms end-to-end and a success rate 21.6% higher than BRIDGER on RoboMimic. To beat it, we must reach higher success and lower violation rate on the obstacle tasks at no more than a 1.5× latency budget.

The strongest constraint-satisfaction baseline is HardFlow, which reports a safety rate of 1.00 on D3IL and Maze2D. No reported result was found on Lacuna for HardFlow's per-decision wall-clock time in milliseconds, so we will measure it ourselves on identical hardware. To beat it, we must match its safety while cutting median and 99th-percentile latency. The strongest population baseline is GDP's genetic selection, given exactly $K+1$ candidates and the same scoring plus our clearance check, so that the batch size is equal.

Three ablations isolate the key ingredient. In the first, the solver's keypoints are replaced with $K$ random feasible keypoints found by rejection sampling. In the second, they are replaced with the result of a local solver started from $\hat z$. In the third, $\lambda=0$. Each of these keeps everything else fixed. If certification and mode enumeration are what matter, random and local starts should lose specifically on episodes where the learned start sits on the wrong side of an obstacle.

Why not just…?

Why not just run a local optimizer from the learned warm start? PCBA/RTD* reports that fmincon gets trapped in local minima and has erratic solve times that cause emergency braking. A local optimizer started from $\hat z$ inherits $\hat z$'s choice of homotopy class, which is exactly the failure we target. The local-solver ablation tests this directly.

Why not just use a larger GDP-style population of random starts? This is the real threat, given GDP's 100% Adroit result. On obstacle-free tasks we expect no advantage and will say so. Our bet is narrower: when feasible regions are thin, random populations need many members to cover each one, whereas the solver returns one representative per region. We will plot success against population size for both methods. If random populations reach parity at an equal batch size on Maze2D and D3IL, the polynomial component is not justified.

Why not just enforce constraints during sampling as HardFlow does? HardFlow already achieves perfect safety, so it is the quality ceiling. Its cost is per-step optimization, which it partly hides by skipping early steps. If HardFlow restricted to 2 steps matches our safety at equal latency, the up-front approach adds nothing, and we will run exactly that configuration.

Why not just execute the certified spline and drop diffusion? On contact-rich or multimodal tasks, a 4-D polynomial plan cannot express demonstration style. That is the lesson of Pearce et al., where regression-style and averaged policies lose badly to diffusion (44% versus 89% on Kitchen). The spline-only fallback will be reported as its own row.

Proposed Experiments

The primary benchmarks are Maze2D and D3IL obstacle avoidance, which are HardFlow's constrained settings. On these we measure success, the continuous-time violation rate, median and p99 latency, and the fallback rate. The secondary benchmarks are RoboMimic (Lift, Can, Square) and Adroit Relocate, used as a do-no-harm check where constraints are trivial. There we compare against STEP and GDP at 2 steps. We also add obstacle-augmented versions of Square and Push-T to test whether gains appear only when constraints bind.

The decisive check is a stratified analysis. Episodes are split by whether STEP's learned start violates clearance. If the proposal works, nearly all of its gain over STEP and over equal-size random populations should come from the violating stratum, while the solver adds under 5 ms at $m\le4$. If the gain is spread evenly, or vanishes against matched random populations, the mechanism is not what we claim. A second check sweeps $\lambda$ to test the STEP-basin concern. If success collapses as $\lambda\to 0$ even though every start is certified, that confirms that feasibility alone does not guarantee convergence in two steps, which would be a useful result in its own right.