Parallelised Diffeomorphic Sampling-based Motion Planning
Authors: Tin Lai
Weiming Zhi
Tucker Hermans
Fabio Ramos
Venue: CoRL2021 Poster (2021)
Keywords: Sampling-based motion planning
diffeomorphism
Normalising flows
Sampling distribution
RRT
PRM

Abstract: We propose Parallelised Diffeomorphic Sampling-based Motion Planning (PDMP). PDMP is a novel parallelised framework that uses bijective and differentiable mappings, or diffeomorphisms, to transform sampling distributions of sampling-based motion planners, in a manner akin to normalising flows. Unlike normalising flow models which use invertible neural network structures to represent these diffeomorphisms, we develop them from gradient information of desired costs, and encode desirable behaviour, such as obstacle avoidance. These transformed sampling distributions can then be used for sampling-based motion planning. A particular example is when we wish to imbue the sampling distribution with knowledge of the environment geometry, such that drawn samples are less prone to be in collision. To this end, we propose to learn a continuous occupancy representation from environment occupancy data, such that gradients of the representation defines a valid diffeomorphism and is amenable to fast parallelise evaluation. We use this to ``morph'' the sampling distribution to draw far less collision-prone samples. PDMP is able to leverage gradient information of costs, to inject specifications, in a manner similar to optimisation-based motion planning methods, but relies on drawing from a sampling distribution, retaining the tendency to find more global solutions, thereby bridging the gap between trajectory optimisation and sampling-based planning methods.


