Trajectory streaming example

This example demonstrates open-ended trajectory streaming: a producer sends points to the controller without declaring a total point count up front, then signals end-of-stream when it is finished. Contrast this with the Trajectory Joint Interface example, which uses the finite trajectory path where the point count is fixed in the initial TRAJECTORY_START message.

The streaming communication contract – the STREAM_START / STREAM_END handshake, the meaning of the end-of-stream point count, the producer’s obligation not to starve the controller, and the trajectory result codes – is described in Streaming trajectories. This page walks through the example code and refers back to that section for the authoritative contract.

Setup

As with the other examples, we create a UrDriver (here through ExampleRobotWrapper) and register a callback that fires when a trajectory finishes:

Listing 74 examples/trajectory_streaming.cpp

  const bool headless_mode = true;
  g_my_robot = std::make_unique<urcl::ExampleRobotWrapper>(robot_ip, OUTPUT_RECIPE, INPUT_RECIPE, headless_mode,
                                                           "external_control.urp");
  if (!g_my_robot->isHealthy())
  {
    URCL_LOG_ERROR("Robot initialization failed. See preceding output for details.");
    return 1;
  }

  g_my_robot->getUrDriver()->registerTrajectoryDoneCallback(&trajDoneCallback);

Trajectory execution is asynchronous, so completion is reported through a callback. The result distinguishes a successful run from a cancellation or a failure:

Listing 75 examples/trajectory_streaming.cpp

void trajDoneCallback(const urcl::control::TrajectoryResult& result)
{
  g_trajectory_result = result;
  g_trajectory_done = true;
  URCL_LOG_INFO("Trajectory done with result %s", urcl::control::trajectoryResultToString(result).c_str());
}

While a trajectory runs, the control PC must keep telling the robot program that the connection is still alive. We do this by sending TRAJECTORY_NOOP messages while we wait; without them the robot program stops waiting for input and the external_control program exits:

Listing 76 examples/trajectory_streaming.cpp

// Pump TRAJECTORY_NOOP on the reverse socket while waiting for the
// trajectory-done callback. Without this the URScript dispatcher's
// reverse_socket read times out, which terminates the external_control
// program entirely.
void waitForTrajectoryDoneWithKeepalives()
{
  while (!g_trajectory_done)
  {
    std::this_thread::sleep_for(std::chrono::milliseconds(100));
    g_my_robot->getUrDriver()->writeTrajectoryControlMessage(urcl::control::TrajectoryControlMessage::TRAJECTORY_NOOP);
  }
}

Generating a motion

To have something to stream, we sample a quintic-Hermite blend between two joint configurations. The blend has zero velocity and acceleration at both endpoints, which – as the streaming contract recommends – gives the final point the controlled-stop property needed to bring the robot cleanly to rest:

Listing 77 examples/trajectory_streaming.cpp

// Sample a quintic-Hermite interpolation between q_start and q_end with zero
// boundary velocity and acceleration, evaluated at fractional time s in [0, 1]
// for a segment of total duration t_total:
//
//   q(s)   = q_start + (q_end - q_start) * (10s^3 - 15s^4 + 6s^5)
//   qd(t)  = (q_end - q_start) * (30s^2 - 60s^3 + 30s^4) / t_total
//   qdd(t) = (q_end - q_start) * (60s  - 180s^2 + 120s^3) / t_total^2
struct SampledPoint
{
  urcl::vector6d_t q;
  urcl::vector6d_t qd;
  urcl::vector6d_t qdd;
};

SampledPoint sampleQuinticHermite(const urcl::vector6d_t& q_start, const urcl::vector6d_t& q_end, const double s,
                                  const double t_total)
{
  const double s2 = s * s;
  const double s3 = s2 * s;
  const double s4 = s3 * s;
  const double s5 = s4 * s;
  const double pos_blend = 10.0 * s3 - 15.0 * s4 + 6.0 * s5;
  const double vel_blend = (30.0 * s2 - 60.0 * s3 + 30.0 * s4) / t_total;
  const double acc_blend = (60.0 * s - 180.0 * s2 + 120.0 * s3) / (t_total * t_total);

  SampledPoint out{};
  for (size_t i = 0; i < q_start.size(); ++i)
  {
    const double delta = q_end[i] - q_start[i];
    out.q[i] = q_start[i] + delta * pos_blend;
    out.qd[i] = delta * vel_blend;
    out.qdd[i] = delta * acc_blend;
  }
  return out;
}

Pre-positioning

Before streaming, we park the robot at a known start pose using an ordinary finite trajectory. This doubles as a demonstration of the TRAJECTORY_START path for comparison:

