Improved ompl planner