Torque compensation as function of position reference, feedback of speed and position