mpc_dmp: split off the optimization formulation in a separate function