Reachability-Tilted Subgoal Sampling for Hierarchical Robot Policies

Many robot systems now split control into two layers. A slow, high-level planner (often a diffusion model, a generator that turns random noise into samples by denoising it step by step) proposes where the robot should go next. A fast, low-level controller then tries to get there. The planner never asks whether the controller can actually reach its proposals, so it sometimes requests the impossible. This gets worse when a system trained in simulation is moved to a real robot. The controller's real reach is smaller and shifted relative to its simulated reach, but the planner keeps proposing from the old, simulated range.

We propose to fix this at the interface between the two layers instead of retraining either one. The planner's output is redrawn from its own distribution, tilted toward subgoals that a learned reachability score says the controller can achieve. The tilting is done with an exact Markov chain Monte Carlo sampler, a procedure that makes many small random moves and accepts or rejects each one so that its samples follow a chosen target distribution. The sampler runs in the planner's input-noise space, where the target density can be computed exactly. Recalibrating to a new robot or a new physical gap then reduces to adjusting one temperature parameter and the sampler's step size, using a handful of real rollouts. Both quantities are known to be fragile in MCMC, so the core empirical question is whether that fragility can be controlled well enough to beat simply drawing many candidates and keeping the best.

Core Claim

In two-layer robot systems, much of the drop from simulation to reality comes from a mismatch at the interface: the planner proposes subgoals that the transferred controller can no longer reach. We claim this can be corrected after training. The method samples plans from the planner's distribution reweighted by $\exp(-\beta E)$, where $E$ is a reachability energy estimated from the controller's own rollouts and $\beta$ is a temperature. The sampler is an exact, Metropolis-corrected Langevin method run in the planner's noise space. Our sharper, falsifiable prediction concerns how the gap size matters. A cheap filter that draws $N$ candidates and keeps the best will match our method when the gap is small. It will fall behind as the gap grows, because the fraction of reachable proposals shrinks, and a filter needs roughly the inverse of that fraction in samples while a chain can move toward reachable regions.

Prior Work and What It Leaves Open

Two systems motivate the problem directly. SuSIE (Zero-Shot Robotic Manipulation with Pre-Trained Image-Editing Diffusion Models) generates future camera images as subgoals for a goal-conditioned diffusion policy. Its authors state that the subgoal generator does not know what the low-level policy can do and may imagine physically impossible subgoals. DTP (Diffusion Trajectory-guided Policy for Long-horizon Robot Manipulation) instead generates 2D end-effector paths on the camera image and feeds them to a causal transformer controller. It too lacks any feasibility check. Both papers treat the diffusion sampler as a fixed component with a fixed schedule. We keep their architectures intact and change only how the plan is drawn.

We build on DTP's interface rather than SuSIE's for a specific reason. A path of about twenty 2D waypoints is a 40-dimensional object. The adaptive-MCMC work of Harnessing the Power of Reinforcement Learning for Adaptive MCMC reports that learned step-size adaptation loses value in high dimensions, so a sampler over full images would likely be too slow and too fragile.

Other work on sim-to-real transfer acts on the controller or the simulator, not the interface. DrEureka (DrEureka: Language Model Guided Sim-To-Real Transfer) uses a language model to design domain randomization, meaning random variation of physical parameters during training, for single-level policies. RGAT (Reinforced Grounded Action Transformation for Sim-to-Real Transfer) learns an action correction that makes the simulator behave like the real system. NavDP (NavDP: Learning Sim-to-Real Navigation Diffusion Policy with Privileged Information Guidance) is the closest relative. It pairs a trajectory diffusion model with a critic trained on privileged simulation data, which it uses to select safe paths. However, it ranks candidates from a fixed schedule and does not recalibrate the critic after transfer. Our method differs from NavDP in two ways: it uses an exact, step-size-adapted sampler rather than candidate ranking, and it adds explicit post-transfer recalibration.

Two papers constrain the design. RLMH, the adaptive-MCMC paper cited above, shows that tuning step sizes with acceptance rate or jump-distance rewards fails catastrophically on a small fraction of tasks. Our step-size adaptation must therefore be tested against that failure mode rather than assumed safe. Gradient Flow Sampler-based Distributionally Robust Optimization runs Langevin-type particle methods under distribution shift and reports both sensitivity to step size and a heavy cost from using many particles. This pushes us toward a single short chain with a small number of steps per plan.

Research Program

Let $z$ be the Gaussian noise fed to DTP's trajectory model. Let $\tau = D(z)$ be the path obtained by running the deterministic denoising process, so the decoder $D$ is differentiable. Let $E(\tau, s)$ be a reachability energy: the predicted tracking failure of the low-level controller on path $\tau$ from state $s$. The target we sample from is

$$ \pi_\beta(z \mid s) \propto \mathcal{N}(z; 0, I)\, \exp\big(-\beta\, E(D(z), s)\big), $$

where $\beta \ge 0$ sets how strongly reachability overrides the planner's preferences. At $\beta = 0$ the method reduces exactly to the original planner. Because the prior on $z$ is a standard Gaussian, this density is known up to a constant. That makes Metropolis-adjusted Langevin sampling exact: each move follows the gradient of the log target plus noise, then is accepted or rejected. We run $K$ such moves from a fresh noise draw at each high-level replanning event, which occurs at the planner's existing low rate. Gradients pass through $D$, so we use a few-step deterministic decoder and report wall-clock time per plan.

