Optimal Control For Safety-Critical Systems