RTDE register round-trip example

This example shows how to write several Real-Time Data Exchange (RTDE) inputs to the robot in a single package, at the robot’s maximum frequency, and how to prove that the robot processed them.

The send...() helpers on RTDEWriter update individual inputs and notify the asynchronous writer separately. When several general purpose registers have to change together, sendPackage() submits them together in one pending-buffer update, avoiding a transmission between separate helper calls. It does not guarantee a separate transmission for every call.

The example’s source code can be found in rtde_roundtrip.cpp.

Note

PolyScope 5 and PolyScope X robots have to be powered on and in remote control mode for the register-processing program to be accepted.

Recipes as argument lists

RTDEClient takes the input and output recipes as two lists of field names. Recipe files work as well; see RTDE Client example. timestamp is part of the output recipe either way, because the client adds it if it is missing.

The general purpose register ranges reserved for external RTDE clients are bit registers 64..127 and integer and double registers 24..47.

Listing 34 examples/rtde_roundtrip.cpp

// We write the inputs, the robot program below writes the outputs.
const std::string INPUT_BIT_REGISTER = "input_bit_register_64";
const std::string INPUT_INT_REGISTER = "input_int_register_24";
const std::string INPUT_DOUBLE_REGISTER = "input_double_register_24";
const std::string OUTPUT_BIT_REGISTER = "output_bit_register_64";
const std::string OUTPUT_INT_REGISTER = "output_int_register_24";
const std::string OUTPUT_DOUBLE_REGISTER = "output_double_register_24";

// RTDE recipes as argument lists, so this example needs no recipe files. RTDEClient also takes two
// filenames instead; see examples/rtde_client.cpp. The general purpose register ranges used here
// are the ones the RTDE guide reserves for external clients: bit registers 64..127, integer and
// double registers 24..47. Register fields need no companion "_mask" key.
const std::vector<std::string> INPUT_RECIPE = { INPUT_BIT_REGISTER, INPUT_INT_REGISTER, INPUT_DOUBLE_REGISTER };
const std::vector<std::string> OUTPUT_RECIPE = { "timestamp", "runtime_state", OUTPUT_BIT_REGISTER, OUTPUT_INT_REGISTER,
                                                 OUTPUT_DOUBLE_REGISTER };

Note

Register fields, unlike the digital and analog outputs and the speed slider, need no companion _mask key in the input recipe.

Processing the registers on the robot

Input registers cannot be written from URScript, and output registers cannot be written through RTDE. Getting values back therefore requires a program on the robot.

The program does not copy the values. RTDE also exposes the input registers as outputs, so a plain echo would be indistinguishable from that read-back. Instead the program inverts the bit, adds one to the integer and negates the double. A value that satisfies those relations can only have been produced by this program. sync() runs the loop once per control cycle.

sendScript() is used rather than sendScriptBlocking(), because the latter would wait until the program stops, and this one loops forever.

Listing 35 examples/rtde_roundtrip.cpp

const std::string MIRROR_PROGRAM = R"(def rtde_register_mirror():
  while (True):
    write_output_boolean_register(64, not read_input_boolean_register(64))
    write_output_integer_register(24, read_input_integer_register(24) + 1)
    write_output_float_register(24, -1.0 * read_input_float_register(24))
    sync()
  end
end)";

Listing 36 examples/rtde_roundtrip.cpp

  // Start the robot program that processes the registers
  primary_interface::PrimaryClient primary_client(robot_ip, notifier);
  primary_client.start();
  try
  {
    primary_client.commandBrakeRelease();
  }
  catch (const UrException& e)
  {
    URCL_LOG_WARN("Could not release the brakes: %s", e.what());
  }
  if (!primary_client.sendScript(MIRROR_PROGRAM))
  {
    URCL_LOG_WARN("Could not upload the register-processing program. Output registers will stay at "
                  "zero until a matching program is running on the robot.");
  }
  // The program keeps running until we stop it later.

An input package with the robot’s field types

The data types of the input recipe belong to the robot and arrive with the handshake, so the package has to be created after init(). createInputDataPackage() returns a zeroed package that already carries those types: setData() then rejects a wrong type immediately, and copying the package into the send buffer is a single memcpy.

