Create a D*-Based Planner *or* Update Smac/Theta*/NavFn to allow for D* behavior optionally

[SteveMacenski]

From a discussion in Slack with Vinny Ruia, Christian Fritz, and Ilia Baranov, there's interest in a D* Lite / Field D* algorithm in Nav2.

My thoughts is that rather than improving dynamic replanning of paths, it by be better to have improved default replanning behaviors (only replan when necessary or after very long periods of time; when replanning make sure it is 'better' than the previous one by some metric like lower cost, shorter, or the original path cost has spiked). I think this deals with alot of the problems one could want, but a incremental replanner could still be useful. And covers the computation portion of the D* algorithm's improvements.

A down-side of one of these methods though is that they assume small amounts of change over the plan, not large changes in dynamic spaces. If a section of the path is invalidated, it'll attempt to replan that, but leave other sections alone -- which may themselves have more optimal solutions with shifting dynamic obstacles. This will create a local minima until a clean replan is done trapping the robot to go down a certain direction (which, in some cases, sounds like the committed behavior some may want, but worth noting that for every good 'commit to this route' there'll be a bad 'I replanned halfway around the warehouse and now I'm committed to it' due to a blocked corridor or something similar).

But, with all that said, I think this may still fill a niche that Nav2 users could leverage. Particularly for

• very low compute platforms
• and environments that don't change very much

---

FYI: In Smac Planner, we have a cache obstacle heuristic parameter that does similarly. We cache the existing cost-space between iterations which is a massive runtime improvement. The A* is the same, but we don't recompute H from scratch which is considering the cost field with a dynamic A*. While not the same, it sounds like its to solve a similar concern. It turns a ~200+ms planning request into single-digit milliseconds, so at that point, any further improvement would be pretty small. However, it probably isn't formulated the same way (since I didn't model it on D*) so probably doesn't have the same functional behavior. Maybe worth exploring that direction for Smac and a standalone D* planner (or modify NavFn/Smac2D/Theta* to have that option)

[chfritz]

Adding my comment from Slack here, too.

Just a clarification here: D* Lite is not a plan repair algorithm, as your examples could suggest, it's a plan-space reuse algorithm. [..] These algorithms essentially maintain the same optimality guarantees of A* while reusing as much of the already annotated forward search space of the original planning effort. It's a bit easier to understand from a (MDP-like) policy point of view than from a sequential plan point of view: if the changes that happened did not cause state-values to change, then the local policy around these unaffected states stays the same and value propagation (Bellman backups) can stop, saving computation.

Just like video encoders that operate on diffs, these algorithms perform well when called often on small changes, rather than rarely on big changes. So it's all about the rate of replanning needed by the application because there is overhead in adding and maintaining the required plan-space annotations.

I believe that D* Lite can be proven to retain optimality in all cases, which is not easily accomplished by a simple caching scheme. The big issue is cache invalidation for which you need to do some form of what we in [1] described as relevance regression: you need to use the same transition semantics used by the planner to propagate the changes and determine for which states in the search space the decision boundary has moved (e.g., turn left instead of right) because of that change. This is exactly what D* Lite does for path planning. It is very similar, btw, to back-propagation in NNs, but not limited to numerical errors, and the "architecture" -- i.e., the graph you propagate the changes over -- is dynamically spun by the planner depending on the situation and goal, not hard-coded.

PS: That said, the current caching approach may very well be good enough in practice.

[SteveMacenski]

Ah - thanks for the clarification :-) Much of that commentary was from my readings some years ago that are clearly a bit fuzzy in memory.

[ibaranov-cp]

I kicked up this discussion mostly for the dynamics use case, that'd be our need for it, particularly planning at fairly high frequency (IE 100Hz or so) at higher speeds, where we have to react suddenly to new information. So not so much raw compute constrained, as we are compute/time constrained

[armgits]

Sounds interesting! If there's currently a need for someone to implement this planner, I’m happy to help.

I’m leaning towards a standalone planner plugin. Based on the discussion, could the Smac planner serve as a reference implementation to look at for getting started?

[SteveMacenski]

I think a good starting point would be to put together a relatively simple version of this that works well as a basic prototype. Then from that learning, look over things like Theta* and Smac Planner and see if there's a path forward to enabling a "D*-mode" so that the feasible planners in Smac can benefit from the rapid replanning rather than making an independent D* planner that would still only be good for holonomic robots. Using the nonholonomic search neighborhoods of Smac Planner would allow for a broader set of platforms to benefit from that performance improvement.

Push come to shove, if it needs to be an independent plugin, then we can refine that implementation into a production-grade performance planner.