mpc_dmp: first working version of time-scheduled constraint mpc for obstacle avoidance