Add trajectory passthrough controller (#944)

This adds a controller that allows sending a complete trajectory to the robot for execution.

---------

Co-authored-by: Felix Exner <[email protected]>

Author: URJala

ur_controllers/CMakeLists.txt

@@ -19,6 +19,10 @@ find_package(std_srvs REQUIRED)
 find_package(ur_dashboard_msgs REQUIRED)
 find_package(ur_msgs REQUIRED)
 find_package(generate_parameter_library REQUIRED)
+find_package(trajectory_msgs REQUIRED)
+find_package(control_msgs REQUIRED)
+find_package(action_msgs REQUIRED)
+
 
 set(THIS_PACKAGE_INCLUDE_DEPENDS
   angles
@@ -34,6 +38,9 @@ set(THIS_PACKAGE_INCLUDE_DEPENDS
   ur_dashboard_msgs
   ur_msgs
   generate_parameter_library
+  control_msgs
+  trajectory_msgs
+  action_msgs
 )
 
 include_directories(include)
@@ -54,6 +61,11 @@ generate_parameter_library(
   src/scaled_joint_trajectory_controller_parameters.yaml
 )
 
+generate_parameter_library(
+  passthrough_trajectory_controller_parameters
+  src/passthrough_trajectory_controller_parameters.yaml
+)
+
 generate_parameter_library(
   ur_configuration_controller_parameters
   src/ur_configuration_controller_parameters.yaml
@@ -63,6 +75,7 @@ add_library(${PROJECT_NAME} SHARED
   src/scaled_joint_trajectory_controller.cpp
   src/speed_scaling_state_broadcaster.cpp
   src/gpio_controller.cpp
+  src/passthrough_trajectory_controller.cpp
   src/ur_configuration_controller.cpp)
 
 target_include_directories(${PROJECT_NAME} PRIVATE
@@ -72,6 +85,7 @@ target_link_libraries(${PROJECT_NAME}
   gpio_controller_parameters
   speed_scaling_state_broadcaster_parameters
   scaled_joint_trajectory_controller_parameters
+  passthrough_trajectory_controller_parameters
   ur_configuration_controller_parameters
 )
 ament_target_dependencies(${PROJECT_NAME}

ur_controllers/controller_plugins.xml

@@ -14,6 +14,11 @@
       This controller publishes the Tool IO.
     </description>
   </class>
+  <class name="ur_controllers/PassthroughTrajectoryController" type="ur_controllers::PassthroughTrajectoryController" base_class_type="controller_interface::ControllerInterface">
+    <description>
+      This controller forwards a joint-based trajectory to the robot controller for interpolation.
+    </description>
+  </class>
   <class name="ur_controllers/URConfigurationController" type="ur_controllers::URConfigurationController" base_class_type="controller_interface::ControllerInterface">
     <description>
       Controller used to get and change the configuration of the robot

ur_controllers/doc/index.rst

@@ -135,3 +135,132 @@ Advertised services
 * ``~/set_speed_slider [ur_msgs/srv/SetSpeedSliderFraction]``: Set the value of the speed slider.
 * ``~/zero_ftsensor [std_srvs/srv/Trigger]``: Zeroes the reported wrench of the force torque
   sensor.
+
+.. _passthrough_trajectory_controller:
+
+ur_controllers/PassthroughTrajectoryController
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+
+This controller uses a ``control_msgs/FollowJointTrajectory`` action but instead of interpolating
+the trajectory on the ROS pc it forwards the complete trajectory to the robot controller for
+interpolation and execution. This way, the realtime requirements for the control PC can be
+massively decreased, since the robot always "knows" what to do next. That means that you should be
+able to run a stable driver connection also without a real-time patched kernel.
+
+Interpolation depends on the robot controller's implementation, but in conjunction with the
+ur_robot_driver it defaults to mimicking ros2_control's spline interpolation. So, any trajectory
+planned e.g. with MoveIt! will be executed following the trajectory exactly.
+
+A trajectory sent to the controller's action server will be forwarded to the robot controller and
+executed there. Once all setpoints are transferred to the robot, the controller goes into a waiting
+state where it waits for the trajectory to be finished. While waiting, the controller tracks the
+time spent on the trajectory to ensure the robot isn't stuck during execution.
+
+This controller also supports **speed scaling** such that and scaling down of the trajectory done
+by the robot, for example due to safety settings on the robot or simply because a slower execution
+is configured on the teach pendant. This will be considered, during execution monitoring, so the
+controller basically tracks the scaled time instead of the real time.
+
+.. note::
+
+   When using this controller with the URSim simulator execution times can be slightly larger than
+   the expected time depending on the simulation host's resources. This effect will not be present
+   when using a real UR arm.
+
+.. note::
+
+   This controller can currently only be used with URSim or a real UR robot. Neither mock hardware
+   nor gazebo support this type of trajectory interfaces at the time being.
+
+Tolerances
+""""""""""
+
+Currently, the trajectory passthrough controller only supports goal tolerances and goal time
+tolerances passed in the action directly. Please make sure that the tolerances are completely
+filled with all joint names.
+
+A **goal time tolerance** of ``0.0`` means that no goal time tolerance is set and the action will
+not fail when execution takes too long.
+
+Action interface / usage
+""""""""""""""""""""""""
+
+To use this controller, publish a goal to the ``~/follow_joint_trajectory`` action interface
+similar to the `joint_trajectory_controller <https://control.ros.org/master/doc/ros2_controllers/joint_trajectory_controller/doc/userdoc.html>`_.
+
+Currently, the controller doesn't support replacing a running trajectory action. While a trajectory
+is being executed, goals will be rejected until the action has finished. If you want to replace it,
+first cancel the running action and then send a new one.
+
+Parameters
+""""""""""
+
+The trajectory passthrough controller uses the following parameters:
+
++----------------------------------+--------------+----------------------------------------+------------------------------------------------------------------------------------------------------------------+
+| Parameter name                   | Type         | Default value                          | Description                                                                                                      |
+|                                  |              |                                        |                                                                                                                  |
++----------------------------------+--------------+----------------------------------------+------------------------------------------------------------------------------------------------------------------+
+| ``joints`` (required)            | string_array | <empty>                                | Joint names to  listen to                                                                                        |
++----------------------------------+--------------+----------------------------------------+------------------------------------------------------------------------------------------------------------------+
+| ``state_interfaces`` (required)  | string_array | <empty>                                | State interfaces provided by the hardware for all joints. Subset of ``["position", "velocity", "acceleration"]`` |
++----------------------------------+--------------+----------------------------------------+------------------------------------------------------------------------------------------------------------------+
+| ``speed_scaling_interface_name`` | string       | ``speed_scaling/speed_scaling_factor`` | Fully qualified name of the speed scaling interface name.                                                        |
++----------------------------------+--------------+----------------------------------------+------------------------------------------------------------------------------------------------------------------+
+| ``tf_prefix``                    | string       | <empty>                                | Urdf prefix of the corresponding arm                                                                             |
++----------------------------------+--------------+----------------------------------------+------------------------------------------------------------------------------------------------------------------+
+
+Interfaces
+""""""""""
+
+In order to use this, the hardware has to export a command interface for passthrough operations for each joint. It always has
+to export position, velocity and acceleration interfaces in order to be able to project the full
+JointTrajectory definition. This is why there are separate fields used, as for passthrough mode
+accelerations might be relevant also for robots that don't support commanding accelerations
+directly to their joints.
+
+.. code:: xml
+
+   <gpio name="${tf_prefix}trajectory_passthrough">
+     <command_interface name="setpoint_positions_0"/>
+     <command_interface name="setpoint_positions_1"/>
+     <command_interface name="setpoint_positions_2"/>
+     <command_interface name="setpoint_positions_3"/>
+     <command_interface name="setpoint_positions_4"/>
+     <command_interface name="setpoint_positions_5"/>
+     <command_interface name="setpoint_velocities_0"/>
+     <command_interface name="setpoint_velocities_1"/>
+     <command_interface name="setpoint_velocities_2"/>
+     <command_interface name="setpoint_velocities_3"/>
+     <command_interface name="setpoint_velocities_4"/>
+     <command_interface name="setpoint_velocities_5"/>
+     <command_interface name="setpoint_accelerations_0"/>
+     <command_interface name="setpoint_accelerations_1"/>
+     <command_interface name="setpoint_accelerations_2"/>
+     <command_interface name="setpoint_accelerations_3"/>
+     <command_interface name="setpoint_accelerations_4"/>
+     <command_interface name="setpoint_accelerations_5"/>
+     <command_interface name="transfer_state"/>
+     <command_interface name="time_from_start"/>
+     <command_interface name="abort"/>
+   </gpio>
+
+.. note::
+
+   The hardware component has to take care that the passthrough command interfaces cannot be
+   activated in parallel to the streaming command interfaces.
+
+Implementation details / dataflow
+"""""""""""""""""""""""""""""""""
+
+* A trajectory passed to the controller will be sent to the hardware component one by one.
+* The controller will send one setpoint and then wait for the hardware to acknowledge that it can
+  take a new setpoint.
+* This happens until all setpoints have been transferred to the hardware. Then, the controller goes
+  into a waiting state where it monitors execution time and waits for the hardware to finish
+  execution.
+* If execution takes longer than anticipated, a warning will be printed.
+* If execution finished taking longer than expected (plus the goal time tolerance), the action will fail.
+* When the hardware reports that execution has been aborted (The ``passthrough_trajectory_abort``
+  command interface), the action will be aborted.
+* When the action is preempted, execution on the hardware is preempted.

ur_controllers/include/ur_controllers/passthrough_trajectory_controller.hpp

@@ -0,0 +1,184 @@
+// Copyright 2024, Universal Robots A/S
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//
+//    * Redistributions of source code must retain the above copyright
+//      notice, this list of conditions and the following disclaimer.
+//
+//    * Redistributions in binary form must reproduce the above copyright
+//      notice, this list of conditions and the following disclaimer in the
+//      documentation and/or other materials provided with the distribution.
+//
+//    * Neither the name of the {copyright_holder} nor the names of its
+//      contributors may be used to endorse or promote products derived from
+//      this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+// POSSIBILITY OF SUCH DAMAGE.
+
+//----------------------------------------------------------------------
+/*!\file
+ *
+ * \author  Jacob Larsen [email protected]
+ * \date    2024-03-11
+ *
+ *
+ *
+ *
+ */
+//----------------------------------------------------------------------
+
+#ifndef UR_CONTROLLERS__PASSTHROUGH_TRAJECTORY_CONTROLLER_HPP_
+#define UR_CONTROLLERS__PASSTHROUGH_TRAJECTORY_CONTROLLER_HPP_
+
+#include <stdint.h>
+
+#include <realtime_tools/realtime_buffer.h>
+#include <realtime_tools/realtime_server_goal_handle.h>
+
+#include <functional>
+#include <limits>
+#include <memory>
+#include <string>
+#include <unordered_map>
+#include <vector>
+
+#include <controller_interface/controller_interface.hpp>
+#include <rclcpp_action/server.hpp>
+#include <rclcpp_action/create_server.hpp>
+#include <rclcpp/rclcpp.hpp>
+#include <rclcpp_action/server_goal_handle.hpp>
+#include <rclcpp/time.hpp>
+#include <rclcpp/duration.hpp>
+#include <rclcpp/clock.hpp>
+
+#include <trajectory_msgs/msg/joint_trajectory.hpp>
+#include <control_msgs/action/follow_joint_trajectory.hpp>
+
+#include "passthrough_trajectory_controller_parameters.hpp"
+
+namespace ur_controllers
+{
+
+/*
+ * 0.0: No trajectory to forward, the controller is idling and ready to receive a new trajectory.
+ * 1.0: The controller has received and accepted a new trajectory. When the state is 1.0, the controller will write a
+ * point to the hardware interface.
+ * 2.0: The hardware interface will read the point written from the controller. The state will switch between 1.0
+ * and 2.0 until all points have been read by the hardware interface.
+ * 3.0: The hardware interface has read all the points, and will now write all the points to the physical robot
+ * controller.
+ * 4.0: The robot is moving through the trajectory.
+ * 5.0: The robot finished executing the trajectory.
+ */
+const double TRANSFER_STATE_IDLE = 0.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;
+const double TRANSFER_STATE_IN_MOTION = 4.0;
+const double TRANSFER_STATE_DONE = 5.0;
+
+using namespace std::chrono_literals;  // NOLINT
+
+class PassthroughTrajectoryController : public controller_interface::ControllerInterface
+{
+public:
+  PassthroughTrajectoryController() = default;
+  ~PassthroughTrajectoryController() override = default;
+
+  controller_interface::InterfaceConfiguration state_interface_configuration() const override;
+
+  controller_interface::InterfaceConfiguration command_interface_configuration() const override;
+
+  controller_interface::CallbackReturn on_init() override;
+
+  controller_interface::CallbackReturn on_configure(const rclcpp_lifecycle::State& previous_state) override;
+
+  controller_interface::CallbackReturn on_activate(const rclcpp_lifecycle::State& state) override;
+
+  controller_interface::CallbackReturn on_deactivate(const rclcpp_lifecycle::State& state) override;
+
+  controller_interface::return_type update(const rclcpp::Time& time, const rclcpp::Duration& period) override;
+
+private:
+  using FollowJTrajAction = control_msgs::action::FollowJointTrajectory;
+  using RealtimeGoalHandle = realtime_tools::RealtimeServerGoalHandle<FollowJTrajAction>;
+  using RealtimeGoalHandlePtr = std::shared_ptr<RealtimeGoalHandle>;
+  using RealtimeGoalHandleBuffer = realtime_tools::RealtimeBuffer<RealtimeGoalHandlePtr>;
+
+  RealtimeGoalHandleBuffer rt_active_goal_;         ///< Currently active action goal, if any.
+  rclcpp::TimerBase::SharedPtr goal_handle_timer_;  ///< Timer to frequently check on the running goal
+  realtime_tools::RealtimeBuffer<std::unordered_map<std::string, size_t>> joint_trajectory_mapping_;
+
+  rclcpp::Duration action_monitor_period_ = rclcpp::Duration(50ms);
+
+  /* Start an action server with an action called: /passthrough_trajectory_controller/forward_joint_trajectory. */
+  void start_action_server(void);
+
+  void end_goal();
+
+  bool check_goal_tolerance();
+
+  // Get a mapping between the trajectory's joint order and the internal one
+  std::unordered_map<std::string, size_t> create_joint_mapping(const std::vector<std::string>& joint_names) const;
+
+  std::shared_ptr<passthrough_trajectory_controller::ParamListener> passthrough_param_listener_;
+  passthrough_trajectory_controller::Params passthrough_params_;
+
+  rclcpp_action::Server<control_msgs::action::FollowJointTrajectory>::SharedPtr send_trajectory_action_server_;
+
+  rclcpp_action::GoalResponse
+  goal_received_callback(const rclcpp_action::GoalUUID& uuid,
+                         std::shared_ptr<const control_msgs::action::FollowJointTrajectory::Goal> goal);
+
+  rclcpp_action::CancelResponse goal_cancelled_callback(
+      const std::shared_ptr<rclcpp_action::ServerGoalHandle<control_msgs::action::FollowJointTrajectory>> goal_handle);
+
+  void goal_accepted_callback(
+      std::shared_ptr<rclcpp_action::ServerGoalHandle<control_msgs::action::FollowJointTrajectory>> goal_handle);
+
+  realtime_tools::RealtimeBuffer<std::vector<std::string>> joint_names_;
+  std::vector<std::string> state_interface_types_;
+
+  std::vector<std::string> joint_state_interface_names_;
+  std::vector<std::reference_wrapper<hardware_interface::LoanedStateInterface>> joint_position_state_interface_;
+  std::vector<std::reference_wrapper<hardware_interface::LoanedStateInterface>> joint_velocity_state_interface_;
+  std::vector<std::reference_wrapper<hardware_interface::LoanedStateInterface>> joint_acceleration_state_interface_;
+
+  bool check_goal_tolerances(std::shared_ptr<const control_msgs::action::FollowJointTrajectory::Goal> goal);
+  bool check_goal_positions(std::shared_ptr<const control_msgs::action::FollowJointTrajectory::Goal> goal);
+  bool check_goal_velocities(std::shared_ptr<const control_msgs::action::FollowJointTrajectory::Goal> goal);
+  bool check_goal_accelerations(std::shared_ptr<const control_msgs::action::FollowJointTrajectory::Goal> goal);
+
+  trajectory_msgs::msg::JointTrajectory active_joint_traj_;
+  // std::vector<control_msgs::msg::JointTolerance> path_tolerance_;
+  realtime_tools::RealtimeBuffer<std::vector<control_msgs::msg::JointTolerance>> goal_tolerance_;
+  realtime_tools::RealtimeBuffer<rclcpp::Duration> goal_time_tolerance_{ rclcpp::Duration(0, 0) };
+
+  std::atomic<size_t> current_index_;
+  std::atomic<bool> trajectory_active_;
+  rclcpp::Duration active_trajectory_elapsed_time_ = rclcpp::Duration::from_nanoseconds(0);
+  rclcpp::Duration max_trajectory_time_ = rclcpp::Duration::from_nanoseconds(0);
+  double scaling_factor_;
+  std::atomic<size_t> number_of_joints_;
+  static constexpr double NO_VAL = std::numeric_limits<double>::quiet_NaN();
+
+  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>> transfer_command_interface_;
+  std::optional<std::reference_wrapper<hardware_interface::LoanedCommandInterface>> time_from_start_command_interface_;
+
+  rclcpp::Clock::SharedPtr clock_;
+};
+}  // namespace ur_controllers
+#endif  // UR_CONTROLLERS__PASSTHROUGH_TRAJECTORY_CONTROLLER_HPP_

ur_controllers/package.xml

@@ -33,6 +33,10 @@
   <depend>std_srvs</depend>
   <depend>ur_dashboard_msgs</depend>
   <depend>ur_msgs</depend>
+  <depend>control_msgs</depend>
+  <depend>trajectory_msgs</depend>
+  <depend>action_msgs</depend>
+
 
   <export>
     <build_type>ament_cmake</build_type>