Model-Based Geometry-Aware Generative Optimization for Constrained Locomotion Planning

Zhilin He; Xinyuan Wang; Changliu Liu

cs.ROOct 6, 2026

Model-Based Geometry-Aware Generative Optimization for Constrained Locomotion Planning

Authors: Zhilin He, Xinyuan Wang, Changliu Liu

Organizations: Carnegie Mellon University · University of Michigan, Ann Arbor

Abstract

Constrained Locomotion Planning (CLP) for quadrupeds and humanoids, where robots must satisfy collision avoidance, contact consistency, kinematic feasibility, and support constraints, is challenging under high-dimensional dynamics and highly non-convex environments. Recent Model-Based Diffusion (MBD) approaches recast trajectory optimization as posterior sampling over trajectories, using known dynamics and Monte Carlo rollouts to analytically estimate the denoising score function without demonstration learning. While constrained variants further incorporate feasibility into model-based score rollouts and show promising performance, they are still limited by (1) lacking a task-modulated active constraint geometry that shapes the score direction and reverse stochasticity, and (2) using deterministic DDPM-style reverse transport without adaptive scheduling across different generative transports. Therefore, we introduce Model-Based Geometry-Aware Generative Optimization (2GO) for constrained locomotion, which turns active constraint geometry into executable denoising operators through normal- induced metric shaping, tangent-space stochastic filtering, and CFS-based retraction. 2GO further decouples generative transport from reverse stochasticity through an adaptive diffusion and flow-like schedule. Experiments on constrained quadruped and humanoid locomotion demonstrate strong performance in discrete foothold selection and continuous posture planning, with higher success rates, fewer violations, and improved execution compatibility.

Figures & tables

Fig. 2 : Overview of the 2GO framework. At each reverse step, 2GO reweights model-based trajectory samples and selects a DDPM-, DDIM-, or flow-like transport. Task-modulated active constraints shape the score through $G^{-1}_{k}$ and filter stochasticity through $P_{k}$ , with optional CFS correction; a reference governor converts the clean trajectory into executable commands. In a two-time-step foothold contact example (bottom right gray panel), the score $\hat{\mathbf{S}}_{k}$ guides two unconstrained foothold points, $Y_{k}^{1}$ and $Y_{k}^{2}$ , which are reshaped into $\tilde{Y}_{k-1}^{1}$ and $\tilde{Y}_{k-1}^{2}$ by the $G^{-1}_{k}$ -shaped $\hat{\mathbf{S}}_{k}^{G}$ and $P_{k}$ -filtered exploration, then retracted into the support regions via CFS. The final swing trajectory is shown as a gray line.

Panel of Figure 2 — Fig. 3 : Qualitative stepping-stone results. Top left: A representative 2GO deployment with the safety governor. Top right: 2GO planning samples at 90%, 50%, and 10% of the diffusion reverse process. Bottom: Final clean trajectories and execution tracking of position $s$ and velocity $v$ for baselines, and ours 2GO. Colored lines show the trajectories of the left rear (LR), left front (LF), right rear (RR), and right front (RF) feet, and the body center; red dots mark constraint violations.

