Anytime Whole-Body Planning/Replanning for Humanoid Robots

In this paper we propose an anytime planning/replanning algorithm aimed at generating motions allowing a humanoid to fulfill an assigned task that implicitly requires stepping. The algorithm interleaves planning and execution intervals: a previously planned whole-body motion is executed while simultaneously planning a new solution for the subsequent execution interval. At each planning interval, a specifically designed randomized local planner builds a tree in configuration-time space by concatenating successions of CoM movement primitives. Such a planner works in two stages. A first lazy stage quickly expands the tree, testing only vertexes for collisions; then, a second validation stage searches the tree for feasible, collision-free whole-body motions realizing a solution to be executed during the next planning interval. We discuss how the proposed planner can avoid deadlock and we propose how it can be extended to a sensor-based planner. The proposed method has been implemented in V-REP for the NAO humanoid and successfully tested in various scenarios of increasing complexity.