Epfl Model Predictive Control Algorithm