MPC solver Arm motion