get_servo_torque implemented