Update OptimalMotionPlanner.cpp