Reviews:
[{"review_id": "hC1FqkYyun4", "reviewer": "Reviewer_QAiv", "summary": "The paper is about a new motion planning framework called PDMP (Parallelised Diffeomorphic Sampling-based motion Planning), which morphs the sampling distribution in the state space of the robot using external cost functions. This framework can be combined with any motion planner and basically replaces the sampling step of each planner with a new informed sampling framework. This informed sampling framework works by employing a diffeomorphism, which maps samples towards a lower cost function in the state space. This creates more valuable samples, which have a higher probability of being collision-free. In two experiments, the authors demonstrate that their framework can be applied to two manipulation scenarios and that any motion planner can be substituted into their framework.", "questions": "", "limitations": "", "rating": 2, "confidence": null, "soundness": null, "presentation": null, "contribution": null, "strengths": "", "weaknesses": "", "quality": null, "clarity": null, "significance": null, "originality": null, "strengths_and_weaknesses": "Strengths:\n\n-- Having a good sampling distribution is key to plan efficiently through high-dimensional state spaces. I fully agree with the authors that it makes sense to think more deeply about how such a sampling distribution could look like and how we can deform/map samples to make them more valuable.\n\nWeaknesses:\n\n-- Your approach works by sampling uniformly, then mapping those samples towards a lower cost in the state space. While I think this idea make sense, it is unclear if you can actually keep completeness (and optimality) guarantees. You use also uniform sampling if there are no informed samples, however, as your Fig. 3 demonstrates, the number of uniform sampled points might (in the limit) converge to zero. In that case, I don't think you could still argue for completeness. I would therefore recommend that the authors think more deeply about how to keep completeness in this framework or by adding a small epsilon bias of uniform sampling. In any case, a discussion of those properties seem to be more valuable compared to the current discussion in Sec. 4.3 for example (see comment below).\n\n-- In Sec. 4.3 you introduce an additional cost which adds a higher probability of getting samples close to zero radians. However, it is unclear why you would do that. There is no motivation given in this section, which would let me see how a zero radian sample might be better than a random sample. It seems the purpose of this whole section is just to demonstrate that combined cost functions are possible. However, I would either remove this section or better motivate why a zero radian cost makes sense in a manipulation task.\n\n-- In Sec. 4.1 you argue that PDMP improves the finding of solutions significantly. However, you (1) did not say what you mean by significantly, and (2) just demonstrating the algorithm on two scenarios is not sufficient to make such claims. The biases you introduced could also be implict biases for those specific tasks you evaluated on. It would be better if you could add more variations or more scenarios to strengthen your claim that PDMP improves upon the given sampling algorithms. \n\nMinor Comments:\n\n-- Consider using a different colorscheme for the robots in Fig. 5, they are difficult to distinguish in b/w print.\n\n-- Fig. 3 should come before Fig. 4, not after Fig. 6.\n\n-- You did not label the axes in Fig. 6\n\n-- Missing Lazy-PRM in Fig. 4 (right). Also: which experiments are shown here? I guess it is the divider and the cupboard. Please clarify.\n\n-- Line 322: \"from defined from\" -> \"from\" ?\n-- Line 153: \"the states its C-space\" -> \"the C-space\" ?\n-- Line 155: gradients -> gradient\n-- Line 168: \"the the Jacobian\" -> \"the Jacobian\"\n-- Line 123: \"is a\" -> \"as a\"\n-- Line 140: \"is the resulting By the\" -> Unclear. Missing sentence part!?", "was_revised": false, "final_justification": "", "rebuttal": "", "claims_and_evidence": "", "methods_and_evaluation_criteria": "", "theoretical_claims": "", "experimental_designs_or_analyses": "", "relation_to_broader_scientific_literature": "", "essential_references_not_discussed": "", "other_strengths_and_weaknesses": "", "other_comments_or_suggestions": "", "questions_for_authors": "", "summary_of_paper": "", "summary_of_recommendation": "While the paper shows promise, the authors have only evaluated their algorithm on two scenarios without randomization, which makes it rather difficult to assess if their method is really beneficial. Also, there is no discussion about completeness/optimality, which I would have expected in a more planning-oriented paper. There were also several instances of sloppy writing and unlabeled axes, which further shows that the paper is still a bit immature and needs further improvement. ", "technical_quality": "Good: Generally convincing, but some additional experiments or analyses needed.", "clarity_of_presentation": "Fair: Major revision needed to improve organization or clarity.", "potential_impact": "", "robotics_focus": "", "quality_of_limitations": "", "questions_for_rebuttal": "", "reasons_to_accept": "", "reasons_to_reject": "", "questions_to_authors": "", "main_review": "", "summary_of_the_paper": "", "summary_of_the_review": "", "strength_and_weaknesses": "", "limitations_and_societal_impact": "", "review_text": "", "extra_scores": {"impact": "2: Unsure, difficult to assess.", "reviewer_expertise": "Very good: Comprehensive knowledge of the area"}, "extra_text": {"issues": "-- Please add a section where you discuss completeness (and optimality) guarantees (see comment above)\n\n-- Remove Sec. 4.3 or add a better motivated cost function\n\n-- Add more experiments and/or domain randomization to strengthen the case for your framework. Please also include Lazy-PRM in the cupboard experiment (or remove it from the divider)"}}, {"review_id": "ZjKvLa-xp1", "reviewer": "Reviewer_9VQ2", "summary": "The paper presents Parallelised Diffeomorphic Sampling-based Motion Planning (PDMP), a novel parallelised framework which leverages diffeomorphisms to transform sampling distributions of sampling-based motion planners based on specified costs (such as those for obstacle avoidance) in the more favorable space (Cartesian space) rather than in the Configuration Space. The paper presents the algorithm which in the baseline scenario will revert back to the performance of the original Sampling-based Motion Planning (SBMP) algorithm (such as the RRT, RRT-connect, etc), but in general improves upon the performance of these algorithm in terms of higher success rate and lower planning times. In the experimental section, the PDMP algorithm is benchmarked against the original SBMP algorithms, as well as tested for motion planning on a real robot setup. Details are also provided for parallelizing the algorithm with GPU computations.", "questions": "", "limitations": "", "rating": 4, "confidence": null, "soundness": null, "presentation": null, "contribution": null, "strengths": "", "weaknesses": "", "quality": null, "clarity": null, "significance": null, "originality": null, "strengths_and_weaknesses": "Strengths:\n(1) The paper is well-written. To be honest: I really enjoyed reading the paper! Great work!\n(2) Implementation code is attached in the Supplementary Materials.\n\nWeaknesses:\nOnly some minor things as mentioned in the `Issues` section.", "was_revised": false, "final_justification": "", "rebuttal": "", "claims_and_evidence": "", "methods_and_evaluation_criteria": "", "theoretical_claims": "", "experimental_designs_or_analyses": "", "relation_to_broader_scientific_literature": "", "essential_references_not_discussed": "", "other_strengths_and_weaknesses": "", "other_comments_or_suggestions": "", "questions_for_authors": "", "summary_of_paper": "", "summary_of_recommendation": "This is an excellent paper. I strongly recommend to accept the paper.", "technical_quality": "Excellent: Very convincing and thorough.", "clarity_of_presentation": "Excellent: Enjoyable to read.", "potential_impact": "", "robotics_focus": "", "quality_of_limitations": "", "questions_for_rebuttal": "", "reasons_to_accept": "", "reasons_to_reject": "", "questions_to_authors": "", "main_review": "", "summary_of_the_paper": "", "summary_of_the_review": "", "strength_and_weaknesses": "", "limitations_and_societal_impact": "", "review_text": "", "extra_scores": {"impact": "4: The work contains interesting new ideas that will potentially have major impact in robotics or machine learning.", "reviewer_expertise": "Very good: Comprehensive knowledge of the area"}, "extra_text": {"issues": "While Equations (1), (2), and (3) explains how diffeomorphisms are leveraged to transform the cost vector field from the task/Cartesian space to the Configuration (C) space, I am not very clear on how the sampling distributions are transformed. Can the authors please provide some concise equations that explain the sampling distribution transformation?"}}, {"review_id": "WE9L6Sr2ucw", "reviewer": "Reviewer_mUoR", "summary": "Combines sampling-based planning with gradient-based information, as a way to both globally explore the space but also exploit local cost information. The idea is to start with a regular sampling distribution (called base distribution) and to morph it into a more informative obstacle-aware distribution. In practice, the paper actually does not morph the distribution, instead it takes individual samples and propagates them along the integral curves of the vector field defined by the cost gradients. This results in samples at some \"earlier\" time t of the vector field flow which should have a higher chance to be away from obstacles (assuming the cost encodes obstacle distance). \n\nThe authors describe an algorithm that relies on parallelization for propagation samples on the CPU for building the tree. Experiments with robotic manipulator reaching constrained targets are presented with comparison to standard RRT methods. \n\n", "questions": "", "limitations": "", "rating": 2, "confidence": null, "soundness": null, "presentation": null, "contribution": null, "strengths": "", "weaknesses": "", "quality": null, "clarity": null, "significance": null, "originality": null, "strengths_and_weaknesses": "+ the paper presents a novel idea for adjusting samples along cost gradients to increase their quality\n+ the method naturally exploits parallelism so could be done efficiently\n+ a NN is used for the occupancy which gives a differentiable cost, the gradient of which is used for the flow\n\n- it is not clear what happens if a sample from the base distribution is in the interior of a solid obstacle where basically the occupancy gradient will be zero, the paper doesn't explain how this is addressed. It almost feels like the method would work only for samples at the boundary of an obstacle? \n- it is not clear for how long the samples need to propagated along the gradients\n- it is not clear how the cost gradient is computed for an arbitrary environment (it's \n- would have been great to see an application to simple planar obstacle environment as an illustration of the method, and to see how the deformed samples look like, there isn't a singe picture in the paper illustrating the sample deformation\n\n\nOther minor comments:\n- not clear what Figure 1 shows, is there a corresponding real-world scene that can be added for clarity?\n- line 140 typo: \"resulting By\"", "was_revised": false, "final_justification": "", "rebuttal": "", "claims_and_evidence": "", "methods_and_evaluation_criteria": "", "theoretical_claims": "", "experimental_designs_or_analyses": "", "relation_to_broader_scientific_literature": "", "essential_references_not_discussed": "", "other_strengths_and_weaknesses": "", "other_comments_or_suggestions": "", "questions_for_authors": "", "summary_of_paper": "", "summary_of_recommendation": "The idea of using gradients from differentiable NN cost and propagating samples along them is promising. Applying this to occupancy gradients though seems to a bit contrary to the original motivation. A simple intuitive and visual illustration of the method would have helped but is lacking. ", "technical_quality": "Fair: Significant additional evidence required to support the claims.", "clarity_of_presentation": "Very Good:  Well organized and clear.", "potential_impact": "", "robotics_focus": "", "quality_of_limitations": "", "questions_for_rebuttal": "", "reasons_to_accept": "", "reasons_to_reject": "", "questions_to_authors": "", "main_review": "", "summary_of_the_paper": "", "summary_of_the_review": "", "strength_and_weaknesses": "", "limitations_and_societal_impact": "", "review_text": "", "extra_scores": {"impact": "4: The work contains interesting new ideas that will potentially have major impact in robotics or machine learning.", "reviewer_expertise": "Very good: Comprehensive knowledge of the area"}, "extra_text": {"issues": "- simple 2d environment with obstacles and some simple manipulator (e.g. even with 2dof is fine) , showing the base samples and how they are propagated will be useful\n- explanation of why the occupancy cost gradients even make sense for this application"}}, {"review_id": "Ms84OdFld_m", "reviewer": "Reviewer_Dafq", "summary": "The paper proposes Parallelized Deiffeomorphic Sampling-based Motion Planning (PDMP) to combine the advantages of trajectory optimization and sampling-based methods for planning. The method borrows ideas from normalizing flows and leverages gradient information to speed up motion planning. A complete diagram of the flow is provided. The experiments are conducted based on two scenarios to show that the proposed method can have higher success rates.", "questions": "", "limitations": "", "rating": 3, "confidence": null, "soundness": null, "presentation": null, "contribution": null, "strengths": "", "weaknesses": "", "quality": null, "clarity": null, "significance": null, "originality": null, "strengths_and_weaknesses": "Strengths\n\n- The attempt to combine trajectory optimization and sampling method for planning is interesting. It can potentially bring about some new ideas in this area.\n- Experiments on real robots are conducted to verify the algorithm. It is also compared with some traditional sampling methods to show that they can have higher success rates.\n- Parallelization is considered to accelerate the implementation of the methods. This optimization effort can make the method more practical.\n\nWeaknesses\n\n- The experiment validation is limited. Only two scenarios are considered in the evaluation.\n- Also, no comparisons with the trajectory optimization method are made.", "was_revised": false, "final_justification": "", "rebuttal": "", "claims_and_evidence": "", "methods_and_evaluation_criteria": "", "theoretical_claims": "", "experimental_designs_or_analyses": "", "relation_to_broader_scientific_literature": "", "essential_references_not_discussed": "", "other_strengths_and_weaknesses": "", "other_comments_or_suggestions": "", "questions_for_authors": "", "summary_of_paper": "", "summary_of_recommendation": "The overall idea and approach are interesting. Despite the concerns on experiment validation, I am leaning towards acceptance.", "technical_quality": "Very Good:  Convincing.", "clarity_of_presentation": "Very Good:  Well organized and clear.", "potential_impact": "", "robotics_focus": "", "quality_of_limitations": "", "questions_for_rebuttal": "", "reasons_to_accept": "", "reasons_to_reject": "", "questions_to_authors": "", "main_review": "", "summary_of_the_paper": "", "summary_of_the_review": "", "strength_and_weaknesses": "", "limitations_and_societal_impact": "", "review_text": "", "extra_scores": {"impact": "3: The work is incremental and unlikely to have much impact, even though it may be technically correct and well executed.", "issues": "The major issues are in the experimental validation.", "reviewer_expertise": "Good: General knowledge of the area"}, "extra_text": {}}]

Author rebuttal:

Meta-review: In this work, the authors propose an approach to combine the respective advantages of the sampling-based and optimization-based planning methods. The reviewers agree regarding the motivation of the approach and the relevance to the field. 

The major concerns are related to the experimental validation and the results -- validation of the proposed approach on additional systems and/or environments (with multiple different initial seeds), and comparisons with additional optimization-based baselines will significantly strengthen the paper. 

The paper will benefit from a clear discussion on the completeness and optimality of the proposed approach and the addition of some visualizations on sampling distribution transformation (even if for simple planning problems).  

Besides addressing the major issues mentioned above, the authors should also revise the manuscript according to the other clarifications requested and the suggestions provided. 

===== Post rebuttal =====

The authors have sufficiently addressed reviewers' concerns; specifically, including additional experiments and baselines was particularly helpful. For the final version, it is still important to address the concerns of Reviewer QAiv regarding the completeness proof.

Decision: Accept (Poster)