Potential Based Diffusion Motion Planning

Thank you for your constructive feedback. We have carefully considered each point in the review and updated the paper accordingly.

Q1. A more thorough literature review.

Thank you for the suggestion. We have updated the Introduction section.

Q2. Comparisons with state of the art sampling-based methods.

We have appended the results of BIT* in three environments: Maze2D, KUKA, Dual KUKA. In addition, we also added a potential-guided sampling-based method P-RRT* for reference. For a more complete quantitative table, please refer to the updated Table VIII in the appendix. Besides, We also added experiment results in another environment with 7 concave obstacles in the Table X.

Maze 2D:

KUKA 7D:

Dual KUKA 14D:

Q3. Do you have guarantees for your approach.

We have added the theoretical analysis of the guarantees of our approach in A.4 in the Appendix. Our approach is guaranteed to be probabilistically complete. The rough intuition is as follows. Since our learned neural network assigns positive likelihood to all trajectories, given a set of valid motion planning paths from A to B, our learned distribution will always assign finite positive likelihood to it. Thus, given a very large number of samples, our sampler is guaranteed to find a solution to the motion planning problem.

Further, with optimal data and training, our approach can also be guaranteed to be asymptotically optimal. At a high level, since neural networks are universal function approximators, an optimal $E_\theta^*(.)$ can be learned by our models and the corresponding output distribution that can generate optimal plans almost surely.

Q4. Since you are using NNs, there is no guarantee that the success rate would be 100%?

As discussed earlier, our approach is also guaranteed to be probabilistically complete with a sufficient number of samples.

W5. Questions on measuring the success rate for RRT* or other sampling based algorithms.

While with unlimited sampling time, both RRT* and BIT* are probabilistically guaranteed to converge, in practice, especially in settings with very narrow paths to solutions, the amount of to sample a valid path can take exponentially long (for instance if there is a very narrow passage way from side of the workspace to the other). Thus, we evaluate the performance of RRT* and BIT* with a fixed time limit.

Q3. About the experiment settings.

(3a): What is the representation of the dynamic obstacles? Is it a vector consisting of the configurations from all timesteps in the trajectory?
Yes. In our setting, the representation of dynamic obstacles is a sequence of configurations indicating the trajectories of the presented obstacles. This representation is aligned with our baseline method SIPP.

(3b): Comparison to BIT*.
We have enclosed the performance of BIT*. The timeout condition for planning is set to 30 seconds for BIT*. We set the timeout condition of RRT* to 30 seconds in Maze2D and KUKA7D, and 60 seconds in Dual KUKA 14D.

Maze 2D:

KUKA 7D:

Dual KUKA 14D:

(3c): A general impression of the diffusion model is that it takes a long time to generate samples. Why is the planning time here lower than the other baselines?
The main speed-up is due to the use of DDIM[1] to accelerate the denoising process, which can reduce the number of denoising steps compared to DDPM. We have included the DDIM sampling details in Appendix A.2.3.

(3d): Quantitative results on the real-world dataset.
Thank you for the suggestion. We have appended quantatitive results on real-world ETH/UCY dataset in Table IX of A.3.1 in the appendix. We found using pixels overlap might be inaccurate due to the volume of pedestrians and their various moving dynamics. Therefore, we directly compare the similarity between the predicted motion trajectories and ground truth human trajectories in a held-out test set. We use ADE as the metric. Please refer to the A.3.1 in the Appendix for more details.

(3e): What is the timeout condition for the planning?
For sampling-based methods, the timeout condition of BIT* is set to 30 seconds across all environments. For RRT* and P-RRT*, the timeout condition is 60 seconds in Dual KUKA 14D, and 30 seconds in all other environments. For MPNet, we directly adopt the default number of trials, 80, as given their codebase. That is, a timeout flag will raise if the model has been called 80 times and still had not reached the target. Likewise, we set the number of trials to 150 for M$\pi$Net and AMP-LS.

