I am trying to use torque control on the MX series motors. Here is my code:

#include <ax12.h>

const int servo = 2;
void setup() {
dxlInit(1000000);
mxSetTorqueControlMode(servo, 1);
delay(33);
Serial.begin(115200);
mxSetGoalTorque(servo, 500);
}

void loop() {

delay(33);
Serial.println(mxGetGoalTorque(servo));
}

The output is always 0. I have latest firmware on the dynamixel and I'm using the latest ax12.h library. Is there something I'm missing? I have tried to change the goal torque using roboplus and that works fine. It only doesn't work when using the arduino IDE with the arbotix.