Fixed bug for torques control mode