Env.	MPPI				MBD				MDOC				MD-COAS				2GO
Env.	pSSR	$pV$	$pT$	eSSR	pSSR	$pV$	$pT$	eSSR	pSSR	$pV$	$pT$	eSSR	pSSR	$pV$	$pT$	eSSR	pSSR	$pV$	$pT$	eSSR
S1	$0.40$	$2.20^{0.14}$	$\mathbf{2.87}^{0.34}$	$0.20$	$0.18$	$3.15^{0.36}$	$\underline{17.85}^{0.75}$	$\underline{0.60}$	$\underline{0.88}$	$1.00^{0.08}$	$18.37^{0.17}$	$\mathbf{0.80}$	$0.32$	$\underline{0.85}^{0.22}$	$288.22^{33.76}$	$0.00$	$\mathbf{0.98}$	$\mathbf{0.47}^{0.14}$	$21.51^{1.01}$	$\mathbf{0.80}$
S2	$0.20$	$2.75^{0.24}$	$\mathbf{2.53}^{0.23}$	$0.00$	$0.04$	$3.01^{0.30}$	$\underline{18.35}^{3.63}$	$\mathbf{0.40}$	$\underline{0.68}$	$\underline{1.11}^{0.12}$	$19.73^{0.33}$	$\underline{0.20}$	$0.14$	$1.45^{0.28}$	$99.78^{42.47}$	$0.00$	$\mathbf{0.88}$	$\mathbf{0.61}^{0.25}$	$23.19^{2.74}$	$\mathbf{0.40}$
ZA	$0.00$	$1.15^{0.40}$	$\mathbf{1.40}^{0.31}$	$\underline{0.40}$	$0.00$	$1.16^{0.21}$	$\underline{10.16}^{1.76}$	$0.00$	$\mathbf{0.96}$	$\mathbf{0.00}^{0.00}$	$18.04^{10.05}$	$\mathbf{1.00}$	$\underline{0.02}$	$\underline{0.07}^{0.02}$	$67.27^{18.94}$	$0.20$	$\mathbf{0.96}$	$\mathbf{0.00}^{0.00}$	$14.08^{6.91}$	$\mathbf{1.00}$
ZB	$0.00$	$3.37^{0.58}$	$\mathbf{1.37}^{0.36}$	$0.00$	$0.00$	$2.78^{0.13}$	$\underline{6.15}^{1.92}$	$0.00$	$\mathbf{0.90}$	$\underline{0.77}^{0.70}$	$39.16^{9.89}$	$\underline{0.20}$	$0.40$	$\mathbf{0.18}^{0.13}$	$99.79^{12.04}$	$\mathbf{0.60}$	$\underline{0.48}$	$1.82^{0.31}$	$37.09^{18.20}$	$\underline{0.20}$
ZC	$0.00$	$1.68^{1.33}$	$\mathbf{1.36}^{0.32}$	$0.00$	$0.00$	$3.12^{0.80}$	$\underline{4.88}^{1.00}$	$0.00$	$\mathbf{1.00}$	$\mathbf{0.00}^{0.00}$	$10.94^{5.95}$	$\mathbf{1.00}$	$\underline{0.02}$	$\underline{0.22}^{0.09}$	$105.30^{3.58}$	$0.20$	$\mathbf{1.00}$	$1.23^{0.16}$	$14.23^{7.98}$	$\mathbf{1.00}$

TABLE I : Planning and execution performance across stepping stones (S1, S2) and corridors (ZA–ZD). pSSR and eSSR (

\uparrow

) are success fractions defined in the evaluation protocol;

pV

(

\downarrow

) is foothold distance (cm) for S1/S2 and sampled obstacle occupancy (%) for ZA–ZD.

pT

(

\downarrow

) is planning time (s). Superscripts give population standard deviations across 5 seeds, with each seed generating 10 clean trajectories. Bold and underlining indicate the best and second-best displayed values, including ties.

Panel of Figure 4 — Fig. 4 : Planning and deployment across the humanoid corridor locomotion benchmark. Per zone (rows): the planned multi-modal whole-body latent trajectories (left), the 2GO adaptive transport schedule $\rho_{k}/\sigma_{k,\mathrm{eff}}^{\rho_{k}}$ within fixed 100 diffusion steps (middle), and the deployed Unitree G1 sport-mode execution as a ghosted motion strip (right).

Appendix figures & tables16 assets

Supplementary material from the paper’s appendix.

Appendix

Task	$d_{x}$	$d_{u}$	$H$	$\Delta t$	State and action summary
Stepping stones	16	12	16	1.0	$x=[s,e_{y},e_{\psi},v_{s},v_{y},\omega,r_{FL}^{\top},r_{FR}^{\top},r_{RL}^{\top},r_{RR}^{\top},m,\vartheta]$ ; $u=[\Delta s_{b},\Delta y_{b},\dot{\psi}_{b},\dot{r}_{FL}^{\top},\dot{r}_{FR}^{\top},\dot{r}_{RL}^{\top},\dot{r}_{RR}^{\top},\dot{\vartheta}]$ .
Humanoid corridor	14	9	50	0.25	$x=[x,y,\psi,h,\psi_{T},a_{L},a_{R},p_{L},p_{R},v_{x},v_{y},\omega,\dot{h},\dot{\psi}_{T}]$ ; $u=[v_{x}^{b},v_{y}^{b},\omega_{c},\dot{h}_{c},\dot{\psi}_{T,c},\dot{a}_{L},\dot{a}_{R},\dot{p}_{L},\dot{p}_{R}]$ .

Appendix

TABLE II : Environment dimensions and 2GO planning discretization. Humanoid baselines use

H=50

. Humanoid velocity actions are body-frame commands.

