Safe Streaming Flow Planning by Aligning Sampling Dynamics with Execution Dynamics

Seunghwan Jang; Jeongyong Yang; Siddharth Ancha; SooJean Han

cs.ROOct 2, 2026

Safe Streaming Flow Planning by Aligning Sampling Dynamics with Execution Dynamics

Authors: Seunghwan Jang, Jeongyong Yang, Siddharth Ancha, SooJean Han

Organizations: Nanyang Technological University Singapore · KAIST Daejeon, Republic of Korea · University of Washington Seattle, WA, USA · University of California, Berkeley Berkeley, CA, USA

Abstract

Generative planners based on diffusion/flow matching can learn to synthesize long-horizon trajectories from demonstrations. However, real-world deployment requires (i) enforcing safety constraints during execution and (ii) tight online replanning at fast execution rates. Prior safe diffusion/flow planners generate the agent's full trajectory at once, while repeatedly perturbing intermediate states to satisfy safety constraints. This approach is not only computationally intensive, but also introduces distribution shift since the learned sampling dynamics is distinct from the system's execution dynamics. We propose SafeStreamingFlow, a goal-conditioned planner that aligns flow sampling dynamics with execution dynamics by sequentially integrating a learned state vector field with hierarchical state prediction. Importantly, we need to enforce safety constraints only for the executed step via high order control barrier functions. Across navigation, racing, and locomotion benchmarks, SafeStreamingFlow reduces planning latency and improves safety compared to existing methods, while maintaining competitive goal-reaching success.

Figures & tables

Figure 1: SafeStreamingFlow aligns flow sampling dynamics with execution dynamics. (a) Trajectory-level safe planners denoise an entire trajectory along a sampling process distinct from the system’s execution dynamics (blue and orange arrows misaligned). Because these intermediate sampling states are not physically executed, enforcing safety during sampling disrupts dynamical consistency and requires $\mathcal{O}(HK)$ optimization solves. (b) SafeStreamingFlow integrates a learned state vector field in physical time, so each sampling step coincides with an executed step (blue and orange aligned). A safety constraint imposed on the sampling dynamics is therefore a constraint on the executed state under the system model, yielding per-step safety enforcement with $\mathcal{O}(H)$ optimization solves (versus $\mathcal{O}(HK)$ ), closed-loop execution without architectural or computational change, and dynamically consistent plans whose executed trajectory matches the generated one by construction.

Paradigm	Cost-fn. free	Multimodal	Exec. safety	Real-time
Classical (RRT, A)	✗	✗	✓	$\triangle$
Generative (Diffuser, SafeDiffuser)	✓	✓	$\triangle$	✗
SafeStreamingFlow (Ours)	✓	✓	✓	✓

Table 1: Conceptual positioning of SafeStreamingFlow against existing planning paradigms.

Figure 2: One step of SafeStreamingFlow , following Eqs. ( 15 )–( 18 ).

		Umaze ( $H=128$ ; $K=64$ )					Medium ( $H=256$ ; $K=256$ )					Large ( $H=384$ ; $K=256$ )
