mpc_dmp: first version of obstacle avoidance mpc seems to be working