Zone	Start $\rightarrow$ goal	Constraint geometry
A	$(0.5,0)\rightarrow(3.5,0)$	Centered thin wall, $x\in[1.7,2.0]$ , $y\in[-0.05,0.05]$ .
B	$(1.0,0.1)\rightarrow(3.0,0.25)$	Connected U-wall with top segment $x\in[1.8,1.95]$ , $y\in[-0.16,0.8]$ .
C	$(0.5,0)\rightarrow(3.5,0)$	Tapered squeeze over $x\in[1.5,2.5]$ with a $0.60$ main gap and quarter-circle entry and exit.
D	$(0.5,0)\rightarrow(3.5,0)$	Two full-height planning obstacles centered at $(2.0,0.25)$ and $(2.5,-0.25)$ , each with radius $0.06$ .

Appendix

TABLE III : Humanoid corridor geometry. All corridors are 4.0 m long.

Component	Value or behavior
Quadruped	Interior clearance $0.07$ , stride $0.22$ , maximum swing $0.35$ , touchdown tolerance $0.035$ , simulation step $0.01$ ; leg order $FL,RR,FR,RL$ ; phase length 36 simulation steps.
Base assistance	Planar PD gains $(140,24)$ , height gains $(260,34)$ , roll/pitch gains $(95,12)$ , yaw gains $(35,6)$ ; direct base wrench scaled by the support-contact count.
Evaluation timing	Interface 50 Hz; simulation step $0.002$ s; warmup $0.8$ s; reference speed factor $0.5$ ; terminal hold 5 s.
Reference feedback	Planar gain $1.5$ , correction cap $0.25$ m/s; yaw feedback gain $0$ ; planar low-pass weight $0.3$ .
Adapter speed box	World- $x$ $[-0.5,1.0]$ and world- $y$ $[-0.55,0.55]$ m/s; local step $0.02$ s.
Command slew	No explicit bound on successive locomotion-command differences.

Appendix

TABLE IV : Simulation execution settings. Distances are in meters; adapter bounds and final policy limits are listed separately.

Quantity	Planning model	Reference adapter	Tracker / policy
Command magnitude	Humanoid body-frame velocity and posture-rate bounds; control-magnitude penalties.	World-frame position-increment bounds specify reference speeds; safety-box fallback can relax nominal bounds.	Body-frame planar velocity and separately generated yaw-rate commands are clipped at the policy input.
Command slew	Control-magnitude penalties do not constrain successive-command differences.	Retiming and low-pass filtering moderate command variation; the reference-speed box is not a slew bound.	Input saturation bounds magnitude; no explicit command-slew limit is imposed.
Posture	Bounds on height, torso/arm posture, and their command rates.	Planned posture enters body clearance; a pelvis-roll proxy adds a lateral-speed bound, not a measured-roll constraint.	Upper-body targets follow a separate mapping/PD path; the learned policy produces leg targets, followed by joint-limit clipping.
Clearance	Task geometry and planning/CFS margins are evaluated along the model rollout.	The corridor wall box and local lateral body-clearance half-space use $m_{\mathrm{track}}$ ; finite correction can retain residuals.	Filtering and saturation follow adaptation without a final-reference clearance recheck; executed proxy clearance is evaluated afterward.
Quadruped footholds and support	Foot-placement geometry, support-offset bounds, and support-proximity cost.	Gait construction, stance anchors, and reachable touchdown projection; support-polygon membership is diagnostic.	Foot tracking and joint PD execute the gait with the common supplementary floating-base wrench.

Appendix

TABLE V : Planning and execution constraints. Numerical execution settings are listed in Table IV .

	pSSR		$pV$ (%)		$pT$ (s)
Seed	CFS on	CFS off	CFS on	CFS off	CFS on	CFS off
0	0.60	0.30	2.01	1.25	111.15	64.33
1	0.70	0.40	0.50	0.67	77.40	34.31
2	0.40	0.20	1.93	1.76	58.54	27.84
3	0.60	0.30	1.44	1.17	60.49	49.19
4	0.50	0.30	2.45	1.53	63.07	37.58

Appendix

TABLE VI : Paired ablation of the candidate-correction variant in Zone B. Both variants use 1024 samples, 100 reverse updates, ten returned candidates, and the same five seeds. Post-transport retraction is disabled in both.

pV

is band occupancy (%);

pT

is CPU planning time (s). The final row gives mean

\pm

population standard deviation.

