Added Kff and Kbemf terms in pid torque control.