CasADi: a software framework for nonlinear optimization and optimal control