Improve cleanup of trajectory buffer (UniversalRobots#466)

**Summary**

When cancelling a trajectory (e.g. a spline trajectory) while it is
still being executed, the `external_control.urscript` currently attempts
to clear the entire trajectory buffer within a single control cycle.
If the trajectory buffer contains a large number of points (≈200 or
more), this cleanup can exceed the real‑time execution budget of the UR
controller, causing a Protective Stop (PSTOP) with error:

_C271A1:6 – Low Level real-time thread: Runtime is too much behind_

In practice, this means that clearing a large trajectory buffer can
block the real‑time thread long enough to trigger a safety stop on the
robot.

To avoid blocking the real‑time thread, trajectory cleanup is now
performed incrementally across multiple cycles instead of in a single
cycle.

Author: srvald

resources/external_control.urscript

@@ -106,6 +106,10 @@ global coulomb_scale = [0.8, 0.8, 0.7, 0.8, 0.8, 0.8]
 thread_move = 0
 thread_trajectory = 0
 thread_script_commands = 0
+thread_clear = 0
+
+# Global thread flags
+global trajectory_cleanup_running = False
 
 ###
 # @brief Function to verify whether the specified target can be reached within the defined time frame while staying within
@@ -614,7 +618,9 @@ thread trajectoryThread():
           is_robot_moving = quinticSplineRun(q, qd, qdd, tmptime, is_last_point, is_first_point)
         else:
           textmsg("Unknown spline type given:", raw_point[INDEX_POINT_TYPE])
-          clear_remaining_trajectory_points()
+          request_trajectory_cleanup()
+          stopj(STOPJ_ACCELERATION)
+          wait_for_trajectory_cleanup()
           trajectory_result = TRAJECTORY_RESULT_FAILURE
         end
 
@@ -687,13 +693,50 @@ thread trajectoryThread():
   textmsg("Trajectory finished with result ", trajectory_result_to_str(trajectory_result))
 end
 
-def clear_remaining_trajectory_points():
+# Thread to cleanup the remaining trajectory_points_left when trajectory canceled
+thread clearTrajectoryPointsThread():
+  local reads = 0
+  local limit = 0
+  local timeout = 0.5
+
+  {% if ROBOT_SOFTWARE_VERSION >= v10.0.0 %}
+    limit = 20
+  {% else %}
+    limit = 10
+  {% endif %}  
+  
   while trajectory_points_left > 0:
-    raw_point = socket_read_binary_integer(TRAJECTORY_DATA_DIMENSION + 2, "trajectory_socket")
+    raw_point = socket_read_binary_integer(TRAJECTORY_DATA_DIMENSION + 2, "trajectory_socket", timeout)
+    if raw_point[0] <= 0:
+      trajectory_points_left = 0
+      textmsg("No more trajectory points in the buffer. Resetting trajectory_points_left to 0.")
+      break
+    end
     trajectory_points_left = trajectory_points_left - 1
+    reads = reads + 1
+    if reads >= limit:
+      sync()            
+      reads = 0
+    end
+  end
+  textmsg("Trajectory point buffer cleared successfully.")
+  trajectory_cleanup_running = False
+end
+
+# Checks that the thread is only called once
+def request_trajectory_cleanup():
+  if not trajectory_cleanup_running:
+    trajectory_cleanup_running = True
+    thread_clear = run clearTrajectoryPointsThread()
   end
 end
 
+# Wait until the trajectory cleanup has finished
+def wait_for_trajectory_cleanup():
+  join thread_clear
+  thread_clear = 0
+end
+
 # Helpers for speed control
 def set_speedl(twist):
   cmd_twist = twist
@@ -755,7 +798,7 @@ def tool_contact_detection():
   if step_back > 0:
     if control_mode == MODE_FORWARD:
       kill thread_trajectory
-      clear_remaining_trajectory_points()
+      request_trajectory_cleanup()
     elif control_mode == MODE_FREEDRIVE:
       textmsg("Leaving freedrive mode")
       end_freedrive_mode()
@@ -766,7 +809,7 @@ def tool_contact_detection():
     # Set control mode to tool in contact, should be cleared by stopping tool contact detection
     control_mode = MODE_TOOL_IN_CONTACT
     stopl(3)
-
+    wait_for_trajectory_cleanup()
     # Move to initial contact point
     q = get_actual_joint_positions_history(step_back)
     movel(q)
@@ -935,6 +978,7 @@ end
 control_mode = MODE_UNINITIALIZED
 thread_move = 0
 thread_trajectory = 0
+thread_clear = 0
 trajectory_points_left = 0
 textmsg("ExternalControl: External control active")
 global read_timeout = 0.0 # First read is blocking
@@ -952,8 +996,9 @@ while control_mode > MODE_STOPPED:
       if control_mode == MODE_FORWARD:
         kill thread_trajectory
         if trajectory_points_left > 0:
-          clear_remaining_trajectory_points()
+          request_trajectory_cleanup()
           stopj(STOPJ_ACCELERATION)
+          wait_for_trajectory_cleanup()
           socket_send_int(TRAJECTORY_RESULT_CANCELED, "trajectory_socket")
         end
         # Stop freedrive
@@ -1000,16 +1045,21 @@ while control_mode > MODE_STOPPED:
       torque = [params_mult[2]/ MULT_jointstate, params_mult[3]/ MULT_jointstate, params_mult[4]/ MULT_jointstate, params_mult[5]/ MULT_jointstate, params_mult[6]/ MULT_jointstate, params_mult[7]/ MULT_jointstate]
       set_torque(torque)
     elif control_mode == MODE_FORWARD:
-      if params_mult[2] == TRAJECTORY_MODE_RECEIVE:
+      if trajectory_cleanup_running:
+          textmsg("Trajectory buffer cleanup in progress, new trajectory ignored")
+          popup("Trajectory buffer cleanup in progress, new trajectory ignored", error = True, blocking = True)
+      elif params_mult[2] == TRAJECTORY_MODE_RECEIVE:
         kill thread_trajectory
-        clear_remaining_trajectory_points()
+        request_trajectory_cleanup()
+        wait_for_trajectory_cleanup()
         trajectory_points_left = params_mult[3]
         thread_trajectory = run trajectoryThread()
       elif params_mult[2] == TRAJECTORY_MODE_CANCEL:
         textmsg("cancel received")
         kill thread_trajectory
-        clear_remaining_trajectory_points()
+        request_trajectory_cleanup()
         stopj(STOPJ_ACCELERATION)
+        wait_for_trajectory_cleanup()
         socket_send_int(TRAJECTORY_RESULT_CANCELED, "trajectory_socket")
       end
     elif control_mode == MODE_SPEEDL:

tests/test_spline_interpolation.cpp

@@ -1115,6 +1115,114 @@ TEST_F(SplineInterpolationTest, switching_control_mode_with_trajectory_produces_
   EXPECT_TRUE(waitForTrajectoryResult(std::chrono::milliseconds(500)));
 }
 
+TEST_F(SplineInterpolationTest, new_trajectory_received_without_cancelling_the_one_executed)
+{
+  rtde_interface::DataPackage data_pkg(g_my_robot->getUrDriver()->getRTDEOutputRecipe());
+  ASSERT_TRUE(g_my_robot->getUrDriver()->getDataPackage(data_pkg));
+
+  urcl::vector6d_t joint_positions_before{ 0, 0, 0, 0, 0, 0 };
+  ASSERT_TRUE(data_pkg.getData("target_q", joint_positions_before));
+
+  std::vector<urcl::vector6d_t> s_pos, s_vel, s_acc;
+  std::vector<urcl::vector6d_t> positions{ { -1.57, -1.57, 0, 0, 0, 0 },
+                                           { -1.57, -1.57, -1.57, 0, 0, 0 },
+                                           { -1.57, -1.57, 0, 0, 0, 0 },
+                                           { -1.57, -1.6, 1.6, -0.7, 0.7, 0.2 } };
+  std::vector<urcl::vector6d_t> velocities{
+    { 0, 0, 0.0, 0, 0, 0 }, { 0, 0, 0.0, 0, 0, 0 }, { 0, 0, 1.5, 0, 0, 0 }, { 0, 0, 0, 0, 0, 0 }
+  };
+  std::vector<double> s_time{ 3.0, 3.0, 3.0, 3.0 };
+  double increment = 0.005;
+  double duration = 0.1;
+
+  // Generate a trajectory with different target points
+  for (size_t i = 0; i < 5; ++i)
+  {
+    s_pos.push_back({ -1.57, -1.6, 1.6, -0.7, 0.7 + i * increment, 0.2 });
+    s_vel.push_back({ 0, 0, 0, 0, increment / duration, 0 });
+    s_acc.push_back({ 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 });
+    s_time.push_back(duration);
+  }
+
+  // Send the trajectory
+  sendTrajectory(s_pos, s_vel, s_acc, s_time);
+  waitForTrajectoryStarted();
+
+  // Send the trajectory again without canceling the before one
+  sendTrajectory(s_pos, s_vel, s_acc, s_time);
+  g_trajectory_running = true;
+  waitForTrajectoryStarted();
+
+  // Ensure that everything goes as expected and the robot reaches the target position
+  urcl::vector6d_t joint_positions;
+  while (g_trajectory_running)
+  {
+    ASSERT_TRUE(g_my_robot->getUrDriver()->getDataPackage(data_pkg));
+    ASSERT_TRUE(data_pkg.getData("target_q", joint_positions));
+    // Keep connection alive
+    ASSERT_TRUE(g_my_robot->getUrDriver()->writeTrajectoryControlMessage(
+        urcl::control::TrajectoryControlMessage::TRAJECTORY_NOOP));
+  }
+  EXPECT_EQ(control::TrajectoryResult::TRAJECTORY_RESULT_SUCCESS, g_trajectory_result);
+  urcl::vector6d_t joint_positions_after;
+  ASSERT_TRUE(data_pkg.getData("actual_q", joint_positions_after));
+  const auto& expected_final = s_pos.back();
+  for (unsigned int i = 0; i < 6; ++i)
+  {
+    EXPECT_NEAR(joint_positions[i], expected_final[i], eps_);
+  }
+}
+
+TEST_F(SplineInterpolationTest, cancel_trajectory_while_being_executed_and_sending_new_one)
+{
+  rtde_interface::DataPackage data_pkg(g_my_robot->getUrDriver()->getRTDEOutputRecipe());
+  ASSERT_TRUE(g_my_robot->getUrDriver()->getDataPackage(data_pkg));
+
+  urcl::vector6d_t joint_positions_before{ 0, 0, 0, 0, 0, 0 };
+  ASSERT_TRUE(data_pkg.getData("target_q", joint_positions_before));
+
+  std::vector<urcl::vector6d_t> s_pos, s_vel, s_acc;
+  std::vector<urcl::vector6d_t> positions{ { -1.57, -1.57, 0, 0, 0, 0 },
+                                           { -1.57, -1.57, -1.57, 0, 0, 0 },
+                                           { -1.57, -1.57, 0, 0, 0, 0 },
+                                           { -1.57, -1.6, 1.6, -0.7, 0.7, 0.2 } };
+  std::vector<urcl::vector6d_t> velocities{
+    { 0, 0, 0.0, 0, 0, 0 }, { 0, 0, 0.0, 0, 0, 0 }, { 0, 0, 1.5, 0, 0, 0 }, { 0, 0, 0, 0, 0, 0 }
+  };
+  std::vector<double> s_time{ 3.0, 3.0, 3.0, 3.0 };
+  double increment = 0.005;
+  double duration = 0.1;
+
+  // Generate a trajectory with 500 points
+  for (size_t i = 0; i < 500; ++i)
+  {
+    s_pos.push_back({ -1.57, -1.6, 1.6, -0.7, 0.7 + i * increment, 0.2 });
+    s_vel.push_back({ 0, 0, 0, 0, increment / duration, 0 });
+    s_acc.push_back({ 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 });
+    s_time.push_back(duration);
+  }
+
+  // Send the trajectory and cancel it right away
+  sendTrajectory(s_pos, s_vel, s_acc, s_time);
+  waitForTrajectoryStarted();
+
+  ASSERT_TRUE(g_my_robot->getUrDriver()->writeTrajectoryControlMessage(
+      urcl::control::TrajectoryControlMessage::TRAJECTORY_CANCEL));
+  EXPECT_TRUE(waitForTrajectoryResult(std::chrono::milliseconds(500)));
+  // Ensure we receive the cancel trajectory result, which will mean that the trajectory buffer will be clear
+  EXPECT_EQ(control::TrajectoryResult::TRAJECTORY_RESULT_CANCELED, g_trajectory_result);
+
+  // Do the same procesure again to ensure that a new trajectory can be send after canceling the before
+  // one in the middle of its execution
+  sendTrajectory(s_pos, s_vel, s_acc, s_time);
+  waitForTrajectoryStarted();
+
+  ASSERT_TRUE(g_my_robot->getUrDriver()->writeTrajectoryControlMessage(
+      urcl::control::TrajectoryControlMessage::TRAJECTORY_CANCEL));
+  EXPECT_TRUE(waitForTrajectoryResult(std::chrono::milliseconds(500)));
+  EXPECT_EQ(control::TrajectoryResult::TRAJECTORY_RESULT_CANCELED, g_trajectory_result);
+}
+
 int main(int argc, char* argv[])
 {
   ::testing::InitGoogleTest(&argc, argv);