Near optimal control with reachability and safety guarantees