debugged motion_planner