The reachability energy is trained in simulation. We roll the frozen DTP controller out on planner samples and on perturbed versions of them, label each path with its tracking error and task progress, and fit a small network. After transfer, the energy network stays frozen. Only $\beta$ and an additive offset are refit, by maximum likelihood on the success or failure outcomes of 20 to 50 target-domain rollouts. The step size is adapted online by targeting an acceptance rate. Because RLMH shows that this heuristic can collapse, we also log every chain whose acceptance rate falls to zero or whose energy diverges, and we compare against a fixed step size chosen in simulation.

The analysis has two parts. First, we map how success depends jointly on $\beta$ and step size at several gap magnitudes. We expect the best $\beta$ to grow with the gap, and we expect the range of usable step sizes to narrow as $\beta$ sharpens the target. Second, we measure the fraction of planner samples the controller can reach. We use this fraction to predict when filtering should match the chain and when it should fall behind.

Baselines

The naive reference point is DTP exactly as published, transferred without any change. On CALVIN with training on environments A, B and C and testing on unseen environment D, DTP reports an average of 3.43 completed instructions in a row, up from 3.06 for its baseline. It also reports a 0.84 average success rate on a real Franka robot, against 0.48 for GR-1 (DTP). For scale, SuSIE reports 2.69 instructions in a row on CALVIN zero-shot (SuSIE). We retrain DTP from the public recipe so that every method shares the same planner and controller weights. Any improvement over this reference must come from the interface alone.

The strongest competitor is best-of-$N$ filtering: draw $N$ independent plans and execute the one with the lowest recalibrated energy. This is the NavDP-style recipe with our recalibrated energy plugged in. It uses the same energy, the same 20 to 50 calibration rollouts, and a value of $N$ chosen so that the total number of decoder forward and backward passes equals $K$ chain steps. We also count a backward pass as roughly two forward passes, so the comparison is fair in compute. To beat it, the chain must achieve longer average completed sequences, with non-overlapping 95% confidence intervals across seeds, at the larger gap magnitudes.

The second competitor is standard classifier guidance. Here the energy gradient is added during denoising with a fixed scale and no accept or reject step, which is the usual way to steer diffusion models. Its guidance scale gets the same tuning budget as our $\beta$.

The third competitor acts on the controller instead. It fine-tunes the DTP controller on the same calibration rollouts, or retrains it with wider domain randomization over the perturbed parameters, in the spirit of DrEureka (DrEureka).

Three ablations isolate the key ingredients. Removing the accept or reject step, which leaves an unadjusted Langevin sampler, tests the exactness claim. Using the simulation energy without recalibration tests the transfer claim. Replacing adaptive step sizes with a fixed step size tests the role of tuning.

Why not just…?

Why not just draw many plans and keep the best? This is the real threat, and it may win when the gap is small. Our argument is that rejection-style filtering needs on the order of the inverse of the reachable fraction in samples, and that fraction shrinks with the gap. The matched-compute sweep over gap magnitude settles the question directly. If filtering stays within noise of the chain at every gap size, we drop the MCMC component. We would then report the result that a recalibrated filter at the interface is sufficient, which would still be a useful finding about where the sim-to-real loss sits.

Why not just add guidance during denoising, as is standard for diffusion models? Unadjusted guidance has no correctness guarantee, and its quality depends on a scale and a step schedule that no one tunes per domain. The step-size and stability failures that RLMH and the gradient-flow sampler paper document are exactly what we expect to see as the energy sharpens after transfer. The ablation without the accept or reject step and the guidance baseline measure this directly. If guidance matches our method at equal cost, the extra machinery is unjustified.

Why not just fix the controller with real data or wider randomization? Wider randomization is known to make contact-rich policies overly cautious, which is part of DrEureka's motivation. Fine-tuning a controller on 20 to 50 rollouts risks overfitting. Interface correction is also complementary, since it can sit on top of an improved controller. We run controller fine-tuning as a baseline and also combine it with our method. If fine-tuning alone closes the gap, the interface claim fails.

Why not just use a single end-to-end vision-language-action model? HiRT (HiRT: Enhancing Robotic Control with Hierarchical Robot Transformers) reports that a monolithic model runs at about 4 Hz and reaches 48% success on dynamic manipulation, compared with 75% for its two-layer design. That evidence argues for keeping two layers. Our question is how to correct a two-layer system once it exists, so we do not rerun this comparison.

Proposed Experiments

The main testbed is CALVIN, trained on environments A, B and C and tested on D, with the metric being average completed instruction sequence length. To control the sim-to-real gap precisely, we follow RGAT's protocol of creating "real" surrogates by changing physics. We perturb actuator gains, friction, controller latency, and camera extrinsics, meaning the camera's position and angle, at five graded magnitudes. RGAT itself reports no tabulated numbers on Lacuna, so this protocol is a methodological borrowing, not a numeric baseline. Surrogate-only validation is a known weakness of this literature, so the decisive check is a Franka hardware run on DTP's five real tasks. That run uses 30 calibration rollouts and is compared with DTP's reported 0.84 success rate.

Five measurements decide the outcome. The first is the matched-compute gap between the chain and best-of-$N$ filtering as a function of perturbation magnitude, where the claim requires a widening gap. The second is the per-plan reachable fraction, which should predict where the crossover between the two methods occurs. The third is chain failure rates under adaptive versus fixed step sizes, to check whether the RLMH failure mode appears in this setting. The fourth is how sharp the success surface is over $\beta$ and step size before and after recalibration. The fifth is wall-clock time per plan, checked against the 100 to 200 ms latency range in which HiRT reports degraded performance. If the chain fails to beat filtering at the largest gaps, or if recalibrating $\beta$ yields nothing over the simulation-trained energy, the core claim is refuted.