Update OptimalMotionPlanner.h

case typo
Update OptimalMotionPlanner.cpp
Merge pull request #1 from jtritell/patch-1

Patch 1