Add G-RRT* (Greedy RRT*) planner#95
Conversation
zkingston
left a comment
zkingston
left a comment
There was a problem hiding this comment.
Thanks for the contribution! I've left some comments on the main GRRT* planner, mostly nits. I'll look at the rest later.
| { return buffer.get() + index * Configuration::num_scalars_rounded; }; | ||
|
|
||
| std::vector<std::size_t> parents(settings.max_samples); | ||
| std::vector<float> costs(settings.max_samples, std::numeric_limits<float>::infinity()); |
There was a problem hiding this comment.
May be careful with infinity due to numeric issues with compiler optimizations.
There was a problem hiding this comment.
Switched to std::numeric_limits::max() to be safe!
| } | ||
| start_chain.push_back(current); | ||
|
|
||
| for (int i = static_cast<int>(start_chain.size()) - 1; i >= 0; --i) |
There was a problem hiding this comment.
Probably not going to overflow, but is int the right width?
There was a problem hiding this comment.
You're right! I just replaced it with reverse iterators.
| float cost_delta = costs[nbr_node.index] - nbr_new_cost; | ||
|
|
||
| auto &old_parent_children = children[parents[nbr_node.index]]; | ||
| old_parent_children.erase( |
There was a problem hiding this comment.
Curious how efficient this is compared to just marking the old as invalid and extending. Also, potentially reserving space for the neighbors in advance. Not sure how expensive all these operations are.
There was a problem hiding this comment.
Yeah, you're right. The erase-remove was directly copied from the OMPL implementation, but it's O(n) for both find and remove.
I've replaced that with the overwrite-pop pattern - same O(n) find but O(1) removal.
I also looked at the mark-as-invalid pattern you mentioned and micro-benchmarked it against different implementations. Overwrite-pop wins overall (the hash-based mark patterns have expensive insert/lookup costs, and a simple linear scan + O(1) pop wins over hash or vector bookkeeping).
Also added neighbors.reserve(settings.max_samples) following the pattern in aorrtc.hh and fcit.hh.
claytonwramsey
left a comment
claytonwramsey
left a comment
There was a problem hiding this comment.
This PR looks mostly good to me.
There's currently no visualization script (at least that I can find) to verify that the planner actually produces correct solutions, unless @zkingston has one that I do not know about. The typical scripts I use for satisficing planners (evaluate_mbm and sphere_cage_example) seem to time out.
I can't sensibly accept a PR for a planner until I can verify its correctness with a viz, so it is currently in request-changes until I figure out how to.
Maybe we should add a demo script for optimal planning visualization, to show that the plan quality improves over search time? I will consider experimenting with this later.
| result.size.emplace_back(1); | ||
| return result; | ||
| } | ||
| } |
There was a problem hiding this comment.
is it necessarily correct to check straight-line solutions first?
Also, this would be incorrect -- selecting the first valid straight line solution necessarily means skipping a (possibly faster) non-straight solution to a distinct goal.
There was a problem hiding this comment.
yup you're right! I totally forgot about the multiple goals. removed that block in new changes
| std::shared_ptr<ProlateHyperspheroidRNG<Robot>> phs_rng_ptr; | ||
| if (settings.use_phs and goals.size() == 1) | ||
| { | ||
| phs_ptr = std::make_unique<ProlateHyperspheroid<Robot>>(start, goals[0]); |
There was a problem hiding this comment.
PHS sampling should support multi-goal; but I am willing to say it's not blocking for this PR.
TODO: make AORRTC and friends also do PHS sampling for multi-goal.
There was a problem hiding this comment.
That's doable! Ping me again once you start working on multi-goal support for PHS sampling. I can help you with the changes in grrtstar
| }; | ||
|
|
||
| // Trees for balanced bidirectional growth | ||
| bool tree_a_is_start = not true; |
There was a problem hiding this comment.
Just jumping in Clayton, on a slightly tangential line, in rrtc we have
vamp/src/impl/vamp/planning/rrtc.hh
Line 76 in 99bd1c4
There was a problem hiding this comment.
yeah i was following the rrtc pattern, just changed back to false
| @@ -0,0 +1,690 @@ | |||
| #pragma once | |||
There was a problem hiding this comment.
perhaps include comment with citation either at top of file or on the class? @zkingston do you have opinions?
There was a problem hiding this comment.
I'm good with any options, up to you folks!
There was a problem hiding this comment.
I think it'd be good practice to include the citation in the file akin to OMPL, please go ahead and add.
|
@claytonwramsey @zkingston I added two more parameters to make it more robust and easier to use:
I think we need to explicitly set the iteration budget in the sphere cage example for anytime planners, otherwise it will run for very long (up to the default iteration budget, I think). Some results below: Sphere cage benchmark (Panda, single goal, halton, default range=2.0, simplification applied to all paths) Satisficing mode - first solution from each planner. After simplification, all three converge to the same cost:
Anytime convergence - both planners with optimize=True at increasing iteration budgets:
|
4 participants
Hi everyone! This follows up on #24, where I mentioned adding a bidirectional asymptotically optimal planner as an alternative to the existing AORRTC (G-RRT* is rewiring-based, as it builds on RRT*). It took a bit longer than expected, but here it is! The related publication can be found here.
I haven't updated the README yet - happy to do so once you're on board with including this planner in VAMP.
I tested this on the existing sphere cage problem. You can find some results below for both deterministic and randomized versions. I also included this test script, which might be useful for others to try out and use as a reference for adding new anytime planners.