(3f): Are the metrics averaged over all the cases, or only the successful cases?
The metrics are averaged over all the test cases.

[1] Song, Jiaming, Chenlin Meng, and Stefano Ermon. "Denoising Diffusion Implicit Models." International Conference on Learning Representations. 2020.

Thank you very much for carefully reading the paper and the valuable insights. We have added experiment results and updated the manuscript accordingly.

Q1. About the formulation of the compositionality.

(1a, 1b): Confusion about the actual equations used in the denoising process.
Sorry for the confusion caused. The equation we used is the Line 7 of Algorithm 1 and the unconditioned score is also used. Thus, the full equation in Line 7 is given by:

$

\epsilon_{\text{comb}} = 
  \nabla_{q_{1:T}} E_\theta(q_{1:T}^s, s, q_{\text{start}}, q_{\text{end}}, \varnothing) + 
   \sum_{i=1}^{N} \omega_i \nabla_{q_{1:T}} \big[ E_\theta^{i}(q_{1:T}^s, s, q_{\text{start}}, q_{\text{end}}, C_i) - E_\theta^{i}(q_{1:T}^s, s, q_{\text{start}}, q_{\text{end}}, \varnothing) \big] 

$

Likewise, in equation 5, the $\nabla_{q_{1:T}} E_\theta(q_{1:T}, t, q_{\text{start}}, q_{\text{end}}, C)$ should be

$

 & \nabla_{q_{1:T}} E_\theta(q_{1:T}, t, q_{\text{start}}, q_{\text{end}}, \varnothing) - \omega \nabla_{q_{1:T}}
 \big[
   E_\theta(q_{1:T}, t, q_{\text{start}}, q_{\text{end}}, C)  -
   E_\theta(q_{1:T}, t, q_{\text{start}}, q_{\text{end}}, \varnothing) 
 \big]    

$

For simplicity, we would like to use abbreviated forms in the original submission to stand from the update gradient but miss a proper declaration in the paper. We have updated the equations in the manuscript.


(1c): About Line 7 of Algorithm 1: Why does the unconditioned score use $E_\theta^1$? What is special about $E_\theta^1$ compared to the other $E_\theta^i$?
In practice, we need an unconditional score, though the particular model we get from is not important. In our experiments, we used $E_\theta^1$, but to make this clear we have replaced $E_\theta^1$ with $E_\theta$ in the algorithm block. The unconditional score can provide a basic prior of a motion trajectory, such as the coherency between two consecutive waypoints and kinematics constraints, while the conditioned score can provide environment specific constraints.

Q2. About the theory side of the compositionality:

(2a): If C1={o1,o2,o3,o4} and C2={o3,o4,o5,o6}, will C1 and C2 still be conditional independent?
We provided a detailed proof in the updated paper appendix A.5. At a high level, if we view the distribution p(traj|C1) as a uniform density over all trajectories avoiding C1 and p(traj|C2) as a uniform density over all trajectories avoiding C2, then p(traj|C1 $\cap$ C2) is directly proportional to the product of p(traj|C1) and p(traj|C2) and is conditionally independent.


(2b): Is the assumption of conditional independence still guaranteed for the intermediate t?
It's true that even if the independence holds at t=T, for the intermediate t, the score functions are not fully independent (due to added noise). However, as discussed in [1], even if the intermediate scores are not fully accurate, as long as they transition to 2 independent distributions at the end, the sampling is still theoretically sound.

[1] Du, Yilun, et al. "Reduce, reuse, recycle: Compositional generation with energy-based diffusion models and mcmc." International Conference on Machine Learning. PMLR, 2023.

Thank you for the patience and detailed illustration of your concerns. In our proof in Appendix A.5, We would like to emphasize that the sampling procedure at inference time actually won't be affected even in the case that $C_1 = \{o_1, o_2, o_3, o_4\}$ and $C_2 = \{ o_3, o_4, o_5, o_6 \}$.