Method	Seed	pSSR	$pV$	$pT$	Goal error (m)	Foot distance (m)	Fall	Success
MPPI	0	1.00	2.232	3.538	0.0622	0.0521	—	0
	1	0.00	2.236	2.750	0.0792	0.0526	—	0
	2	1.00	2.300	2.626	0.1364	0.0477	—	1
	3	0.00	1.932	2.652	0.0974	0.0615	—	0
	4	0.00	2.314	2.761	0.0837	0.0520	—	0
MBD	0	0.30	2.736	17.594	0.0512	0.0454	—	1

Appendix

TABLE VII : Per-seed outcomes for S1.

pV

is in cm and

pT

in seconds. Goal error is measured from the executed final base position to the fixed task goal. Foot distance is the maximum support-region distance of recorded swing-end foot positions, including unsuccessful touchdown attempts. Success is the binary event contributing to eSSR; all five trials are included.

Method	Seed	pSSR	$pV$	$pT$	Goal error (m)	Foot distance (m)	Fall	Success
MPPI	0	0.00	2.719	2.647	0.0713	0.0533	—	0
	1	0.00	2.559	2.693	0.0823	0.0510	—	0
	2	1.00	2.445	2.231	0.0814	0.0550	—	0
	3	0.00	3.133	2.794	0.1479	0.0594	—	0
	4	0.00	2.873	2.263	0.1402	0.0583	—	0
MBD	0	0.00	2.781	16.041	0.0639	0.0489	—	1

Appendix

TABLE VIII : Per-seed outcomes for S2; metric definitions and units follow Table VII .

Method	Seed	$pV$	$pT$	Goal error (m)	Min. proxy SDF (m)	Success
MPPI	0	0.667	1.966	0.1618	0.0090	1
	1	0.750	1.163	0.1610	0.0074	1
	2	1.333	1.136	0.1909	-0.0105	0
	3	1.250	1.476	0.4769	-0.0395	0
	4	1.750	1.244	0.2070	-0.0615	0
MBD	0	1.067	12.037	0.1056	-0.0525	0

Appendix

TABLE IX : Per-seed outcomes for Zone A.

pV

is in % and

pT

in seconds. Goal error is measured from the executed final base position to the fixed task goal. Clearance uses the reduced body proxy at the executed base pose and commanded upper-body posture. Success is the binary event contributing to eSSR; all five trials are included.

Method	Seed	$pV$	$pT$	Goal error (m)	Min. proxy SDF (m)
MPPI	0	3.333	2.087	0.2071	-0.0433
	1	3.917	1.140	0.4910	-0.1241
	2	3.917	1.177	0.4753	-0.2029
	3	2.333	1.218	0.7573	-0.0170
	4	3.333	1.208	0.6378	-0.2059
MBD	0	2.858	8.711	0.1561	-0.1236

Appendix

TABLE X : Per-seed outcomes for Zone B; metric definitions and units follow Table IX .

Method	Seed	$pV$	$pT$	Goal error (m)	Min. proxy SDF (m)
MPPI	0	0.000	1.999	0.1393	-0.0753
	1	1.250	1.150	0.2134	-0.0792
	2	4.083	1.170	0.0737	-0.1283
	3	1.417	1.235	0.2089	-0.0359
	4	1.667	1.240	0.4041	-0.0181
MBD	0	1.708	4.112	0.1093	-0.0400

Appendix

TABLE XI : Per-seed outcomes for Zone C; metric definitions and units follow Table IX .

Method	Seed	$pV$	$pT$	Goal error (m)	Min. proxy SDF (m)
MPPI	0	0.250	1.997	0.1389	-0.0261
	1	0.500	1.230	0.2114	-0.0297
	2	0.917	1.117	0.0761	-0.0681
	3	0.000	1.124	0.2080	-0.0632
	4	0.583	1.280	0.4044	-0.0273
MBD	0	0.558	4.821	0.1049	-0.0354

Appendix

TABLE XII : Per-seed outcomes for Zone D; metric definitions and units follow Table IX .

Method	Constraint and sampling mechanism
MPPI	Gaussian action perturbations with exponential cost weighting; no geometry-aware reverse transport; 100 update iterations.
MBD	Model-based denoising with task cost and no explicit local constraint geometry.
MDOC	MBD with CBF-style derivative correction using the configured step, gain, and margin; no 2GO metric/tangent split or route/risk gate.
MD-COAS	Model-based diffusion with its own active-set correction, scheduled margins, and configured cap on local constraints.
2GO	Reward-weighted reverse transport with task-local metric and tangent maps and the task-specific correction variants in Appendix B-C .