A package constructed from getInputRecipe() still works. Its types are taken from the values written to it and are only checked when the package is sent.

Listing 37 examples/rtde_roundtrip.cpp

  // RTDE client at the robot's maximum frequency
  rtde_interface::RTDEClient my_client(robot_ip, notifier, OUTPUT_RECIPE, INPUT_RECIPE);
  my_client.init();
  URCL_LOG_INFO("RTDE target frequency: %f Hz", my_client.getTargetFrequency());

  // An input package carrying the data types the robot reported for the input recipe. Those types
  // are only known once the RTDE handshake has run, which is why this is created after init().
  rtde_interface::DataPackage input_pkg = my_client.createInputDataPackage();
  // The output package is still untyped; the first read applies the robot's types to it in place.
  auto output_pkg = std::make_unique<rtde_interface::DataPackage>(my_client.getOutputRecipe());

  my_client.start(false);

target_frequency = 0.0 (the default) requests the robot’s maximum: 125 Hz on CB3, 500 Hz on PolyScope 5 and PolyScope X robots. See Setup for real-time scheduling and RTDEClient.

Both DataPackage objects are allocated before the loop, so the normal RTDE data receive and submission paths reuse their storage without allocation. This does not extend to logging, error handling or reconnection. The output package is built from getOutputRecipe() and is therefore still untyped; the first read applies the robot’s types to it in place, without allocation.

Letting the robot pace the loop

start(false) leaves the background read thread off. getDataPackageBlocking() returns once per RTDE cycle and is this loop’s time base. The input package is produced immediately after the read so it reaches the robot in time to be acted on in the next cycle. Printing is throttled to about once per second, so it stays out of the hot path.

