Physics-informed Temporal Difference Metric Learning for Robot Motion Planning

We sincerely thank the reviewer for the comprehensive evaluation and response to the major concerns below.

1. lack of quantitative analysis of the generalization capability of the approach. A more comprehensive analysis would be more appropriate, for example, evaluating the performance across a wide range of unseen environments with varying complexities and against the Sota approaches.

Authors: We have added our generalization setup to NTFields and P-NTFields to show the ability of our method compared with previous physics-informed methods. In addition, we have now also separately reported the results of seen and unseen experiments in Appendix B.

2. For the ablation part, is it possible that using only one space configuration might be biasing your evaluations? Have you tried testing on different maze configurations or other types of spaces?

Authors: We have included more complex mazes in Appendix B. The results are consistent with what we reported in a single maze environment. Additionally, we would like to highlight that our experiments in Gibson, cluttered 3D environments, and 7 DOF manipulations can be considered ablation studies when compared to NTFields and P-NTFields. The previous methods do not utilize TD Loss, obstacle alignment loss, or causality loss, and they have been shown to perform poorly compared to our methods. Furthermore, these previous methods do not function at all in the 12 DOF manipulation environment, which further validates the significance of our proposed methods and their novel objective functions.

3. The claim of achieving high SR in the comparisons in Table 1 could be made less strong or reworded. The SR of the method is consistently on par or lower from the top SR in all examples. "Superior speed" in s5.3, maybe reword to computational performance or similar.

Authors: We thank reviewers for these suggestions and we have made the suggested changes as highlighted in the paper.

4. For table 1, clarify what you measure for time in the different cases.

Authors: We report computational time representing wall clock time taken by a CPU-based execution of all presented planners. We have clarified it in Section 5.

5. Expand on and give examples of the failure cases you mention when generalizing to unseen environments.

Authors: Currently, our method generalizes to relatively simple, unseen scenarios, such as 3D box environments and 7 DOF manipulations in a table-top setting. However, generalizing to unseen complex environments e.g. the Gibson dataset remains a challenging and unresolved issue. We believe this difficulty arises from the varying intricate obstacle geometries present in different Gibson scenarios, which are hard to represent within our current training set and model architecture. Therefore, in our future work, we aim to enhance the generalization ability of our method for complex scenarios with intricate obstacle geometries.

Dear Reviewer, thank you for your valuable feedback. We have revised the manuscript and provided detailed responses to address your concerns. We hope our revisions meet your expectations and kindly request your reassessment. Thank you for your time and consideration.

We sincerely thank the reviewer for the comprehensive evaluation and response to the major concerns below.

1. The ablation study is conducted on a single 2D maze environment

Authors: We have included more complex mazes in Appendix B. The results are consistent with what we reported in a single maze environment. Additionally, we would like to highlight that our experiments in Gibson, cluttered 3D environments, and 7 DOF manipulations can be considered ablation studies when compared to NTFields and P-NTFields. The previous methods do not utilize TD Loss, obstacle alignment loss, or causality loss, and they have been shown to perform poorly compared to our methods. Furthermore, these previous methods do not function at all in the 12 DOF manipulation environment, which further validates the significance of our proposed methods and their novel objective functions.

2. In the generalization to novel environments, both seen and unseen environments are used during testing

Authors: In Appendix B, Table 2, we have included the generalization statistics for both seen and unseen environments separately. The results indicate that the success rate (SR) for unseen environments is only slightly lower than that for seen environments, while the overall performance remains similar. This demonstrates our method's ability to generalize to unseen tasks. Additionally, we have extended the prior methods, NTFFields, and P-NTFields, with our attention-based architecture to enable their generalization across multiple environments, as their original architecture was not capable of scaling to accommodate multiple environments.

3. The article is too technical and a bit complex to follow for a reader who is not completely familiar with this class of approaches. Moreover, the figures are hard to decrypt, captions should provide more details on how to read the figures with legends depicting what each element represents.

