Rapid Replanning in Consecutive Pick-and-Place Tasks with Lazy Experience Graph

Abstract

In an environment where a manipulator needs to execute multiple consecutive tasks, the act of object manoeuvre will change the underlying configuration space, affecting all subsequent tasks. Previously free configurations might now be occupied by the manoeuvred objects, and previously occupied space might now open up new paths. We propose Lazy Tree-based Replanner (LTR*) -- a novel hybrid planner that inherits the rapid planning nature of existing anytime incremental sampling-based planners. At the same time, it allows subsequent tasks to leverage prior experience via a lazy experience graph. Previous experience is summarised in a lazy graph structure, and LTR* is formulated to be robust and beneficial regardless of the extent of changes in the workspace. Our hybrid approach attains a faster speed in obtaining an initial solution than existing roadmap-based planners and often with a lower cost in trajectory length. Subsequent tasks can utilise the lazy experience graph to speed up finding a solution and take advantage of the optimised graph to minimise the cost objective. We provide proofs of probabilistic completeness and almost-surely asymptotic optimal guarantees. Experimentally, we show that in repeated pick-and-place tasks, LTR* attains a high gain in performance when planning for subsequent tasks.