Listing 38 examples/rtde_roundtrip.cpp

    // The blocking read is this loop's clock
    if (!my_client.getDataPackageBlocking(output_pkg))
    {
      // Ctrl-C interrupts the blocking read, which is a normal stop rather than an error.
      if (!running)
      {
        break;
      }
      URCL_LOG_ERROR("Could not get a fresh data package from the robot.");

Writing several inputs in one package

The newly created input package contains zeros. Reusing it preserves previously written values unless they are changed or reset. sendPackage() copies all fields into the pending buffer and notifies the writer thread without waiting for transmission. Multiple calls before the writer consumes that buffer can be coalesced, with a later package replacing an earlier pending one. Separate send...() calls may be transmitted separately or coalesced; they are not an atomic update of several fields. A successful return means the buffer update was accepted, not that the robot received or processed it.

Without a run duration the loop runs until Ctrl-C is pressed, so the counter wraps at one million rather than growing past what an integer register can hold. Every answer carries the counter value it belongs to, so verification is unaffected, and the lag is measured modulo the same period.

Listing 39 examples/rtde_roundtrip.cpp

    // Writing several general purpose inputs in one package
    ++cycles;
    counter = counter % COUNTER_WRAP + 1;
    const bool sent_bit = (counter % 2) == 0;
    const double sent_double = std::sin(counter * SINE_INCREMENT);
    bool write_ok = input_pkg.setData(INPUT_BIT_REGISTER, sent_bit);
    write_ok = write_ok && input_pkg.setData(INPUT_INT_REGISTER, counter);
    write_ok = write_ok && input_pkg.setData(INPUT_DOUBLE_REGISTER, sent_double);
    if (!write_ok || !my_client.getWriter().sendPackage(input_pkg))
    {
      URCL_LOG_ERROR("Sending RTDE data failed.");

Verifying that the robot processed the data

All three values sent in a cycle are derived from the cycle counter, so the integer the robot returns identifies which cycle an answer belongs to. echoed_int - 1 is that counter. The expected bit is its inversion and the expected double is the negated sine. The robot’s double register is a 64-bit value, so the negated sine comes back bit for bit and is compared exactly. Together with the inverted bit, that is what makes an answer attributable to this program rather than to RTDE’s own read-back of the input registers.

Against URSim the lag is one cycle: the values written after the read of cycle N are processed by the robot and observed in the read of cycle N+1. getData() needs a variable of the field’s own type; getDataType() reports that type if the recipe is not known in advance.

Listing 40 examples/rtde_roundtrip.cpp

    // Reading what the robot made of the previous package
    bool echoed_bit = false;
    int32_t echoed_int = 0;
    double echoed_double = 0.0;
    uint32_t runtime_state = 0;
    if (!output_pkg->getData(OUTPUT_BIT_REGISTER, echoed_bit) ||
        !output_pkg->getData(OUTPUT_INT_REGISTER, echoed_int) ||
        !output_pkg->getData(OUTPUT_DOUBLE_REGISTER, echoed_double) ||
        !output_pkg->getData("runtime_state", runtime_state))
    {
      URCL_LOG_ERROR("Could not read the output registers from the received package.");
      exit_code = 1;
      break;
    }

    bool verified_this_cycle = false;
    if (echoed_int > 1)
    {
      const int32_t origin = echoed_int - 1;  // the counter value the robot processed
      const bool expected_bit = !((origin % 2) == 0);
      const double expected_double = -std::sin(origin * SINE_INCREMENT);
      // Taken modulo the wrap period, so a lag measured across a wrap is still a small number.
      last_lag_cycles = (counter - origin + COUNTER_WRAP) % COUNTER_WRAP;
      // The robot's double register is a 64-bit value, so the negated sine has to come back bit
      // for bit. Together with the inverted bit that is the proof the robot processed this cycle.
      if (echoed_bit == expected_bit && echoed_double == expected_double)
      {
        ++verified;
        verified_this_cycle = true;
      }
      else
      {
        ++mismatches;

Cleanup

The input registers are reset and the robot program is stopped. A failed stop is only logged, because CI runs the example for one second and still requires exit code 0.

The loop ends when the run duration has passed, when an RTDE read or write fails, or when Ctrl-C is pressed. A SIGINT handler only clears running, so every one of these paths leaves the loop and runs cleanup(), and the program does not keep running on the robot. Ctrl-C can interrupt the blocking read, so a failed read after Ctrl-C ends the example normally. Resetting the registers may then fail, but the robot program is still stopped.

Listing 41 examples/rtde_roundtrip.cpp

// Cleared on Ctrl-C, so the main loop ends and the cleanup below still runs.
volatile std::sig_atomic_t running = 1;

void signalHandler(int /*signum*/)
{
  running = 0;
}

// Reset the input registers and stop the robot program before leaving
void cleanup(rtde_interface::RTDEClient& rtde_client, rtde_interface::DataPackage& input_pkg,
             primary_interface::PrimaryClient& primary_client)
{
  input_pkg.setData(INPUT_BIT_REGISTER, false);
  input_pkg.setData(INPUT_INT_REGISTER, static_cast<int32_t>(0));
  input_pkg.setData(INPUT_DOUBLE_REGISTER, 0.0);
  rtde_client.getWriter().sendPackage(input_pkg);
  std::this_thread::sleep_for(std::chrono::milliseconds(100));
  try
  {
    primary_client.commandStop(false);
  }
  catch (const UrException& e)
  {
    URCL_LOG_WARN("Could not stop the robot program: %s", e.what());
  }
}

Example output

The following shows a run against URSim 5.25.1 asking for 500 Hz. The echoed integer trails the sent integer by one cycle, and verified=1 means the bit and the double match the transformations the robot program applies to that cycle.

[INFO] RTDE target frequency: 500.000000 Hz
sent: bit=1 int=484 double=-0.991869 | robot: bit=0 int=483 double=0.994216 | verified=1 lag_cycles=1 freq=483.063 Hz playing=1
sent: bit=0 int=967 double=-0.242772 | robot: bit=1 int=966 double=0.223323 | verified=1 lag_cycles=1 freq=482.826 Hz playing=1
sent: bit=1 int=1450 double=0.934895 | robot: bit=0 int=1449 double=-0.941806 | verified=1 lag_cycles=1 freq=482.669 Hz playing=1
[INFO] Cycles: 1931, average frequency: 482.628400 Hz, verified: 1929, mismatches: 0, last lag: 1 cycles

A simulator shares the host’s CPU, so the measured frequency stays somewhat below the requested one; on a real controller it tracks the target closely.