Appendix

TABLE XIII : Baseline mechanisms used in the reported comparison.

Parameter	Symbol	Value	Parameter	Symbol	Value
Reverse steps	$K$	100	Sampling temperature	$T_{\mathrm{samp}}$	0.3
Beta endpoints	$(\beta_{0},\beta_{1})$	$(10^{-5},10^{-2})$	Returned modes	$N$	10
Seeds	—	$0,1,2,3,4$	Active-row cap	$K_{A}$	8
CVaR tail level	$\alpha$	0.9	Geometry regularizers	$\varrho,\epsilon_{G},\epsilon_{P}$	$10^{-6}$
Base refinement gain	$\eta_{\mathrm{base}}$	0.08	Route EMA coefficient	—	0.7
Penalty reference	$\mu_{\mathrm{ref}}$	500	$(\kappa_{0},\kappa_{\mu})$	—	$(1,0.01)$

Appendix

TABLE XIV : Common 2GO generative and geometry parameters.

Task	$M$	$W/S$	$\sigma_{\max}$	$\sigma_{u}$	$\lambda_{g}$	$m_{k}$	$b_{i}$	Corrected candidates
Stones S1/S2	512	$8/4$	0.10	0.20	0.0	0.02	—	0
Zone A	1024	$6/3$	0.15	0.15	0.3	0.02	0.01	32
Zone B	1024	$6/3$	0.15	0.35	0.1	0.05	0.04	32
Zone C	1024	$6/3$	0.15	0.15	0.3	0.02	0.01	32
Zone D	1024	$6/3$	0.15	0.15	0.3	0.07	0.01	32

Appendix

TABLE XV : Task-specific parameters for the experimental variants in Appendix B-C . Here

\lambda_{g}

is the task-direction gain,

m_{k}

the scheduled clearance margin, and

b_{i}

the candidate-CFS offset. Post-transport retraction is disabled in these experiments.

Task	Term	Coefficient
Stepping stones	Smoothness	$0.92[0.165$ body step $+0.15$ yaw $+0.165$ foot residual rate $+0.042$ phase rate $]$ .
Stepping stones	Feasibility and shape	$1.12(40)$ foothold violation; $1.08(14)$ support violation; $1.0[1.2$ geometric shape $+0.8$ residual shape $]$ ; step-bound factors $8$ and $4$ .
Stepping stones	Task terms	Terminal goal weight $120$ , terminal-foot multiplier $0.5$ , plus progress, time guide, lateral/velocity/yaw/backward, $0.08$ heading, $1.05$ support-center, and phase-ramped terminal-foot terms.
Humanoid	Task terms	$0.2w_{g}$ terminal goal, $1.0$ corridor centering, $0.3$ each arm tuck, $0.3$ torso yaw, $6.0$ height, $0.05$ velocity, $0.06$ control, $-0.3$ progress, and $5.0$ squeeze term.
Humanoid	Zone-specific goal	$w_{g}=5$ in Zone C and $w_{g}=1$ in Zones A, B, and D. Goal deceleration is enabled only in Zone C.
Humanoid	Obstacle cost	$0$ ; obstacle feasibility enters through the violation evaluator, local geometry, augmented-Lagrangian schedule, and candidate correction.

Appendix

TABLE XVI : Explicit weights used by the task objectives.

Domain	Method	Samples	Steps	Additional parameters
Quadruped	MPPI	512	100	Action noise $0.20$ , temperature $1.0$ .
Quadruped	MBD	512	100	$T_{\mathrm{samp}}=0.3$ , extra action noise $0.02$ , 10 returned modes.
Quadruped	MDOC	512	100	$T_{\mathrm{samp}}=0.3$ , correction step $0.01$ , gain $0.1$ , margin $0.01$ , 10 modes.
Quadruped	MD-COAS	512	100	$T_{\mathrm{samp}}=0.3$ , at most 12 constraints, constraint margin $0.01$ , scheduler margin $0.02$ , 10 modes.
Humanoid	MPPI	1024	100	Action noise $0.15$ ( $0.35$ in Zone B), temperature $1.0$ .
Humanoid	MBD	1024	100	$T_{\mathrm{samp}}=0.3$ , extra action noise $0.15$ ( $0.35$ in Zone B), 10 modes.

Appendix

TABLE XVII : Optimizer parameters used in the baseline comparison.

Explore similar work

Jul 16, 2026cs.RO

