Potential Based Diffusion Motion Planning

Effective motion planning in high dimensional spaces is a long-standing open problem in robotics. One class of traditional motion planning algorithms corresponds to potential-based motion planning. An advantage of potential based motion planning is composability -- different motion constraints can be easily combined by adding corresponding potentials. However, constructing motion paths from potentials requires solving a global optimization across configuration space potential landscape, which is often prone to local minima. We propose a new approach towards learning potential based motion planning, where we train a neural network to capture and learn an easily optimizable potentials over motion planning trajectories. We illustrate the effectiveness of such approach, significantly outperforming both classical and recent learned motion planning approaches and avoiding issues with local minima. We further illustrate its inherent composability, enabling us to generalize to a multitude of different motion constraints.

Learning Potentials for Motion Planning. Given a motion plan $q_{1:T}$ from start state $q_{\text{st}}$ to end state $q_{\text{e}}$ and a characterization of the configuration space $C$ ($\textit{i.e.}$ the set of obstacles in the environment), we propose to learn a trajectory-level potential function $U_\theta$ so that where $q_{1:T}^*$ is a successful motion plan from $q_{\text{st}}$ to $q_{\text{e}}$. To learn the potential function above, we propose to learn an EBM across a dataset of solved motion planning $D = \{q_{\text{st}}^i, q_{\text{e}}^i, q_{1:T}^i, C^i \}$, where $e^{-E_\theta(q_{1:T}|q_{\text{st}}, q_{\text{e}}, C)} \propto p(q_{1:T}|q_{\text{st}}, q_{\text{e}}, C)$. Since the dataset $D$ is of solved motion planning problems, the learned energy function $E_\theta$ will have minimal energy at successful motion plans $q_{1:T}^*$ and thus satisfy our potential function $U_\theta$.


Learning EBM for Motion Plan Generation. To learn the EBM landscape that enables us to effectively optimize and generate motion plans $q_{1:T}^*$, we propose to shape the energy landscape using denoising diffusion training objective. Formally, to train our potential, we use the energy based diffusion training objective, where the gradient of energy function is trained to denoise noise-corrupted motion plans $q_{1:T}^i$ from dataset $D$, where $\epsilon$ is sampled from Gaussian noise $\mathcal{N}(0, 1)$, $s \in\{1,2,...,S\}$ is the denoising diffusion step (we set $S = 100$), and $\beta_s$ is the corresponding Gaussian noise corruption on a motion planning path $q_{1:T}^i$. We refer to $E_\theta$ as the diffusion potential function.


Optimize and Sample from Diffusion Potential Function. To optimize and sample from our diffusion potential function, we initialize a motion path $q_{1:T}^S$ at diffusion step $S$ from Gaussian noise $\mathcal{N}(0, 1)$ and optimize for motion path following the gradient of the energy function $E_{\theta}$. We iteratively refine the motion path $q_{1:T}^s$ across each diffusion step following To parameterize the energy function $E_\theta(q_{1:T}, s, q_{\text{st}}, q_{\text{e}}, C)$, we use classifier-free guidance to form a peaker composite energy function conditioned on $C$. $\gamma$ and $\sigma^2_s$ are diffusion specific scaling constants (A rescaling term at each diffusion step is omitted above for clarity). The final predicted motion path $q_{1:T}^*$ corresponds to the output $q_{1:T}^0$ after running $S$ steps of optimization from the diffusion potential function.


Composing Diffusion Potential Functions Given two separate diffusion potential functions $E^1_\theta(\cdot)$ and $E^2_\theta(\cdot)$, encoding separate constraints for motion planning, we can likewise form a composite potential function $E^{\text{comb}}(\cdot) = E^1(\cdot) + E^2(\cdot)$ by directly summing the corresponding potentials. To sample from the new diffusion potential function $E^{\text{comb}}$, we can follow This potential function $E^{\text{comb}}$ will have low energy precisely at motion planning paths $q_{1:T}$ which satisfy both constraints, with sampling corresponding to optimizing this potential function. Hence, by iteratively sampling from the composite potential function, we can obtain motion plans that satisfy both constraints. Please see our compositional results below.