Authors: We have revised our explanations for Figures 1 and 3 (previous 2), as these two figures are critical for understanding our method. Please let us know if this helps the reviewers understand our approach.

4. Have you conducted a sensitivity analysis to measure the importance of the choice of hyperparameters, and how to set them for each case?

Authors: We have now included guidelines for tuning hyperparameters in our revised Appendix C. Following these guidelines makes the hyperparameter selection process quite straightforward.

5. Is there a reason why the ablation study was not done in a quantitative manner showcasing that the obtained results are consistent across different environments?

Authors: We include Fig. 3 (previous Fig. 2) as it is crucial to understand the contour lines in that figure. These lines represent points with the same travel time function value, $T(q_s,q_g)$, and their normal direction indicates the optimal motion path toward the goal configuration. In a globally optimal value function, contour lines should not exhibit local minima. In Fig. 3 (previous Fig. 2), we observe that the contours produced by other methods contain spurious local minima or the normal direction penetrates the solid boundary, whereas our method eliminates these artifacts.

It’s important to note that the loss function measures the correctness of contour lines across the entire configuration space, while spurious local minima are localized phenomena. As a result, the improvement in loss may not appear substantial. However, this does not diminish the importance of the difference. Even a single spurious local minimum can trap the robot in its vicinity, preventing it from reaching the true goal configuration. We have revised our description in Section 5.1 to highlight the importance of correct contour lines while having low errors.

6. Why are both seen and unseen environments used during testing for the evaluation of the generalization to novel environments?

Authors: We highlight two types of unseen factors in our learning task: unseen start-goal configurations and unseen environments. In all tests, whether in seen or unseen environments, we consistently use unseen start-goal configurations. Testing in unseen environments ensures that our learned value function can handle any motion planning query within a newly encountered environment.

7. In Fig.1, what do the green points represent? What about the black line?

Authors: We revised Fig. 1 and its caption to better illustrate the example problem. In summary, to understand this figure, note that the Eikonal equation we want to solve is: $|T’(q)|=1, T(0)=0$. Our Eikonal loss, $L_E=(|T’(q)|-1)^2$, is evaluated at discrete sample points (green). The top figure shows the correct solution $T(q)=|q|$, and the bottom figure has spurious local minima. If we rely on $L_E$, on all sampled green points, we have $L_E=0$ for both plots. However, suppose we use $L_{TD}$. In that case, we can enforce for some point $q_1>0$, we have $T(q_1)-T(q_1-d)=d$ for a local perturbation $d>0$. This leads to a higher loss for the bottom solution than the top solution, which aligns with our desired behavior. We have updated the figure caption to reflect these points more clearly.

8. In Fig.2, what does the color and the contour lines represent?

Authors: The color shows the speed field, which is also the Truncated SDF of the obstacle, and we are training based on this speed field. The contour lines consist of points with the same travel-time function value $T(q_s,q_g)$. Following the negative gradient direction of the value function (normal direction of the counter line), we can find a globally optimal path leading to the goal configuration. We have briefly included that description in Section 5.1

9. Why is the success rate lower for the 7DOF manipulator at 87% compared to the other experiments, especially since for the 12DOF dual-arm, the method achieves a 91% success rate?

Authors: (Please refer to Answer 6) We appreciate the reviewer’s observation. The difference in success rates arises from the nature of the experiments. The 12-DOF dual-arm experiment focuses on scalability within a seen environment and unseen start and goal pairs. In contrast, the 7-DOF manipulator experiment evaluates the method’s ability to generalize to unseen environments, which poses a significantly greater challenge. This highlights the inherent trade-off between scalability in fixed settings and generalization across diverse scenarios.

Dear Reviewer, thank you for your valuable feedback. We have revised the manuscript and provided detailed responses to address your concerns. We hope our revisions meet your expectations and kindly request your reassessment. Thank you for your time and consideration.

We sincerely thank the reviewer for the comprehensive evaluation and response to the major concerns below.

1. Lower success rate than Sampling-based Motion Planners (SMP).

