refactor motion planner out
more refactoring cleanup