MPC_robot_agnostic