Authors: We acknowledge that, given sufficient computational time (up to 30 seconds in our evaluation), NTFields-type methods currently cannot achieve the same success rate as Sampling-based Motion Planners (SMPs) like RRT-Connect. This limitation affects all existing learning-based methods, and ongoing research in learning-based motion planning aims to address these challenges, including our work. In contrast, many SMPs are probabilistically complete and are expected to achieve a high success rate, as they will eventually converge to find a feasible or optimal solution if one exists. However, our method, like other learning-based approaches, offers significantly faster inference times than SMPs, making it far more suitable for time-sensitive, real-time control applications.

Regarding the incorporation of our approach with Model Predictive Control (MPC): The proposed method provides a value function by solving the Eikonal equation. This value function can be integrated into any downstream planner, such as RRT, $A^\star$, MPC, or trajectory optimization. We use MPC as a demonstration of this flexibility. Additionally, these downstream planners can accommodate extra costs, such as collision constraints that can be layered on top of the original value function to improve robustness.

2. Hyperparameters need tuning

Authors: Our method needs some hyperparameters, but their tuning guideline has been provided in our revised Appendix C. Under these guidelines, we discuss that tuning hyperparameters in our experiments is a trivial task

3. What is the benefit of solving the Eikonal equation for motion planning?

Authors: The primary advantage of using a neural Eikonal equation solver is its ability to quickly infer the globally optimal direction of motion to reach a goal configuration. In other words, it enables real-time responses to planning queries. In contrast, Sampling-based Motion Planners (SMP) are guaranteed to be probabilistically complete, that is, they will always find a feasible or optimal solution, but they often require significant computational effort for each query.

The Eikonal equation solution indicates the shortest travel time and defines the geometrically shortest path between two locations. The key assumption underlying the neural Eikonal solver is that the optimal value function can be effectively approximated by a neural network. While this assumption generally holds, it can be violated, leading to spurious local minima, as shown in Fig. 3 (previous Fig. 2). Nonetheless, our method demonstrates substantial improvements over previous approaches to solving the Eikonal equation.

As the reviewer noted, a limitation of our method is its inability to handle kinodynamic constraints directly. We have acknowledged this in the limitations section. However, our value function can be effectively integrated as a cost-to-go function within kinodynamic MPC frameworks. Notably, prior work [1] has addressed this challenge by leveraging a precomputed RRT tree as a terminal cost, and similar strategies could be applied to extend our method for handling kinodynamic constraints.

[1] The Value of Planning for Infinite-Horizon Model Predictive Control

4. MPC is not good and $A^\star$ search is better.

Authors: By integrating the learned travel-time function with online MPC, we effectively generalize the concept of travel time to a more versatile value or cost-to-go function. This approach provides a flexible framework for addressing planning problems. For example, we could leverage completeness-guaranteed search algorithms, such as $A^\star$ or T-RRT [1], guided by our learned value function. However, combining the value function with different search frameworks is beyond the scope of this work. Our primary focus is on learning a good value function.

Each search framework offers distinct advantages. Online MPC can quickly escape local minima in the value function but performs only local searches and lacks completeness. In contrast, methods like T-RRT and $A^\star$ guarantee completeness but require more computational time. If desired, we could conduct experiments demonstrating the performance of our approach when combined with these alternative search frameworks.

[1] Sampling-Based Path Planning on Configuration-Space Costmaps

5. What is the running time for RRTC for successful cases solved by our method

Authors: In the manipulation environment, for only successful cases the RRTC statistics are: Time $0.37 \pm 0.24$, Length $1.83 \pm 0.92$ and L-PRM statistics are: Time $0.15 \pm 0.42$, Length $1.84 \pm 0.94$. Note that these times are still significantly higher than our approaches in general.

Dear Reviewer, thank you for your valuable feedback. We have revised the manuscript and provided detailed responses to address your concerns. We hope our revisions meet your expectations and kindly request your reassessment. Thank you for your time and consideration.

