Full body planner for the PR2