Our previous response might cause your concerns and following is a more thorough explanation. We define $f_{C_i}(q_{1:T})$ as the probability density function over trajectories, where we uniformly assign positive likelihood to the trajectory $q_{1:T}$ if it satisfies the constraint $C_i$; otherwise, the likelihood is set to 0. Let $\mathcal{J}_{c_i}$ denote the set of trajectories that satisfy $C_i$. Then, we have

$

f_{C_i}(q_{1:T}) =   \rho_i  \text{ if } q_{1:T} \in \mathcal{J}, \quad  0 \text{ if } q_{1:T} \notin \mathcal{J}_{c_i}

$

where $\rho_i$ is a small constant (the first $\mathcal{J}$ should also have subscript $c_i$, we cannot render the whole equation if we put the $c_i$ in place). Similarly, we can define $f_{C_1 \cup C_2}$ as the probability density function of trajectories that satisfy both $C_1$ and $C_2$. Clearly, given any trajectory $q_{1:T}$, the probability density of $q_{1:T}$ is positive if and only if both $f_{C_1}(q_{1:T})$ and $f_{C_2}(q_{1:T})$ are positive. More specifically, we have

$

f_{C_1 \cup C_2}(q_{1:T}) = \gamma f_{C_1}({q_{1:T}}) f_{C_2}({q_{1:T}})

$

where $\gamma$ is a constant (note the proportionality constant in the previous equation). We can see that the joint probability density function equals to the scaled product of $f_{C_1}$ and $f_{C_2}$. While the above equation doesn't indicate independence, sampling using the score function for left side of the equation is the same as sampling from the summed score function for the right side of the equation because the score function is the gradient of the log probability and is invariant to the constant multiplier. Thus, the constant $\gamma$ here will not affect the test-time sampling process and thus, the procedure is correct.

Thank you for your constructive feedback. We have taken your comments into careful consideration and made revisions to our paper accordingly.

Q1. Missing some references.

We apologize for missing the references to RMPs and GF potential field methods. We have updated our Introdution and added the references in the Related Work. However, we believe that the contributions of our work is very different than these works, as we aim to learn potential fields generically from data for any types of potentials, including ones without clear close-form solutions such as multi-agent interaction. As potentials are learned from data, we found that our approach is much less vulnerable to adversarial minima compare to RMP (see newly updated Table X and Figure IX in the appendix our updated paper). Our approach could also be applied to different task spaces, by using the Jacobian to transform the inputs to each learned potential function into a shared task space.

Q2. Concerns on directly summing the potentials.

Empirically, we found that this worked well as demonstrated in our quantitative results in Table 3 and Table XI, and the inferred energy functions have a naturally defined scale based off the learned probability density over demonstrations. In cases when this is inadequate, we can use failures in motion planning to help tune the right coefficient to add to energy functions.

Q3. Usage of SDF as the repulsor.

SDF does provide accurate geometry to guide potential-based motion planning methods in the corresponding workspace. However,

1. simply using a SDF might not be able to resolve the local minima issue for the potential-based methods.
2. Constructing a SDF is highly workspace and robot dependent, and requires sophisticated computatation framework, which is especially challenging given limited observations of the environment.

Q4: The obstacle potential should not be in the C-space, it would be much easier to be constructed in the workspace.

As we directly learn potentials for both obstacles and attractors, we can define our learned potentials in any space. In our experiments, we directly learn our potentials in C-space, allowing them to be directly combined via simple addition.

Q1. Additional results on environments with concave obstacles and potential-based methods.

We constructed an additional environment with 7 concave obstacles and the quantitative results are shown below. Besides, we added a traditional potential-based motion planning method RMP[1] and a potential-guided sampling-based method P-RRT*[2]. We have also appended the qualitative results in Figure IX in the updated paper. For a more completethe quantitative table, please refer to the updated Table VIII and Table X in the appendix.

Motion Planning Performance on Maze2D with 7 Concave Obstacles:

Additional results of P-RRT:*

Maze 2D (convex):