We sincerely thank the reviewer for the comprehensive evaluation and response to the major concerns below.

1. Provide more explanation about Fig.1.

Authors: To understand this figure, note that the Eikonal equation we want to solve is: $|T’(q)|=1, T(0)=0$. Our Eikonal loss, $L_E=(|T’(q)|-1)^2$, is evaluated at discrete sample points (green). The top figure shows the correct solution, and the bottom figure has spurious local minima.

If we rely on $L_E$, on all sampled green points, we have $L_E=0$ for both plots. However, suppose we use $L_{TD}$. In that case, we can enforce for some point $q_1>0$, we have $T(q_1)-T(q_1-d)=d$ for a local perturbation $d>0$. This leads to a higher loss for the bottom solution than the top solution, which aligns with our desired behavior. We have updated the figure caption to reflect these points more clearly.

2. The empirical result does not significantly improve over NTFields.

2a. In Fig.2 (the maze), $L_E$ is the most important component, other loss terms only provide small improvements.

Authors: To appreciate the significance of our improvement, it is crucial to understand the contour lines in Fig. 3 (previous Fig. 2). These lines represent points with the same travel time function value, $T(q_s,q_g)$, and their normal direction indicates the optimal motion path toward the goal configuration. In a globally optimal value function, contour lines should not exhibit local minima. In Fig. 3 (previous Fig. 2), we observe that the contours produced by other methods contain spurious local minima or the normal direction penetrates the solid boundary, whereas our method eliminates these artifacts.

It’s important to note that the loss function measures the correctness of contour lines across the entire configuration space, while spurious local minima are localized phenomena. As a result, the improvement in loss may not appear substantial. However, this does not diminish the importance of the difference. Even a single spurious local minimum can trap the robot in its vicinity, preventing it from reaching the true goal configuration. We have revised our description in Section 5.1 to highlight the importance of correct contour lines while having low errors.

2b. Why is planning fast important?

Authors: In the real world, fast planning is crucial for realizing a wide range of real-time robotic applications in fields such as smart cars, robotic surgery, UAVs (unmanned aerial vehicles), and robot-assisted manufacturing. As the reviewer pointed out, our training assumes arbitrary start-goal configurations for the robot, enabling zero-shot responses to any planning query, which enhances its applicability across all the domains mentioned above. Additionally, the Eikonal equation solution provides a global value function. It can be incorporated into any off-the-shelf planning strategies such as MPC and trajectory optimization. These downstream planners can deal with scenarios including dynamic obstacles guided by our learned value function. Nevertheless, such analysis is beyond the scope of this paper.

3. Clarification about paper writing

3a. Since $S^\star$ is manually defined, why learn $S$ rather than use $S^\star$ for planning? Eq.3 seems to propose training the function S such that it matches $S^\star$ at starting and goal configurations. Then, a perhaps naive question is why learn $S$ rather than just using $S^\star$ as the motion planner? How is the learned $S$ used to generate motion plans during online inference time?

Authors: Please note that $S$ or $S^\star$ cannot be directly used for motion planning as they simply provide a scalar value of a given configuration based on its distance to the obstacle. The Eikonal equation provides a way to relate this speed function to the travel time function $T$. Our objective is to learn $T$ in a self-supersized manner by utilizing $S^\star$. Our network prediction is $T$ and we compute $S=1/\|\nabla T\|$ and the $L_E$ loss is based on $S$ and $S^\star$. Finally, we can use $\nabla T$ for motion planning. With only $S^\star$, the problem becomes a trajectory optimization problem, and it is easy to get into the local minimum, while our method can provide an approximate global solution. We have explained this issue in Section 3.2.

3b. What is the motivation of metric learning?

Authors: It is well-received in the learning community that, when a learned function has a certain property, such as being a metric or being Lipschitz, the neural network should adhere to these properties [1]. In our case, our learned value function is exactly a distance metric between start and goal configurations. The three properties, non-negativity, symmetry, and triangle inequality, hold for such functions. By aligning the network output with these properties, we enhance the training efficacy and robustness.

