Optimal control of systems with constraints