Changed thread to control robot in torque mode