Ffw of position and speed error to compensate torque