FMTX: Lazy Wavefront Search for Dynamic Replanning

FMT* plans efficiently in static worlds by expanding a cost-ordered wavefront and collision-checking lazily, but its single-pass unvisited rule cannot revise paths when obstacles change. We present FMTX, an anytime, asymptotically optimal generalization of that wavefront for dynamic replanning. A cost-improvement test replaces the unvisited set, allowing a node to be revisited for best-parent selection whenever a lower-cost potential connection is found. This induces implicit rewiring within the wavefront while preserving lazy collision checking. FMTX retains the online densification of RRT* but defers its eager neighborhood rewiring to the cost-ordered wavefront, so a node is revisited only when the expansion reaches it. Obstacle updates orphan the affected subtree and reseed the wavefront. A direct cost push from each parent to its children propagates cost improvements through validated tree edges, thereby preserving descendant cost consistency, a property not guaranteed by implicit rewiring alone. We compare a fixed-graph batch variant, Dynamic FMT* (D-FMT*), with D* Lite on identical PRM* graphs, and FMTX with eager RRTX and path-centric LLPT* in geometric and kinodynamic scenes, including partial observability. D-FMT* stays close on path quality at far fewer collision checks. FMTX lies between LLPT* and RRTX on repair effort while tracking the reliability and trajectory quality of eager RRTX. The study shows that collision-checking policy affects median repair latency, repair-time tails, and executed trajectory quality.