Nonprehensile whole arm rearrangement planning on physics manifolds

We present a randomized kinodynamic planner that solves rearrangement planning problems. We embed a physics model into the planner to allow reasoning about interaction with objects in the environment. By carefully selecting this model, we are able to reduce our state and action space, gaining tractability in the search. The result is a planner capable of generating trajectories for full arm manipulation and simultaneous object interaction. We demonstrate the ability to solve more rearrangement by pushing tasks than existing primitive based solutions. Finally, we show the plans we generate are feasible for execution on a real robot.