replanning with trajectory visualization works (needs refactoring)