Cleaned up position planner