Listing 78 examples/trajectory_streaming.cpp

  // Park the robot at a known starting pose before the streaming demo. This
  // also exercises the finite TRAJECTORY_START path for comparison.

  const urcl::vector6d_t pose_a = { -1.57, -1.57, 0.0, -1.57, 0.0, 0.0 };
  const urcl::vector6d_t pose_b = { -1.57, -1.20, 0.5, -1.57, 0.0, 0.0 };
  const urcl::vector6d_t zero = { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 };

  URCL_LOG_INFO("Pre-positioning to pose A via finite trajectory");
  g_trajectory_done = false;
  g_my_robot->getUrDriver()->writeTrajectoryControlMessage(urcl::control::TrajectoryControlMessage::TRAJECTORY_START,
                                                           1);
  g_my_robot->getUrDriver()->writeTrajectorySplinePoint(pose_a, zero, zero, 3.0f);
  waitForTrajectoryDoneWithKeepalives();
  if (g_trajectory_result != urcl::control::TrajectoryResult::TRAJECTORY_RESULT_SUCCESS)
  {
    URCL_LOG_ERROR("Pre-positioning failed");
    return 1;
  }

Streaming the trajectory

We precompute all of the points up front. The final point inherits the qd = 0, qdd = 0 boundary condition of the quintic blend, so it is a compliant controlled-stop terminal:

Listing 79 examples/trajectory_streaming.cpp

  // Stream a quintic-Hermite motion from pose A to pose B. All points are
  // precomputed and written back-to-back; the robot works through them at its
  // own pace. See doc/examples/trajectory_streaming.rst for details.

  const int k_num_points = 200;
  const double k_motion_duration_s = 2.0;
  const double dt = k_motion_duration_s / k_num_points;

  std::vector<SampledPoint> points;
  points.reserve(k_num_points);
  for (int i = 1; i <= k_num_points; ++i)
  {
    const double s = static_cast<double>(i) / k_num_points;
    points.push_back(sampleQuinticHermite(pose_a, pose_b, s, k_motion_duration_s));
  }
  // The final quintic-Hermite point has qd=0, qdd=0: the controlled-stop
  // terminal the streaming contract requires.

The stream itself is the STREAM_START -> points -> STREAM_END handshake. We open the stream, write every point back-to-back, then close it, passing the total number of points written so the controller knows how many are still outstanding before it reports completion:

Listing 80 examples/trajectory_streaming.cpp

  URCL_LOG_INFO("Streaming %d points over %.2f s nominal motion duration", k_num_points, k_motion_duration_s);
  g_trajectory_done = false;
  g_trajectory_result = urcl::control::TrajectoryResult::TRAJECTORY_RESULT_UNKNOWN;

  const auto stream_start_wall = std::chrono::steady_clock::now();
  g_my_robot->getUrDriver()->writeTrajectoryControlMessage(
      urcl::control::TrajectoryControlMessage::TRAJECTORY_STREAM_START);
  for (const auto& p : points)
  {
    if (!g_my_robot->getUrDriver()->writeTrajectorySplinePoint(p.q, p.qd, p.qdd, static_cast<float>(dt)))
    {
      URCL_LOG_ERROR("writeTrajectorySplinePoint failed mid-stream");
      return 1;
    }
  }
  const auto stream_end_send_wall = std::chrono::steady_clock::now();
  g_my_robot->getUrDriver()->writeTrajectoryControlMessage(
      urcl::control::TrajectoryControlMessage::TRAJECTORY_STREAM_END, k_num_points);

  waitForTrajectoryDoneWithKeepalives();
  const auto callback_wall = std::chrono::steady_clock::now();

  const auto send_duration =
      std::chrono::duration_cast<std::chrono::milliseconds>(stream_end_send_wall - stream_start_wall);
  const auto end_to_callback =
      std::chrono::duration_cast<std::chrono::milliseconds>(callback_wall - stream_end_send_wall);
  URCL_LOG_INFO("All %d points written in %lld ms wall-clock", k_num_points,
                static_cast<long long>(send_duration.count()));
  // Time from STREAM_END to the done callback. Here it is dominated by the
  // consumer draining the batched backlog, not by STREAM_END processing; a
  // steady-state producer would see roughly one controller step plus the
  // stopj at trajectory thread exit. See doc/examples/trajectory_streaming.rst.
  URCL_LOG_INFO("Time from sending STREAM_END to trajectory-done callback: %lld ms",
                static_cast<long long>(end_to_callback.count()));
  URCL_LOG_INFO("Final result: %s", urcl::control::trajectoryResultToString(g_trajectory_result).c_str());

  if (g_trajectory_result != urcl::control::TrajectoryResult::TRAJECTORY_RESULT_SUCCESS)
  {
    URCL_LOG_ERROR("Streaming trajectory did not complete successfully");
    return 1;
  }

Because this example delivers the whole motion up front, it can never starve the controller: all the work is in hand before execution finishes, so the robot simply plays through it. The example also measures the time from sending STREAM_END to the trajectory-done callback. That time is large here, but not because ending a stream is costly: the producer sends every point and then STREAM_END within milliseconds, long before the robot has finished moving, so the callback cannot fire until the robot has worked through the entire backlog. A producer that instead fed points in step with the robot’s motion would send STREAM_END just as the robot reached the final point, and the callback would follow almost immediately.

For the general case, where points are produced on the fly and pacing does matter, see the producer obligations in Streaming trajectories.