[1] Optimal Goal-Reaching Reinforcement Learning via Quasimetric Learning

3c. Why is $L_1$ distance preferred to Euclidean distance?

Authors: To illustrate why our distance metric is preferable, consider a scenario where the environment is a 2D circle. Let’s denote the two poles of the circle as points A and B. We know that there are two shortest paths (two half-circle arcs) between these points. However, if we use Euclidean distance, there exists only one shortest path between the latent representations of points A and B. The midpoints of the two half-circle arcs map to the same point in the latent space, yet the actual distance between them on the 2D circle is not zero.

As a result, to accurately reflect the true distance between points A and B, the 2D circle would need to be compressed into a single straight line. In contrast, by utilizing our distance metric with $L_1$ distance, we can avoid such inconsistencies. For example, if we transform the circle into a 2D rectangle with points A and B mapped to two opposite corners and the midpoints to the other corners, we can learn the correct distances. We have included the above discussion and its illustration in our paper in Section 4.2.1.

4. What is the benefit of MPC?

Authors: The main advantage of Model Predictive Control (MPC) is its flexibility rather than its speed. In the original work on NTFields, the learned value function gradients were utilized directly for motion planning. In contrast, with MPC, the learned value function serves only as a heuristic to guide the motion planning process. The MPC algorithm employs search-based techniques to calculate feasible and nearly optimal solutions based on these heuristics. This approach allows the online planning algorithm to accommodate additional constraints, such as dynamic obstacles or new user-defined cost functions that can be layered on top of the original value function. Furthermore, our results indicate that combining MPC with our approach leads to an improved computation time of 0.056 seconds, which is even faster than the 0.074 seconds recorded with NTFields. This point is discussed in Section 4.3.

5. Mention known environment, free space.

Authors: We have now explicitly mentioned this assumption in section 3.1.

6. Fig.2 is a key result. However, for people outside the field, it is unclear how to interpret it. It would be great to add some guidance.

Authors: We have revised our description in 5.2 to provide guidance on interpreting the results presented in Fig. 3 (previous Fig. 2).

7. Clarification about the attention mechanism

Authors: Our network architecture utilizes implicit neural representations, which are MLP-like structures that take spatial coordinates as input to predict signals such as the travel time field. To condition this structure on environmental input, we compute a latent code from the environment's point cloud and integrate it with the MLP. While a straightforward approach is to concatenate the latent code with one hidden layer, recent research has shown that using attention mechanisms significantly enhances the network's representational power.

The attention mechanism allows the network to selectively focus on relevant parts of the environment, dynamically adjusting its output based on the spatial context provided by the point cloud. This capability improves the network's ability to generalize across diverse and unseen environments, making it an essential component for robust motion planning in complex settings. We have revised our Section 4.2.2 to include the above contextual information.

8. Connection between online RL and NTFields

Authors: We agree that physics-informed motion planning methods can be viewed as a specific form of offline goal-conditioned RL. These approaches focus on solving motion planning tasks by leveraging the structure of the environment rather than directly interacting with it, as in classical RL.

While traditional RL often involves learning policies under complex dynamics, physics-informed methods simplify dynamics (e.g., assuming constant velocity control) and instead focus on handling more intricate environmental constraints. The Eikonal equation provides a principled way to represent the cost-to-go function in such settings, effectively embedding goal-conditioned behavior directly into the training process.

In Sec. 2, we have highlighted the synergy between our method and offline RL.

9. As $S$ is used for collision avoidance, why do we need $L_N$?

Authors: $L_N$ is used to provide an initial guess for the collision-avoiding behavior of the value function. Our findings indicate that NTFields often get stuck in local minima during the early stages of training, making it difficult for them to escape without the influence of this term.

Dear Reviewer, thank you for your valuable feedback. We have revised the manuscript and provided detailed responses to address your concerns. We hope our revisions meet your expectations and kindly request your reassessment. Thank you for your time and consideration.