Switched ROBOT_JOINTS_WBITOOLBOX to ROBOT_TORQUE_CONTROL_JOINTS