ompl branch containing a multiquery planner with dynamic cost map