Start executing passthrough trajectories earlier than all points are transferred. (#1313)

* Start executing passthrough trajectories earlier than all points are transferred.

When transferring trajectories with many waypoints it can take quite a
while until all points are transferred from the controller to the
hardware interface, especially on lower hardware_interface update rates.

With this change, execution starts as soon as enough trajectory points
are transferred to fill 5 control cycles.

This PR effectively allows reducing the hardware_interface update_rate
when using the passthrough_trajectory_controller significantly. This
will reduce the networking requirements improving the overall user
experience.

* Make sure we acknowledge a new trajectory before sending points

If we miss the new trajectory, wen cannot resize the vectors
appropriately.

(cherry picked from commit 14f80ac)

# Conflicts:
#	ur_controllers/src/passthrough_trajectory_controller.cpp
#	ur_robot_driver/urdf/ur.ros2_control.xacro

Author: urfeex

ur_controllers/include/ur_controllers/passthrough_trajectory_controller.hpp

@@ -81,6 +81,7 @@ namespace ur_controllers
  * 5.0: The robot finished executing the trajectory.
  */
 const double TRANSFER_STATE_IDLE = 0.0;
+const double TRANSFER_STATE_NEW_TRAJECTORY = 6.0;
 const double TRANSFER_STATE_WAITING_FOR_POINT = 1.0;
 const double TRANSFER_STATE_TRANSFERRING = 2.0;
 const double TRANSFER_STATE_TRANSFER_DONE = 3.0;
@@ -174,6 +175,7 @@ class PassthroughTrajectoryController : public controller_interface::ControllerI
 
   std::optional<std::reference_wrapper<hardware_interface::LoanedStateInterface>> scaling_state_interface_;
   std::optional<std::reference_wrapper<hardware_interface::LoanedCommandInterface>> abort_command_interface_;
+  std::optional<std::reference_wrapper<hardware_interface::LoanedCommandInterface>> trajectory_size_command_interface_;
   std::optional<std::reference_wrapper<hardware_interface::LoanedCommandInterface>> transfer_command_interface_;
   std::optional<std::reference_wrapper<hardware_interface::LoanedCommandInterface>> time_from_start_command_interface_;
 

ur_controllers/src/passthrough_trajectory_controller.cpp

@@ -130,6 +130,7 @@ controller_interface::InterfaceConfiguration PassthroughTrajectoryController::co
   config.names.push_back(tf_prefix + "trajectory_passthrough/abort");
   config.names.emplace_back(tf_prefix + "trajectory_passthrough/transfer_state");
   config.names.emplace_back(tf_prefix + "trajectory_passthrough/time_from_start");
+  config.names.emplace_back(tf_prefix + "trajectory_passthrough/trajectory_size");
 
   return config;
 }
@@ -178,6 +179,19 @@ controller_interface::CallbackReturn PassthroughTrajectoryController::on_activat
     }
   }
 