KUKA 7D:

Dual KUKA 14D:

[1] Ratliff, Nathan D., et al. "Riemannian motion policies." arXiv preprint arXiv:1801.02854 (2018).

[2] Qureshi, Ahmed Hussain, and Yasar Ayaz. "Potential functions based sampling heuristic for optimal path planning." Autonomous Robots 40 (2016): 1079-1093.

Thank you for your insightful review and feedback. We have updated our manuscript and added experiment results based on your comments.

W1. Literature review and comparison with reactive local planning methods.

We have updated Introduction and Related Work of the paper and added the discussion on the mentioned methods. Our approach is very different from the traditional reactive motion planning approaches in that:

1. Our method is a learning-based motion planning approach that directly generates an end-to-end trajectory conditioned on the robot state state, goal state, and global environmental geometry. In comparison, reactive local planning methods usually adopt a greedy strategy and focus on local geometry, which is prone to be stuck in the local minima.
2. Purely reactive planning methods typically need to construct implicit obstacle representation in the robot configuration space, which demands sophisticated computational frameworks or even might require case-by-case designs. By contrast, our model is trained directly on the demonstrations, which is more accessible especially in high-dimensional robot configuration space since the potential of obstacles is learned in the robot configuration space.

W2. How our methods can overcome the local minima issues.

Convex obstacles can also result in the local minima issue in traditional potential-based planning. For example, the agent might get stuck due to the balance of the attractive force and repulsive force.

We summarize the reasons why the proposed planner can effectively avoid local minima below:

1. Planning with the learned global potential field. Our method generates motion plans by effectively considering the global geometry, start pose, and goal pose holistically in the latent space. The learned potential field can provide a strong global prior in long-horizon planning, in contrast to traditional potential-based methods that primarily consider the obstacles in the vicinity of the current pose.
2. Stochasticity in motion plan generation. The denoising process of our potential-based diffusion model is inherently stochastic, lowering the risk of local minima. Even when the the planner is stuck in a local minima, new plans can be generated by starting denoising from another noise, or by refining the problematic motion plans as given in Section 3.4. By contrast, most traditional potential based methods are deterministic and with limited capability to avoid or recover from the local minima.
3. Adaptive weighting in the latent space. Traditional potential-based methods usually comprise of attraction forces and repulsion forces and are sensitive to their weights. Such weights are highly problem dependent and a bad set of weights is one important reason for the local minima issues. In comparison, our learned potential method maps these two forces together in the unified learned latent space. Thus, the weighting is implicitly performed in the latent space based on the given conditions, which also avoids the heavy weight-tuning tasks.

W3&Q3: Is our method a single-query planning technique that would require re-training?

Our method does not need to be re-trained and can be generalized to any valid start and goal configuration. In test time, the input is the start state, goal state, and the environment geometry (e.g., obstacles information). No parameters of the model need to be updated.

Our model directly learns from the valid demonstration trajectories with no violation of kinematics and dynamic constraints of a given robot model. Such physically improbable trajectories are out-of-distribution samples and contradicts to the model's output distribution. If there is any newly added test-time dynamic constraints, we believe that our method can further subject to them by composing the corresponding potentials during motion generation.

W4&Q2: Proof of probabilistic completeness and optimality.

We give a proof on the probabilistic completeness and optimality in Appendix A.4. At a high level, since our learned neural network assigns positive likelihood to all trajectories, given a set of valid motion planning paths from A to B, our learned distribution will always assign finite positive likelihood to it. Thus, given a very large number of samples, our sampler is guaranteed to find a solution to the motion planning problem.

Finding the optimal $E_\theta^*(.)$ for asymptotic optimality, however, is hard. Empirically, we found that the optimization is easy, robust, and can produce decent trajectories, as shown in Table VIII, IX, X, XI, and Figure 6, 7, IX, X. Moreover, as for speed, it is sufficient for our method to generate high-quality motion trajectories with less than 10 optimization steps across many different environments.