Category	Method	Succ.	$\bar{h}_{\min}{>}0$	Viol.	Sm.	Trap	Succ.	$\bar{h}_{\min}{>}0$	Viol.	Sm.	Trap	Succ.	$\bar{h}_{\min}{>}0$	Viol.	Sm.	Trap
Classical	RRT* [ 18 ]	91.0	✓	5.0	$1.980{\scriptstyle\pm 2.582}$	-	94.0	✓	1.0	$0.629{\scriptstyle\pm 0.678}$	-	86.0	✓	8.0	$0.592{\scriptstyle\pm 0.548}$	-
Classical	A* [ 10 ]	93.0	✓	7.0	$2.613{\scriptstyle\pm 3.404}$	-	85.0	✓	14.0	$1.377{\scriptstyle\pm 1.143}$	-	85.0	✓	6.0	$0.689{\scriptstyle\pm 0.628}$	-
Diffusion	Diffuser [ 14 ]	$94.0^{\dagger}$	-	38.0	$57.560{\scriptstyle\pm 19.586}$	-	$94.0^{\dagger}$	-	53.0	$59.995{\scriptstyle\pm 11.655}$	-	$74.0^{\dagger}$	-	65.0	$59.939{\scriptstyle\pm 8.099}$	-
	Diffuser + CG [ 7 ]	36.0	✓	51.0	$302.012{\scriptstyle\pm 375.947}$	82.0	2.0	✓	56.0	$6.87\!\times\!10^{3}{\scriptstyle\pm 6.00\!\times\!10^{4}}$	100.0	1.0	✓	37.0	$4.49\!\times\!10^{4}{\scriptstyle\pm 4.39\!\times\!10^{5}}$	100.0
	SafeDiffuser [ 28 ]	67.0	✓	33.0	$107.749{\scriptstyle\pm 190.770}$	24.0	49.0	✓	46.0	$59.366{\scriptstyle\pm 9.082}$	0.0	13.0	✓	71.0	$70.873{\scriptstyle\pm 30.022}$	18.0

Table 2: Maze2D results across Umaze, Medium, Large maps.

Budapest (

H{=}696;\ K{=}512

)

Catalunya (

H{=}724;\ K{=}512

)

Appendix

Symbol	Meaning
$\mathbf{x}=(\mathbf{p},\mathbf{v})$	state: position and velocity
$\mathbf{u}$ , $\mathcal{U}$	control input and admissible input set
$f,g$ ( $\tilde{f},\tilde{g}$ )	drift and input terms of the dynamics (of the velocity equation)
$T$ , $H$ , $\Delta t=T/H$	horizon time, number of steps, sampling time
$k$ , $\mathcal{T}=\{0,\ldots,H-1\}$	discrete step and its index set
$K$	sampling steps of a trajectory-level baseline

Appendix

Table 6: Notation.

Figure 4: Position prediction loss during training. The FSP quickly reaches near-zero loss by exploiting the kinematic shortcut, whereas HSP converges more slowly to a higher plateau.

	Umaze ( $H{=}128$ )		Medium ( $H{=}256$ )		Large ( $H{=}384$ )
Method	Open	Closed	Open	Closed	Open	Closed
FSP	80.2	35.0	51.1	10.8	39.6	5.8
HSP (ours)	94.8	99.7	92.4	91.9	88.6	89.7

Appendix

Table 7: Ablation on HSP and FSP for the kinematic shortcut: goal-reaching success (%) across Maze2D maps, open- and closed-loop, without the safety filter. Evaluated over

1{,}000

rollouts.

$\sigma_{0}$	Goal	Viol.	Succ.
$0$	$81.3{\scriptstyle\pm 4.2}$	$2.0{\scriptstyle\pm 1.7}$	$81.3{\scriptstyle\pm 4.2}$
$0.005$	$85.0{\scriptstyle\pm 4.0}$	$3.0{\scriptstyle\pm 0.0}$	$85.0{\scriptstyle\pm 4.0}$

Appendix

Table 8: Training noise

\sigma_{0}

