Forage RRT — An efficient approach to task-space goal planning for high dimensional systems

Achieving efficient end-effector planning for manipulators in real world workspaces is challenging due to the fact that planning is performed in manipulator joint space, while the planning goal is given in end-effector or tool space. For manipulator planning, the problem becomes a joint path planning and inverse kinematics problem to be resolved efficiently, in spite of the potentially infinite number of inverse solutions to the end-effector goal state and the nonlinear relationship between joint configurations and world obstacles. The Forage-RRT algorithm described in this paper attempts to efficiently and quickly resolve the end-effector or tool planning problem. Using ideas from foraging theory, Forage-RRT implements a diffusion-based search strategy with two rates of diffusion, one high and one low, which simulate both long jumps (coarse search) and focused small area exploration (fine search) in the joint space, respectively. During coarse search, it is important to keep track of past fine searches, therefore the traditional RRT algorithm is augmented with a goal heap that keeps track of potential focused search regions and discards them when they result in failure. By mixing between two search distributions with different spreads, the search space is rapidly covered and potentially fruitful avenues are finely explored. The search coverage advantages of foraging identified in the biological research literature are demonstrated here for end-effector based, manipulator path planning.