Changed to use more difficult ompl interface