on Maze2D Large (closed loop, safety filter on; mean

\pm

std over three training seeds,

100

rollouts each; violations counted at

\kappa=0

Map	floor $\\|\mathbf{r}\\|^{2}$	frac	FSP MSE	HSP MSE	FSP / HSP / floor
Umaze	$2.69\mathrm{e}{-5}$	$2.1\mathrm{e}{-5}$	$1.31\mathrm{e}{-5}$	$7.49\mathrm{e}{-4}$	$0.49$ / $27.8$
Medium	$1.50\mathrm{e}{-5}$	$3.7\mathrm{e}{-6}$	$5.81\mathrm{e}{-6}$	$8.11\mathrm{e}{-4}$	$0.39$ / $53.9$
Large	$1.27\mathrm{e}{-5}$	$1.3\mathrm{e}{-6}$	$4.76\mathrm{e}{-6}$	$8.99\mathrm{e}{-4}$	$0.37$ / $70.6$

Appendix

Table 9: E1/E1b. Data residual floor and per-model next-position MSE (physical units). The shortcut is

5

–

6

orders below

\mathrm{Var}(\mathbf{p}_{t+\Delta t})

; FSP sits at the floor, HSP

28

–

71\times

above it.

Map	analytic slope	measured slope	rel. err	$R^{2}_{\min}$	diag. frac
Umaze	$(4.696,5.121)$	$(4.666,5.088)$	${<}0.7\%$	$0.9959$	$0.9972$
Medium	$(4.805,4.643)$	$(4.785,4.643)$	${<}0.5\%$	$0.9959$	$0.9983$
Large	$(5.870,4.094)$	$(5.856,4.080)$	${<}0.4\%$	$0.9967$	$0.9948$

Appendix

Table 10: FSP matches the analytical kinematic-shortcut slope to

<1\%

on every map; the map is essentially linear (

R^{2}\!\geq\!0.996

, diagonal fraction

>0.994

). HSP

|\mathbf{J}|_{\max}\!=\!0

exactly, since its position branch has no

\mathbf{v}

input.

Map	$\bar{S}_{\mathrm{HSP}}$	$\bar{S}_{\mathrm{FSP}}$	$\bar{S}_{\mathrm{FSP}}/\bar{S}_{\mathrm{HSP}}$	perm. ratio
Umaze	$0.695$	$1.54\mathrm{e}{-3}$	$2.2\mathrm{e}{-3}$	$0.037$
Medium	$0.173$	$2.13\mathrm{e}{-4}$	$1.2\mathrm{e}{-3}$	$0.026$
Large	$0.144$	$1.68\mathrm{e}{-4}$	$1.2\mathrm{e}{-3}$	$0.021$

Appendix

Table 11: E2. Goal sensitivity, normalized by data-side discriminative variance, and goal-permutation change. FSP is two-to-three orders of magnitude less goal-sensitive than HSP on every map.

	closed, filter off		open, filter off
Map	FSP $0{\to}.1$	HSP $0{\to}.1$	FSP $0{\to}.1$	HSP $0{\to}.1$
Umaze	$.33{\to}.20$	$1.0{\to}1.0$	$.87{\to}.93$	$.93{\to}.93$
Medium	$.07{\to}.07$	$1.0{\to}1.0$	$.47{\to}.53$	$1.0{\to}1.0$
Large	$.00{\to}.10$	$1.0{\to}1.0$	$.60{\to}.60$	$.90{\to}.90$

Appendix

Table 12: E4. Success@

\varepsilon{=}0.5

\sigma{=}0\!\to\!0.10

, over

15

15

10

rollouts per cell (Umaze/Medium/Large). HSP is flat everywhere. FSP collapses in closed loop already at

\sigma{=}0

, worsening with horizon (

0.33\!\to\!0.07\!\to\!0.00

); the gap is far smaller open-loop, isolating the failure to closed-loop velocity feedback.

Figure 5: Visual instantiation of E2 on Maze2D Large. For three goal-discriminative probes (

\bullet,\blacksquare,\blacktriangle

; deterministic selection from the upper-median-

D_{\mathrm{var}}

pool), we sweep the same

64

-goal bank used in Table 11 and plot the unit-direction one-step displacement

(\hat{\mathbf{p}}-\mathbf{p})/\|\hat{\mathbf{p}}-\mathbf{p}\|

, color-matched to goal index. Columns: FSP vs HSP; rows: flow time

t\in\{0.25,0.5,0.75\}

(both networks consume

t

via a sinusoidal step embedding). A tight clump of arrows per probe means the model ignores the goal channel; a multi-color fan means it attends to it. FSP arrows clump regardless of the goal; HSP arrows fan toward their respective goals, with the fan most pronounced as

t\!\to\!1

Figure 6: Plans from one start (

\bullet

) to one goal (

\star

) on Maze2D Large; small samples of

\mathbf{p}_{0}

near

\mathbf{p}_{s}

yield two routes, shown in two colors.

	Maze2D						F1TENTH				Warehouse
	Umaze		Medium		Large		Budapest		Catalunya
Method	G	S	G	S	G	S	G	S	G	S	G	S
RRT* [ 18 ]	96	91	95	94	92	86	–	–	–	–	56	56
A* [ 10 ]	100	93	96	85	89	85	–	–	–	–	41	41
Diffuser [ 14 ]	94	62	94	46	74	25	79	0	69	69	50	35
Diffuser + CG [ 7 ]	54	36	2	2	4	1	86	0	59	59	44	37

Appendix

Table 13: Goal-reaching rate (G) and safe success (S, goal reached without violation) of every method, in %. Violations use

\kappa=0.01

in Maze2D and F1TENTH and

\kappa=0

in Warehouse. –: not evaluated.

Benchmark	$\sigma_{0}$	$\lambda$	$H$	$\Delta t$ (s)	PD $(K_{p},K_{v})$	Input bound	Solved (%)	Failed (%)	At bound (%)
Maze2D Umaze	$0.005$	$0.1$	$128$	$0.01$	$(1,1)$	$[-1,1]^{2}$	$12.92$	$0$	$19.95$
Maze2D Medium	$0.005$	$0.1$	$256$	$0.01$	$(1,1)$	$[-1,1]^{2}$	$11.95$	$0$	$17.20$
Maze2D Large	$0.005$	$0.1$	$384$	$0.01$	$(1,1)$	$[-1,1]^{2}$	$10.72$	$0.125$	$15.64$
Hopper	$10^{-4}$	$2.0$	$1000$	$0.008$	–	$[-1,1]^{3}$	$0.42$	$0$	$3.84$
Warehouse	$0.005$	$0.1$	$512$	$0.05$	$(2.0,0.2)$	$\\|\mathbf{v}\\|\leq 0.5$ m/s	$10.31$	$0$	$46.06$

Appendix

Table 14: Filter settings and closed-loop solver statistics of SafeStreamingFlow . Solved: steps at which the filter solves its optimization problem; Failed: steps at which SLSQP reports failure; At bound: steps at which the applied input reaches the input bound (% of all steps).

Panel of Figure 7 — Figure 7: Maze2D obstacle layouts for the three D4RL maps. Red regions indicate the safety-barrier obstacles enforced via Eq. ( 31 ); remaining cells form the free space from which start–goal pairs are uniformly sampled.

Figure 8: Plan (left) and rollout (right) on Maze2D Large. $\star$ denotes the goal $\mathbf{x}_{g}$ ; the colored line is the trajectory from start (blue) to end (red). Black arrows visualize step-wise HOCBF safety corrections.

Method	NN (ms)	$t_{\text{Opt}}$ (ms)	$t_{\text{total}}$ (ms)	MFLOPs/step	#Opt
Diffuser	921.6	–	921.6	771.1	-
Diffuser + CG	972.8	3865.6	4838.4	771.1	-
SafeDiffuser	972.8	9113.6	10086.4	771.1	$H\times K$
FM	896.0	–	896.0	771.1	-
SafeFM	998.4	9139.2	10137.6	771.1	$H\times K$
FlowMatcher	896.0	–	896.0	771.1	-

Appendix

Table 15: Computational efficiency on Maze2D Large. NN,

t_{\text{Opt}}

, and

t_{\text{total}}

are totals for generating one full trajectory; FLOPs are reported per emitted step.

Panel of Figure 9 — Figure 9: F1TENTH track layouts. Blue: centerline; green: minimum-curvature raceline; black: track walls. Red circles mark the safety-barrier obstacles (radius $r$ in legend); zoom-in panels show each obstacle in the context of the surrounding track walls. The safety filter constrains only these super-ellipse obstacles; track-wall avoidance is encoded implicitly through the demonstration distribution.

Figure 10: Hopper rollout under SafeStreamingFlow with the velocity-augmented ceiling barrier (Eq. 32 ). Red horizontal line: ceiling $z_{\max}=1.5$ m. The torso remains beneath the ceiling throughout the hopping gait.

Figure 11: Warehouse environment in Gazebo (top-down view). Gray polygons: physical obstacles (shelves, pallets, crates) defined in the Gazebo SDF world. Red dashed circles: abstract safety-filter barriers enforced via Eq. ( 31 ) at $(8.5,10.0)$ and $(16.0,11.0)$ with radius $r=1.5$ m. The two safety barriers are intentionally inflated relative to the physical shelf geometry to provide a margin and to match the line-density heatmap of training trajectories in the high-traffic central corridor.

Figure 12: Gazebo Fortress 3D view: the mecanum robot among warehouse obstacles, simulated with a physics engine under the ROS2/Nav2 stack.

Goal-Conditioned Planning

See all 18 papers

HDFlow: Hierarchical Diffusion-Flow Planning for Long-horizon Tasks
May 6, 2026Nandiraju Gireesh, Yuanliang Ju, Chaoyi Xu +3Hierarchical Planning Long-Horizon Robotic Manipulation
1Peking University · University of Toronto. · 2Galbot
DiRecT: Safe Diffusion-Based Planning via Receding-Horizon Denoising
Jun 13, 2026Paolo Giaretta, Zeyang Li, Navid AzizanConstrained Motion Planning Diffusion-Based Planning
MIT
PolyFlow: Safe and Efficient Polytope-Constrained Flow Matching with Constraint Embedding and Projection-free Update
Jun 11, 2026Jianming Ma, Qiyue Yang, Yang Zhang +4Constrained Generative Modeling Flow Matching
Shanghai Jiao Tong University, Shanghai, China · Shanghai Innovation Institute, Shanghai, China

May 6, 2026J/K move · Enter open · S save

HDFlow: Hierarchical Diffusion-Flow Planning for Long-horizon Tasks

Nandiraju Gireesh, Yuanliang Ju, Chaoyi Xu, Weiheng Liu+2

1Peking University · University of Toronto. · 2Galbot

Hierarchical Planning Long-Horizon Robotic Manipulation Trajectory Generation Diffusion-Based Planning Robot Motion Generation Flow-Based Generative Modeling

Open paper AskPDF arXiv

Recent advances in generative models have shown promise in generating behavior plans for long-horizon, sparse reward tasks. While these approaches have achieved promising results, they often lack a principled framework for hierarchical decomposition and struggle with the computational demands of real-time execution, due to their iterative denoising process. In this work, we introduce Hierarchical Diffusion-Flow (HDFlow), a novel hierarchical planning framework that optimally leverages the strengths of diffusion and rectified flow models to overcome the limitations of single-paradigm generative planners. HDFlow employs a high-level diffusion planner to generate sequences of strategic subgoals in a learned latent space, capitalizing on diffusion's powerful exploratory capabilities. These subgoals then guide a low-level rectified flow planner that generates smooth and dense trajectories, exploiting the speed and efficiency of ordinary differential equation (ODE)-based trajectory generation. We evaluate HDFlow on four challenging furniture assembly tasks in both simulation and real-world, where it significantly outperforms state-of-the-art methods. Furthermore, we also showcase our method's generalizability on two long-horizon benchmarks comprising diverse locomotion and manipulation tasks. Project website: https://hdflow-page.github.io/

Abstract

Goal-Conditioned Planning

Explore similar work

HDFlow: Hierarchical Diffusion-Flow Planning for Long-horizon Tasks

DiRecT: Safe Diffusion-Based Planning via Receding-Horizon Denoising

PolyFlow: Safe and Efficient Polytope-Constrained Flow Matching with Constraint Embedding and Projection-free Update