Motion Planning with Model-Based Diffusion via Constraint Optimization and Adaptive Scheduling

Single-Robot Motion Planning (SRMP) in highly non-convex constrained environments, where robots must satisfy collision-free guarantees, dynamic feasibility, and task-related constraints, is challenging under complex constraints and computational limits. Recent Model-Based Diffusion (MBD) approaches recast the SRMP as trajectory optimization that samples from a posterior over trajectories, using known dynamics, and analytically estimates the score function from rollout samples to guide diffusion denoising toward a low-cost, clean trajectory without demonstration learning. While existing works further adapt MBD to constrained environments and showcase promising performance, they are still limited by (1) enforcing safety either via soft feasibility diffusion priors or hard projection operators, but lack a unified framework to integrate both, and (2) fixing safety enforcement to neglect the changing of diffusion scheduling. Therefore, we introduce Model-Based Diffusion via Constraint Optimization and Adaptive Scheduling (MD-COAS) for SRMP that unifies the inexact Augmented Lagrangian Method (iALM) soft diffusion prior with a Convex Feasible Set (CFS)-based hard projection operator, and adaptively schedules and co-optimizes safety enforcement, along with diffusion scheduling. Experiments demonstrate that our method achieves higher safety & success rates, faster convergence, and lower final costs than baseline planners on randomly generated highly non-convex 2D benchmarks and a 7-DoF robot arm avoidance task.

Zhilin He, Bowei Li, Jianlin Dou +2

Carnegie Mellon University · University of Pennsylvania

Motion Planning Multi-Robot Motion Planning Safety Constraints Planning Diffusion Priors Nonconvex+1

Ask PDF

Jun 23, 2026cs.RO

Grounding Generative Policies in Physics: Optimization-Guided Diffusion for Robot Control

Diffusion models sample effectively from high-dimensional, multimodal distributions, but their outputs may violate deployment constraints. For task-space robot policies, generated grasps, waypoints, or trajectories can be distributionally valid yet infeasible, violating reachability, collision-avoidance, or closed-loop executability requirements. This embodiment gap limits zero-shot deployment across robots, even when the task-space behavior itself is transferable. We propose an inference-time optimization framework that couples the behavior generation to physical feasibility by formulating diffusion guidance as a constrained optimization problem. Our key insight is to replace the sampling perturbation in the backward process with an optimized correction, allowing hard constraints or soft penalties to be imposed during sampling without the need to retrain the diffusion model, while keeping samples close to the learned prior. We evaluate the method on dexterous grasp synthesis with reachability and collision-avoidance constraints, and dynamic manipulation with controller-level trackability constraints. Across settings and robot embodiments, optimization-guided denoising matches the feasibility of projection- and gradient-guidance baselines while better preserving grasp quality, and improving controller-level executability and task success, with task success improving by up to 20pp. on dexterous grasping and 23pp. on visuomotor manipulation over the best baseline.

Sabrina Bodmer, René Zurbrügg, Tifanny Portela +5

ETH Zurich · EPFL

Diffusion Policies Robot Policies Diffusion Models Robot Systems Inference-Time

Ask PDF

Jul 14, 2026cs.RO

Model-Based Diffusion Optimal Control for Multi-Robot Motion Planning

Multi-Robot Motion Planning in continuous environments, where robots must generate dynamically feasible, collision-free trajectories, is challenging due to the combinatorial growth of the joint trajectory space and the difficulty of enforcing dynamic feasibility and hard safety constraints. Recent approaches recast trajectory planning as probabilistic inference, sampling from a posterior over trajectories using diffusion models whose score functions are learned from demonstration data. While showing promising performance, these approaches are limited: they often rely on sizable demonstration datasets and struggle to rigorously enforce dynamics and hard safety constraints during sampling. To this end, we introduce Model-Based Diffusion Optimal Control (MDOC), a model-based diffusion planner that efficiently produces dynamically feasible trajectories without relying on data. Crucially, we show that MDOC's safety mechanism -- combining known dynamics models with Control Barrier Function-constrained projections -- naturally scales to multi-robot planning settings through Conflict-Based Search. Across simulation experiments, this integrated method consistently outperforms representative baseline planners in sample efficiency, geometric smoothness, and success rate, while reducing computation time and producing collision-free trajectories.

Zhilin He, Yorai Shaoul, Jiaoyang Li

Carnegie Mellon University, Pittsburgh, PA, USA

Multi-Robot Motion Planning Diffusion Planning Collision-Free Trajectories Planning

Ask PDF