Streaming API for trajectory forwarding (#528)

This enables streaming trajectory segments using the FORWARD communication protocol
without knowing the number of segments ahead of time.

It includes:
- The addition of the `STREAM_START` and `STREAM_END` control messages
to the `TrajectoryControlMesssage` enumeration.
- An example using the new API
- Documentation to cover all the changes

Author: acmorrow

doc/architecture/reverse_interface.rst

@@ -49,8 +49,8 @@ meaning:
            - joint velocities (SPEEDJ)
            - trajectory instructions (FORWARD)
 
-             - field 1: Trajectory control mode(1: TRAJECTORY_MODE_RECEIVE, -1: TRAJECTORY_MODE_CANCEL)
-             - field 2: Number of trajectory points left to transfer
+             - field 1: Trajectory control mode (1: TRAJECTORY_MODE_RECEIVE, 2: TRAJECTORY_MODE_STREAM_START, 3: TRAJECTORY_MODE_STREAM_END, -1: TRAJECTORY_MODE_CANCEL). See :ref:`streaming_trajectories` for the streaming modes.
+             - field 2: Trajectory point count. Its interpretation depends on the control mode in field 1.
 
            - Cartesian velocities (SPEEDL)
            - Cartesian pose (POSE)
@@ -88,6 +88,65 @@ meaning:
 
 Depending on the control mode one can use the ``write()`` (SERVOJ, SPEEDJ, SPEEDL, POSE, TORQUE), ``writeTrajectoryControlMessage()`` (FORWARD) or ``writeFreedriveControlMessage()`` (FREEDRIVE) function to write a message to the "reverse_socket".
 
+.. _streaming_trajectories:
+
+Streaming trajectories
+~~~~~~~~~~~~~~~~~~~~~~~~
+
+A ``FORWARD``-mode trajectory can be executed in one of two ways.
+
+A *finite* trajectory declares its total length up front:
+``writeTrajectoryControlMessage(TRAJECTORY_START, n)`` promises exactly ``n`` points, and execution
+completes once they have been consumed. This is the mode used by
+:ref:`trajecotry_joint_interface_example`.
+
+A *streaming* trajectory is open-ended. The producer does not commit to a point count in advance; it
+opens the stream, writes points for as long as it likes, and signals end-of-stream explicitly. This
+suits points produced on the fly -- from a planner or a teleoperation source -- where the total length
+is unknown when motion begins. The message sequence is:
+
+.. code-block:: c++
+
+   writeTrajectoryControlMessage(TRAJECTORY_STREAM_START)   // open the stream
+   // write a motion primitive, any number of times:
+   // writeTrajectoryPoint(...) / writeTrajectorySplinePoint(...) / writeMotionPrimitive(...)
+   writeTrajectoryControlMessage(TRAJECTORY_STREAM_END, n)  // close the stream
+
+``TRAJECTORY_STREAM_START`` opens the trajectory; its point-count argument is unused. The producer then
+writes motion primitives -- via ``writeTrajectoryPoint()``, ``writeTrajectorySplinePoint()`` or
+``writeMotionPrimitive()`` -- for as long as it likes. ``TRAJECTORY_STREAM_END`` closes it.
+
+.. important::
+   The ``n`` passed to ``TRAJECTORY_STREAM_END`` must equal the total number of motion primitives the
+   producer wrote since ``TRAJECTORY_STREAM_START``. The controller consumes primitives asynchronously
+   as it executes them and cannot otherwise tell how many are still outstanding; it uses ``n`` to
+   account for those still in flight before completing. Aborting a stream with ``TRAJECTORY_CANCEL``
+   requires the same count, for the same reason. (For a finite ``TRAJECTORY_START`` trajectory the
+   cancel argument is unused.)
+
+Two obligations fall on the producer:
+
+- **Do not starve the controller.** The robot consumes the stream as it executes it, so the producer
+  must keep work available: the next primitive must arrive before the robot finishes the ones it
+  already holds. The budget for delivering it is simply the duration of the work in hand -- a primitive
+  that runs for five minutes gives five minutes to produce its successor, while a stream of
+  eight-millisecond primitives demands one every eight milliseconds. If the robot runs out of submitted
+  work before the next primitive arrives and before ``TRAJECTORY_STREAM_END`` is sent, the trajectory
+  ends with ``TRAJECTORY_RESULT_FAILURE``. Delivering primitives as they are produced, without holding
+  them back, is the natural way to satisfy this.
+
+- **End at rest.** Streaming adds no wind-down deceleration of its own. Make the final point a
+  controlled stop, with zero velocity and acceleration, so the spline interpolates smoothly to rest at
+  the goal. The trajectory thread issues ``stopj`` on exit regardless, so a non-zero terminal point
+  still stops the robot, but the transition will be less smooth.
+
+On completion the callback registered with ``setTrajectoryEndCallback()`` reports
+``TRAJECTORY_RESULT_SUCCESS`` for a clean end (``TRAJECTORY_STREAM_END`` processed, all points
+consumed), ``TRAJECTORY_RESULT_FAILURE`` for a producer underrun, or ``TRAJECTORY_RESULT_CANCELED`` if
+the stream was aborted with ``TRAJECTORY_CANCEL``.
+
+See :ref:`trajectory_streaming_example` for a complete, working example.
+
 .. _direct_torque_control_mode:
 
 Direct torque control mode

doc/examples.rst

@@ -31,4 +31,5 @@ may be running forever until manually stopped.
    examples/tool_contact_example
    examples/direct_torque_control
    examples/trajectory_point_interface
+   examples/trajectory_streaming
    examples/ur_driver

doc/examples/trajectory_streaming.rst

@@ -0,0 +1,121 @@
+:github_url: https://github.com/UniversalRobots/Universal_Robots_Client_Library/blob/master/doc/examples/trajectory_streaming.rst
+
+.. _trajectory_streaming_example:
+
+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 :ref:`trajecotry_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 :ref:`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:
+
+.. literalinclude:: ../../examples/trajectory_streaming.cpp
+   :language: c++
+   :caption: examples/trajectory_streaming.cpp
+   :linenos:
+   :lineno-match:
+   :start-at: const bool headless_mode = true;
+   :end-before: // --------------- PRE-POSITION VIA FINITE TRAJECTORY
+
+Trajectory execution is asynchronous, so completion is reported through a callback. The result
+distinguishes a successful run from a cancellation or a failure:
+
+.. literalinclude:: ../../examples/trajectory_streaming.cpp
+   :language: c++
+   :caption: examples/trajectory_streaming.cpp
+   :linenos:
+   :lineno-match:
+   :start-at: void trajDoneCallback
+   :end-at: }
+
+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:
+
+.. literalinclude:: ../../examples/trajectory_streaming.cpp
+   :language: c++
+   :caption: examples/trajectory_streaming.cpp
+   :linenos:
+   :lineno-match:
+   :start-at: // Pump TRAJECTORY_NOOP
+   :end-before: // Sample a quintic-Hermite
+
+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:
+
+.. literalinclude:: ../../examples/trajectory_streaming.cpp
+   :language: c++
+   :caption: examples/trajectory_streaming.cpp
+   :linenos:
+   :lineno-match:
+   :start-at: // Sample a quintic-Hermite
+   :end-before: int main(
+
+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:
+
+.. literalinclude:: ../../examples/trajectory_streaming.cpp
+   :language: c++
+   :caption: examples/trajectory_streaming.cpp
+   :linenos:
+   :lineno-match:
+   :start-after: // --------------- PRE-POSITION VIA FINITE TRAJECTORY
+   :end-before: // ----------------- STREAMING TRAJECTORY DEMO
+
+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:
+
+.. literalinclude:: ../../examples/trajectory_streaming.cpp
+   :language: c++
+   :caption: examples/trajectory_streaming.cpp
+   :linenos:
+   :lineno-match:
+   :start-after: // ----------------- STREAMING TRAJECTORY DEMO
+   :end-before: URCL_LOG_INFO("Streaming %d points
+
+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:
+
+.. literalinclude:: ../../examples/trajectory_streaming.cpp
+   :language: c++
+   :caption: examples/trajectory_streaming.cpp
+   :linenos:
+   :lineno-match:
+   :start-at: URCL_LOG_INFO("Streaming %d points
+   :end-before: g_my_robot->getUrDriver()->stopControl();
+
+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 :ref:`streaming_trajectories`.

examples/CMakeLists.txt

@@ -59,6 +59,10 @@ add_executable(trajectory_point_interface_example
 trajectory_point_interface.cpp)
 target_link_libraries(trajectory_point_interface_example ur_client_library::urcl)
 
+add_executable(trajectory_streaming_example
+trajectory_streaming.cpp)
+target_link_libraries(trajectory_streaming_example ur_client_library::urcl)
+
 add_executable(instruction_executor
 instruction_executor.cpp)
 target_link_libraries(instruction_executor ur_client_library::urcl)