Hierarchical Diffusion Motion Planning with Task-Conditioned Uncertainty-Aware Priors

1. Problem Statement

Standard diffusion models for robotic motion planning typically rely on zero-mean, isotropic Gaussian noise during the forward corruption process. While effective, this approach ignores the inherent structural properties of robotic trajectories, such as:

• Temporal Smoothness: Dynamic feasibility requires consistent evolution of states over time.
• Task Semantics: Trajectories must satisfy specific constraints (e.g., start/goal states, contact events, waypoints).

Existing methods attempt to inject this structure via conditioning (feeding task info into the network) or guidance (using cost functions during inference). However, these methods treat the corruption process as unstructured noise, meaning the model must "learn" to ignore the noise and satisfy constraints simultaneously, which can be inefficient and lead to suboptimal sampling.

The Core Challenge: How to embed task-specific structure (smoothness, constraints, semantics) directly into the noise model itself, rather than just the network conditioning, to guide the generative process more effectively.

---

2. Methodology

The authors propose a Hierarchical Diffusion Planner that redefines the corruption process using Task-Conditioned Non-Isotropic Gaussians. The framework consists of two levels:

A. Theoretical Foundation: Biased, Non-Isotropic Diffusion

The authors generalize the standard Denoising Diffusion Probabilistic Model (DDPM). Instead of isotropic noise $\epsilon \sim \mathcal{N}(0, I)$, they introduce a corruption process with a task-conditioned mean ($\xi$) and covariance ($K$):
$$\tau_i \sim \mathcal{N}\left(\sqrt{\bar{\alpha}_i}\tau_0 + (1-\sqrt{\bar{\alpha}_i})\xi, (1-\bar{\alpha}_i)K\right)$$

• $\xi$ (Mean): Encodes the expected trajectory structure (e.g., smooth interpolation between key points).
• $K$ (Covariance): Encodes temporal correlations and uncertainty.
• Result: The reverse diffusion process operates in a Mahalanobis geometry, where the loss function weights errors based on the inverse of the task-conditioned covariance ($K^{-1}$). This encourages the model to stay consistent with the prior structure while allowing flexibility where uncertainty is high.

B. Hierarchical Architecture

The framework separates prior instantiation from trajectory denoising:

1. Upper Level (Key State Prediction):

   • Goal: Predict sparse, task-centric key states (e.g., waypoints, grasp/release configurations) and their corresponding timings.
   • Mechanism: A diffusion model predicts these sparse constraints.
   • Uncertainty Handling: These predictions are treated as soft observations with explicit uncertainty, rather than hard constraints. This allows the system to handle imperfect predictions without rejection.

2. Prior Construction (GPMP Conditioning):

   • Using the predicted key states ($Y$) and timings (encoded in a selection matrix $C$), the system constructs a Gaussian Process Motion Planning (GPMP) prior.
   • A Linear Time-Varying (LTV) Gaussian Process is conditioned on the key states to produce the mean ($\xi$) and covariance ($K$) for the lower level.
   • Effect: The mean interpolates the key states smoothly, while the covariance is tight near key states (enforcing constraints) and looser elsewhere (allowing exploration).

3. Lower Level (Trajectory Denoising):

   • The full trajectory is generated by reverse diffusion starting from the constructed prior $\mathcal{N}(\xi, K)$.
   • Training: The network minimizes a Mahalanobis loss (weighted by $K^{-1}$), ensuring the denoising steps respect the temporal coupling and confidence levels encoded in the GP prior.
   • Inference: The process generates a full, smooth trajectory that naturally adheres to the task constraints defined by the upper level.

---

3. Key Contributions

1. Task-Conditioned Noise Model: A novel formulation that replaces isotropic Gaussian corruption with non-isotropic, task-conditioned Gaussians. This embeds smoothness and task semantics directly into the generative process.
2. Hierarchical GPMP Integration: A two-level design where an upper-level planner predicts sparse key states, which are used to instantiate a GPMP-based prior for the lower-level diffusion. This separates high-level task logic from low-level trajectory generation.
3. Soft Constraint Handling: By treating key states as noisy observations with explicit uncertainty, the method avoids the brittleness of hard constraints or rejection sampling, allowing for robust planning even with imperfect upper-level predictions.
4. Closed-Form Formulation: The authors derive closed-form expressions for the forward marginals and posterior distributions, enabling efficient training via a generalized DDPM pipeline with a Mahalanobis loss.

---

4. Experimental Results

The method was evaluated on two benchmarks: Maze2D (goal reaching) and KUKA Block Stacking (manipulation).

• Success Rates: The proposed method significantly outperformed baselines.
  • Maze2D: 81/100 success (vs. 48/100 for standard Diffuser).
  • KUKA Stacking: 70/100 success (vs. 31/100 for standard Diffuser).
• Trajectory Quality:
  • Smoothness: Generated trajectories had significantly lower "Average Roughness" (AR) and achieved dataset-level smoothness much earlier in training compared to isotropic baselines.
  • Dynamic Feasibility: The method showed superior position-velocity consistency (lower Mean Absolute Error between generated velocities and finite-difference derivatives), indicating better adherence to dynamic constraints.
• Ablation Studies:
  • Comparing against a "Diffuser + KeyCond" (hierarchical but isotropic noise) showed that the structured noise model itself provides the primary benefit, not just the neural conditioning.
  • The method outperformed cost-guided diffusion (EDMP), demonstrating that embedding structure in the prior is more effective than guiding inference with cost functions.

---

5. Significance and Impact

• Efficiency: By concentrating probability mass near feasible and semantically meaningful trajectories, the model requires fewer training steps to converge to high-quality solutions.
• Robustness: The "soft observation" approach makes the planner robust to errors in high-level planning (e.g., imperfect waypoint prediction), a common failure mode in hierarchical systems.
• Generalizability: The framework is agnostic to how key states are obtained (rule-based, model-based, or learned), making it applicable to a wide range of robotic tasks involving complex phase-dependent constraints (e.g., grasp, transport, release).
• Theoretical Advancement: This work bridges the gap between Gaussian Process Motion Planning (which excels at smoothness and constraints) and Diffusion Models (which excel at multimodality and complex distributions), creating a hybrid that leverages the strengths of both.

In summary, this paper demonstrates that structuring the corruption process is a more powerful inductive bias for robotic motion planning than simply conditioning the network, leading to more reliable, smoother, and task-compliant trajectories.