Modified code to work with upgraded OMPL-0.14.0