Optimization based control of nonlinear constrained systems