Simple drake motion planning on PR2 robot