+  {
+    const std::string interface_name = passthrough_params_.tf_prefix + "trajectory_passthrough/"
+                                                                       "trajectory_size";
+    auto it = std::find_if(command_interfaces_.begin(), command_interfaces_.end(),
+                           [&](auto& interface) { return (interface.get_name() == interface_name); });
+    if (it != command_interfaces_.end()) {
+      trajectory_size_command_interface_ = *it;
+    } else {
+      RCLCPP_ERROR(get_node()->get_logger(), "Did not find '%s' in command interfaces.", interface_name.c_str());
+      return controller_interface::CallbackReturn::ERROR;
+    }
+  }
+
   const std::string tf_prefix = passthrough_params_.tf_prefix;
   {
     const std::string interface_name = tf_prefix + "trajectory_passthrough/transfer_state";
@@ -247,7 +261,13 @@ controller_interface::return_type PassthroughTrajectoryController::update(const
       active_trajectory_elapsed_time_ = rclcpp::Duration(0, 0);
       max_trajectory_time_ =
           rclcpp::Duration::from_seconds(duration_to_double(active_joint_traj_.points.back().time_from_start));
+<<<<<<< HEAD
       transfer_command_interface_->get().set_value(TRANSFER_STATE_WAITING_FOR_POINT);
+=======
+      write_success &= transfer_command_interface_->get().set_value(TRANSFER_STATE_NEW_TRAJECTORY);
+      write_success &=
+          trajectory_size_command_interface_->get().set_value(static_cast<double>(active_joint_traj_.points.size()));
+>>>>>>> 14f80ac (Start executing passthrough trajectories earlier than all points are transferred. (#1313))
     }
     auto active_goal_time_tol = goal_time_tolerance_.readFromRT();
     auto joint_mapping = joint_trajectory_mapping_.readFromRT();

ur_robot_driver/include/ur_robot_driver/hardware_interface.hpp

@@ -167,8 +167,7 @@ class URPositionHardwareInterface : public hardware_interface::SystemInterface
   void stop_force_mode();
   void check_passthrough_trajectory_controller();
   void trajectory_done_callback(urcl::control::TrajectoryResult result);
-  bool has_accelerations(std::vector<std::array<double, 6>> accelerations);
-  bool has_velocities(std::vector<std::array<double, 6>> velocities);
+  bool is_valid_joint_information(std::vector<std::array<double, 6>> data);
 
   urcl::vector6d_t urcl_position_commands_;
   urcl::vector6d_t urcl_position_commands_old_;
@@ -240,6 +239,7 @@ class URPositionHardwareInterface : public hardware_interface::SystemInterface
   // Passthrough trajectory controller interface values
   double passthrough_trajectory_transfer_state_;
   double passthrough_trajectory_abort_;
+  double passthrough_trajectory_size_;
   bool passthrough_trajectory_controller_running_;
   urcl::vector6d_t passthrough_trajectory_positions_;
   urcl::vector6d_t passthrough_trajectory_velocities_;

ur_robot_driver/src/hardware_interface.cpp

@@ -45,6 +45,7 @@
 #include "ur_client_library/exceptions.h"
 #include "ur_client_library/ur/tool_communication.h"
 #include "ur_client_library/ur/version_information.h"
+#include "ur_client_library/ur/robot_receive_timeout.h"
 
 #include <rclcpp/logging.hpp>
 #include "hardware_interface/types/hardware_interface_type_values.hpp"
@@ -93,9 +94,10 @@ URPositionHardwareInterface::on_init(const hardware_interface::HardwareInfo& sys
   freedrive_mode_abort_ = 0.0;
   passthrough_trajectory_transfer_state_ = 0.0;
   passthrough_trajectory_abort_ = 0.0;
-  trajectory_joint_positions_.clear();
-  trajectory_joint_velocities_.clear();
-  trajectory_joint_accelerations_.clear();
+  passthrough_trajectory_size_ = 0.0;
+  trajectory_joint_positions_.reserve(32768);
+  trajectory_joint_velocities_.reserve(32768);
+  trajectory_joint_accelerations_.reserve(32768);
 
   for (const hardware_interface::ComponentInfo& joint : info_.joints) {
     if (joint.command_interfaces.size() != 2) {
@@ -381,6 +383,9 @@ std::vector<hardware_interface::CommandInterface> URPositionHardwareInterface::e
   command_interfaces.emplace_back(
       hardware_interface::CommandInterface(tf_prefix + PASSTHROUGH_GPIO, "abort", &passthrough_trajectory_abort_));
 
+  command_interfaces.emplace_back(hardware_interface::CommandInterface(tf_prefix + PASSTHROUGH_GPIO, "trajectory_size",
+                                                                       &passthrough_trajectory_size_));
+
   for (size_t i = 0; i < 6; ++i) {
     command_interfaces.emplace_back(hardware_interface::CommandInterface(tf_prefix + PASSTHROUGH_GPIO,
                                                                          "setpoint_positions_" + std::to_string(i),
@@ -540,6 +545,9 @@ URPositionHardwareInterface::on_configure(const rclcpp_lifecycle::State& previou
     driver_config.handle_program_state =
         std::bind(&URPositionHardwareInterface::handleRobotProgramState, this, std::placeholders::_1);
     ur_driver_ = std::make_unique<urcl::UrDriver>(driver_config);
+    if (ur_driver_->getControlFrequency() != info_.rw_rate) {
+      ur_driver_->resetRTDEClient(output_recipe_filename, input_recipe_filename, info_.rw_rate);
+    }
   } catch (urcl::ToolCommNotAvailable& e) {
     RCLCPP_FATAL_STREAM(rclcpp::get_logger("URPositionHardwareInterface"), "See parameter use_tool_communication");
 
@@ -786,7 +794,9 @@ hardware_interface::return_type URPositionHardwareInterface::write(const rclcpp:
       ur_driver_->writeFreedriveControlMessage(urcl::control::FreedriveControlMessage::FREEDRIVE_NOOP);
 
     } else if (passthrough_trajectory_controller_running_) {
-      ur_driver_->writeTrajectoryControlMessage(urcl::control::TrajectoryControlMessage::TRAJECTORY_NOOP);
+      ur_driver_->writeTrajectoryControlMessage(
+          urcl::control::TrajectoryControlMessage::TRAJECTORY_NOOP, 0,
+          urcl::RobotReceiveTimeout::millisec(1000 * 5.0 / static_cast<double>(info_.rw_rate)));
       check_passthrough_trajectory_controller();
     } else {
       ur_driver_->writeKeepalive();
@@ -1335,59 +1345,102 @@ void URPositionHardwareInterface::stop_force_mode()
 void URPositionHardwareInterface::check_passthrough_trajectory_controller()
 {
   static double last_time = 0.0;
+  static size_t point_index_received = 0;
+  static size_t point_index_sent = 0;
+  static bool trajectory_started = false;
   // See passthrough_trajectory_controller.hpp for an explanation of the passthrough_trajectory_transfer_state_ values.
 
   // We should abort and are not in state IDLE
   if (passthrough_trajectory_abort_ == 1.0 && passthrough_trajectory_transfer_state_ != 0.0) {
     ur_driver_->writeTrajectoryControlMessage(urcl::control::TrajectoryControlMessage::TRAJECTORY_CANCEL);
+  } else if (passthrough_trajectory_transfer_state_ == 6.0) {
+    if (passthrough_trajectory_size_ != trajectory_joint_positions_.size()) {
+      RCLCPP_INFO(get_logger(), "Got a new trajectory with %lu points.",
+                  static_cast<size_t>(passthrough_trajectory_size_));
+      trajectory_joint_positions_.resize(passthrough_trajectory_size_);
+      trajectory_joint_velocities_.resize(passthrough_trajectory_size_);
+      trajectory_joint_accelerations_.resize(passthrough_trajectory_size_);
+      trajectory_times_.resize(passthrough_trajectory_size_);
+      point_index_received = 0;
+      point_index_sent = 0;
+      trajectory_started = false;
+      last_time = 0.0;
+      passthrough_trajectory_transfer_state_ = 1.0;
+    }
   } else if (passthrough_trajectory_transfer_state_ == 2.0) {
     passthrough_trajectory_abort_ = 0.0;
-    trajectory_joint_positions_.push_back(passthrough_trajectory_positions_);
+    trajectory_joint_positions_[point_index_received] = passthrough_trajectory_positions_;
 
-    trajectory_times_.push_back(passthrough_trajectory_time_from_start_ - last_time);
+    trajectory_times_[point_index_received] = passthrough_trajectory_time_from_start_ - last_time;
     last_time = passthrough_trajectory_time_from_start_;
 
-    if (!std::isnan(passthrough_trajectory_velocities_[0])) {
-      trajectory_joint_velocities_.push_back(passthrough_trajectory_velocities_);
-    }
-    if (!std::isnan(passthrough_trajectory_accelerations_[0])) {
-      trajectory_joint_accelerations_.push_back(passthrough_trajectory_accelerations_);
-    }
+    trajectory_joint_velocities_[point_index_received] = passthrough_trajectory_velocities_;
+    trajectory_joint_accelerations_[point_index_received] = passthrough_trajectory_accelerations_;
+
+    point_index_received++;
     passthrough_trajectory_transfer_state_ = 1.0;
-    /* When all points have been read, write them to the physical robot controller.*/
+
+    // Once we received enough points so we can move for at least 5 cycles, we start executing
+    if ((passthrough_trajectory_time_from_start_ > 5.0 / static_cast<double>(info_.rw_rate) ||
+         point_index_received == passthrough_trajectory_size_ - 1) &&
+        !trajectory_started) {
+      ur_driver_->writeTrajectoryControlMessage(urcl::control::TrajectoryControlMessage::TRAJECTORY_START,
+                                                trajectory_joint_positions_.size());
+      trajectory_started = true;
+    }
   } else if (passthrough_trajectory_transfer_state_ == 3.0) {
-    /* Tell robot controller how many points are in the trajectory. */
-    ur_driver_->writeTrajectoryControlMessage(urcl::control::TrajectoryControlMessage::TRAJECTORY_START,
-                                              trajectory_joint_positions_.size());
+    passthrough_trajectory_abort_ = 0.0;
+    passthrough_trajectory_transfer_state_ = 4.0;
+  } else if (passthrough_trajectory_transfer_state_ == 4.0) {
+    if (point_index_sent == trajectory_joint_positions_.size()) {
+      trajectory_joint_positions_.clear();
+      trajectory_joint_accelerations_.clear();
+      trajectory_joint_velocities_.clear();
+      trajectory_times_.clear();
+      last_time = 0.0;
+    }
+  }
+
+  // We basically get a setpoint from the controller in each cycle. We send all the points that we
+  // already received down to the hardware.
+  if (trajectory_started && point_index_sent <= trajectory_joint_positions_.size() &&
+      point_index_sent < point_index_received) {
+    bool error = false;
     /* Write the appropriate type of point depending on the combination of positions, velocities and accelerations. */
-    if (!has_velocities(trajectory_joint_velocities_) && !has_accelerations(trajectory_joint_accelerations_)) {
-      for (size_t i = 0; i < trajectory_joint_positions_.size(); i++) {
-        ur_driver_->writeTrajectorySplinePoint(trajectory_joint_positions_[i], urcl::vector6d_t{ 0, 0, 0, 0, 0, 0 },
-                                               trajectory_times_[i]);
-      }
-    } else if (has_velocities(trajectory_joint_velocities_) && !has_accelerations(trajectory_joint_accelerations_)) {
-      for (size_t i = 0; i < trajectory_joint_positions_.size(); i++) {
-        ur_driver_->writeTrajectorySplinePoint(trajectory_joint_positions_[i], trajectory_joint_velocities_[i],
-                                               trajectory_times_[i]);
-      }
-    } else if (!has_velocities(trajectory_joint_velocities_) && has_accelerations(trajectory_joint_accelerations_)) {
-      for (size_t i = 0; i < trajectory_joint_positions_.size(); i++) {
-        ur_driver_->writeTrajectorySplinePoint(trajectory_joint_positions_[i], trajectory_joint_accelerations_[i],
-                                               trajectory_times_[i]);
-      }
-    } else if (has_velocities(trajectory_joint_velocities_) && has_accelerations(trajectory_joint_accelerations_)) {
-      for (size_t i = 0; i < trajectory_joint_positions_.size(); i++) {
-        ur_driver_->writeTrajectorySplinePoint(trajectory_joint_positions_[i], trajectory_joint_velocities_[i],
-                                               trajectory_joint_accelerations_[i], trajectory_times_[i]);
+    for (size_t i = point_index_sent; i < point_index_received; i++) {
+      if (is_valid_joint_information(trajectory_joint_positions_)) {
+        if (!is_valid_joint_information(trajectory_joint_velocities_) &&
+            !is_valid_joint_information(trajectory_joint_accelerations_)) {
+          ur_driver_->writeTrajectorySplinePoint(trajectory_joint_positions_[i], urcl::vector6d_t{ 0, 0, 0, 0, 0, 0 },
+                                                 trajectory_times_[i]);
+        } else if (is_valid_joint_information(trajectory_joint_velocities_) &&
+                   !is_valid_joint_information(trajectory_joint_accelerations_)) {
+          ur_driver_->writeTrajectorySplinePoint(trajectory_joint_positions_[i], trajectory_joint_velocities_[i],
+                                                 trajectory_times_[i]);
+        } else if (!is_valid_joint_information(trajectory_joint_velocities_) &&
+                   is_valid_joint_information(trajectory_joint_accelerations_)) {
+          RCLCPP_ERROR(get_logger(), "Accelerations but no velocities given. If you want to specify accelerations with "
+                                     "a 0 velocity, please do that explicitly.");
+          error = true;
+          break;
+
+        } else if (is_valid_joint_information(trajectory_joint_velocities_) &&
+                   is_valid_joint_information(trajectory_joint_accelerations_)) {
+          ur_driver_->writeTrajectorySplinePoint(trajectory_joint_positions_[i], trajectory_joint_velocities_[i],
+                                                 trajectory_joint_accelerations_[i], trajectory_times_[i]);
+        }
+      } else {
+        RCLCPP_ERROR(get_logger(), "Trajectory points without position information are not supported.");
+        error = true;
+        break;
       }
+      point_index_sent++;
+    }
+    if (error) {
+      passthrough_trajectory_abort_ = 1.0;
+      passthrough_trajectory_transfer_state_ = 5.0;
+      return;
     }
-    trajectory_joint_positions_.clear();
-    trajectory_joint_accelerations_.clear();
-    trajectory_joint_velocities_.clear();
-    trajectory_times_.clear();
-    last_time = 0.0;
-    passthrough_trajectory_abort_ = 0.0;
-    passthrough_trajectory_transfer_state_ = 4.0;
   }
 }
 
@@ -1402,14 +1455,9 @@ void URPositionHardwareInterface::trajectory_done_callback(urcl::control::Trajec
   return;
 }
 
-bool URPositionHardwareInterface::has_velocities(std::vector<std::array<double, 6>> velocities)
-{
-  return (velocities.size() > 0);
-}
-
-bool URPositionHardwareInterface::has_accelerations(std::vector<std::array<double, 6>> accelerations)
+bool URPositionHardwareInterface::is_valid_joint_information(std::vector<std::array<double, 6>> data)
 {
-  return (accelerations.size() > 0);
+  return (data.size() > 0 && !std::isnan(data[0][0]));
 }
 
 }  // namespace ur_robot_driver