plan from current robot pose