Trajectory until node (backport of #1461) (#1522)

mergify[bot] · web-flow · commit ee5e57be2d9b · 2025-10-17T15:51:50.000+02:00
The trajectory until node allows a user to execute a trajectory with an "until" condition enabled (currently only tool contact is available) without having to call 2 actions at the same time.
diff --git a/ur_robot_driver/CMakeLists.txt b/ur_robot_driver/CMakeLists.txt
@@ -126,8 +126,27 @@ add_executable(urscript_interface
 )
 ament_target_dependencies(urscript_interface ${${PROJECT_NAME}_EXPORTED_TARGETS} ${THIS_PACKAGE_INCLUDE_DEPENDS})
 
+#
+# trajectory_until_node
+#
+add_executable(trajectory_until_node
+  src/trajectory_until_node.cpp
+)
+target_link_libraries(trajectory_until_node PUBLIC
+  rclcpp::rclcpp
+  ${std_msgs_TARGETS}
+  rclcpp_action::rclcpp_action
+  ${ur_msgs_TARGETS}
+)
+
 install(
-  TARGETS dashboard_client ur_ros2_control_node controller_stopper_node urscript_interface robot_state_helper
+  TARGETS
+    controller_stopper_node
+    dashboard_client
+    robot_state_helper
+    trajectory_until_node
+    ur_ros2_control_node
+    urscript_interface
   DESTINATION lib/${PROJECT_NAME}
 )
 
@@ -177,6 +196,7 @@ install(PROGRAMS
   scripts/start_ursim.sh
   examples/examples.py
   examples/force_mode.py
+  examples/move_until_example.py
   DESTINATION lib/${PROJECT_NAME}
 )
 
@@ -246,5 +266,9 @@ if(BUILD_TESTING)
       TIMEOUT
         800
     )
+    add_launch_test(test/integration_test_trajectory_until.py
+      TIMEOUT
+        800
+    )
   endif()
 endif()
diff --git a/ur_robot_driver/doc/index.rst b/ur_robot_driver/doc/index.rst
@@ -18,9 +18,4 @@ Welcome to ur_robot_driver's documentation!
    generated/index
    robot_state_helper
    controller_stopper
-
-
-Indices and tables
-==================
-
-* :ref:`genindex`
+   trajectory_until_node
diff --git a/ur_robot_driver/doc/trajectory_until_node.rst b/ur_robot_driver/doc/trajectory_until_node.rst
@@ -0,0 +1,23 @@
+.. _trajectory_until_node:
+
+Trajectory until node
+=====================
+
+The trajectory until node allows a user to execute a trajectory with an "until" condition enabled (currently only tool contact is available) without having to call 2 actions at the same time. This means that the trajectory will execute until either the trajectory is finished or the "until" condition has been triggered. Both scenarios will result in the trajectory being reported as successful.
+
+Action interface / usage
+""""""""""""""""""""""""
+The node provides an action to execute a trajectory with tool contact enabled. For the node to accept action goals, both the motion controller and the tool contact controller need to be in ``active`` state.
+
+* ``/trajectory_until_node/execute [ur_msgs/action/TrajectoryUntil]``
+
+The action contains all the same fields as the ordinary `FollowJointTrajectory <http://docs.ros.org/en/noetic/api/control_msgs/html/action/FollowJointTrajectory.html>`_ action, but has two additional fields.
+One in the goal section called ``until_type``, which is used to choose between different conditions that can stop the trajectory. Currently only tool contact is available.
+The result section contains the other new field called ``until_condition_result``, which reports whether the chosen condition was triggered or not.
+
+Implementation details
+""""""""""""""""""""""
+Upon instantiation of the node, the internal trajectory action client will connect to an action named ``motion_controller/follow_joint_trajectory``.
+This action does not exist, but upon launch of the driver, the node is remapped to connect to the ``initial_joint_controller``, default is ``scaled_joint_trajectory_controller``.
+If you wish to use the node with another motion controller use the launch argument ``initial_joint_controller:=<your_motion_controller>`` when launching the driver.
+The node is only compatible with motion controllers that use the FollowJointTrajectory action interface.
diff --git a/ur_robot_driver/examples/move_until_example.py b/ur_robot_driver/examples/move_until_example.py
@@ -0,0 +1,132 @@
+#!/usr/bin/env python3
+# Copyright 2025, 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.
+
+import time
+
+import rclpy
+from rclpy.action import ActionClient
+
+from builtin_interfaces.msg import Duration
+from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint
+from ur_msgs.action import FollowJointTrajectoryUntil
+
+ROBOT_JOINTS = [
+    "elbow_joint",
+    "shoulder_lift_joint",
+    "shoulder_pan_joint",
+    "wrist_1_joint",
+    "wrist_2_joint",
+    "wrist_3_joint",
+]
+
+
+class MoveUntilExample(rclpy.node.Node):
+    def __init__(self):
+        super().__init__("move_until_example")
+
+        self._send_goal_future = None
+        self._get_result_future = None
+        self._goal_handle = None
+        self._action_client = ActionClient(
+            self, FollowJointTrajectoryUntil, "/trajectory_until_node/execute"
+        )
+        self._action_client.wait_for_server()
+        self.test_traj = {
+            "waypts": [[1.5, -1.5, 0.0, -1.5, -1.5, -1.5], [2.1, -1.2, 0.0, -2.4, -1.5, -1.5]],
+            "time_vec": [Duration(sec=3, nanosec=0), Duration(sec=6, nanosec=0)],
+        }
+        self.get_logger().info("Initialized")
+
+    def cancel_goal(self):
+        if self._goal_handle is not None:
+            self.get_logger().info("Cancelling goal")
+            future = self._goal_handle.cancel_goal_async()
+            future.add_done_callback(self.cancel_done)
+
+    def cancel_done(self, future):
+        try:
+            future.result()
+            self.get_logger().info("Goal cancelled successfully")
+        except Exception as e:
+            self.get_logger().error(f"Failed to cancel goal: {e}")
+
+    def process(self):
+        trajectory = JointTrajectory()
+        trajectory.joint_names = ROBOT_JOINTS
+
+        trajectory.points = [
+            JointTrajectoryPoint(
+                positions=self.test_traj["waypts"][i], time_from_start=self.test_traj["time_vec"][i]
+            )
+            for i in range(len(self.test_traj["waypts"]))
+        ]
+        goal = FollowJointTrajectoryUntil.Goal(
+            trajectory=trajectory, until_type=FollowJointTrajectoryUntil.Goal.TOOL_CONTACT
+        )
+        self._send_goal_future = self._action_client.send_goal_async(goal)
+        rclpy.spin_until_future_complete(self, self._send_goal_future)
+        self._goal_handle = self._send_goal_future.result()
+        if not self._goal_handle.accepted:
+            self.get_logger().error("Goal rejected :(")
+            raise RuntimeError("Goal rejected :(")
+
+        self.get_logger().info(
+            f"Goal accepted with ID: {bytes(self._goal_handle.goal_id.uuid).hex()}\n"
+        )
+
+        result_future = self._goal_handle.get_result_async()
+        rclpy.spin_until_future_complete(self, result_future)
+        result = result_future.result().result
+        print(result)
+        if result is None:
+            self.get_logger().error("Result is None")
+            return
+        if result.error_code != FollowJointTrajectoryUntil.Result.SUCCESSFUL:
+            self.get_logger().error(f"Result error code: {result.error_code}")
+            return
+        if result.until_condition_result == FollowJointTrajectoryUntil.Result.NOT_TRIGGERED:
+            self.get_logger().info("Trajectory executed without tool contact trigger")
+        else:
+            self.get_logger().info("Trajectory executed with tool contact trigger")
+
+        self.get_logger().info("Trajectory executed successfully with tool contact condition")
+
+
+if __name__ == "__main__":
+    rclpy.init()
+
+    node = MoveUntilExample()
+    try:
+        node.process()
+    except KeyboardInterrupt:
+        node.get_logger().info("Interrupted")
+        node.cancel_goal()
+        time.sleep(2)
+
+    rclpy.shutdown()
diff --git a/ur_robot_driver/include/ur_robot_driver/trajectory_until_node.hpp b/ur_robot_driver/include/ur_robot_driver/trajectory_until_node.hpp
@@ -0,0 +1,140 @@
+// Copyright 2025, 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 jala@universal-robots.com
+ * \date    2025-01-17
+ *
+ *
+ *
+ *
+ */
+//----------------------------------------------------------------------
+
+#ifndef UR_ROBOT_DRIVER__TRAJECTORY_UNTIL_NODE_HPP_
+#define UR_ROBOT_DRIVER__TRAJECTORY_UNTIL_NODE_HPP_
+
+#include <memory>
+#include <string>
+#include <variant>
+
+#include <rclcpp/rclcpp.hpp>
+#include <std_msgs/msg/bool.hpp>
+#include <rclcpp_action/server.hpp>
+#include <rclcpp_action/rclcpp_action.hpp>
+#include <rclcpp_action/create_server.hpp>
+#include <rclcpp_action/server_goal_handle.hpp>
+#include <rclcpp/duration.hpp>
+#include <ur_msgs/action/tool_contact.hpp>
+#include <ur_msgs/action/follow_joint_trajectory_until.hpp>
+#include <control_msgs/action/follow_joint_trajectory.hpp>
+
+namespace ur_robot_driver
+{
+class TrajectoryUntilNode : public rclcpp::Node
+{
+public:
+  explicit TrajectoryUntilNode(const rclcpp::NodeOptions& options = rclcpp::NodeOptions());
+  ~TrajectoryUntilNode();
+
+private:
+  using TrajectoryUntilAction = ur_msgs::action::FollowJointTrajectoryUntil;
+
+  rclcpp_action::Server<TrajectoryUntilAction>::SharedPtr action_server_;
+  rclcpp_action::Client<control_msgs::action::FollowJointTrajectory>::SharedPtr trajectory_action_client_;
+  std::string trajectory_action_uri_;
+
+  // Add new until types here, when available
+  using TCClient = rclcpp_action::Client<ur_msgs::action::ToolContact>::SharedPtr;
+  std::variant<TCClient> until_action_client_variant;
+  std::string until_action_uri_;
+
+  rclcpp::CallbackGroup::SharedPtr server_callback_group;
+  rclcpp::CallbackGroup::SharedPtr clients_callback_group;
+
+  std::shared_ptr<rclcpp_action::ServerGoalHandle<TrajectoryUntilAction>> server_goal_handle_;
+
+  template <class ActionType, class ClientType>
+  void send_until_goal(std::shared_ptr<const TrajectoryUntilAction::Goal> goal);
+
+  template <class T>
+  void until_response_callback(const typename rclcpp_action::ClientGoalHandle<T>::SharedPtr& goal_handle);
+
+  template <class T>
+  void until_result_callback(const typename rclcpp_action::ClientGoalHandle<T>::WrappedResult& result);
+
+  void cancel_until_goal();
+
+  void trajectory_response_callback(
+      const rclcpp_action::ClientGoalHandle<control_msgs::action::FollowJointTrajectory>::SharedPtr& goal_handle);
+  void trajectory_feedback_callback(
+      const rclcpp_action::ClientGoalHandle<control_msgs::action::FollowJointTrajectory>::SharedPtr& goal_handle,
+      const std::shared_ptr<const control_msgs::action::FollowJointTrajectory::Feedback> feedback);
+  void trajectory_result_callback(
+      const rclcpp_action::ClientGoalHandle<control_msgs::action::FollowJointTrajectory>::WrappedResult& result);
+
+  bool assign_until_action_client(std::shared_ptr<const TrajectoryUntilAction::Goal> goal);
+
+  rclcpp_action::GoalResponse goal_received_callback(const rclcpp_action::GoalUUID& uuid,
+                                                     std::shared_ptr<const TrajectoryUntilAction::Goal> goal);
+  void
+  goal_accepted_callback(const std::shared_ptr<rclcpp_action::ServerGoalHandle<TrajectoryUntilAction>> goal_handle);
+  rclcpp_action::CancelResponse
+  goal_cancelled_callback(const std::shared_ptr<rclcpp_action::ServerGoalHandle<TrajectoryUntilAction>> goal_handle);
+
+  void send_trajectory_goal(std::shared_ptr<const TrajectoryUntilAction::Goal> goal);
+
+  void report_goal(rclcpp_action::ClientGoalHandle<control_msgs::action::FollowJointTrajectory>::WrappedResult result);
+
+  template <typename UntilResult>
+  void report_goal(UntilResult result);
+
+  void reset_node();
+
+  rclcpp_action::ClientGoalHandle<control_msgs::action::FollowJointTrajectory>::SharedPtr
+      current_trajectory_goal_handle_;
+  rclcpp_action::ClientGoalHandle<ur_msgs::action::ToolContact>::SharedPtr current_until_goal_handle_;
+
+  std::shared_ptr<TrajectoryUntilAction::Result> prealloc_res_;
+
+  std::shared_ptr<TrajectoryUntilAction::Feedback> prealloc_fb_;
+
+  std::atomic<bool> trajectory_accepted_;
+  std::atomic<bool> until_accepted_;
+
+  std::condition_variable cv_until_;
+  std::condition_variable cv_trajectory_;
+  std::mutex mutex_until;
+  std::mutex mutex_trajectory;
+};
+
+}  // namespace ur_robot_driver
+
+#endif  // UR_ROBOT_DRIVER__TRAJECTORY_UNTIL_NODE_HPP_
diff --git a/ur_robot_driver/launch/ur_control.launch.py b/ur_robot_driver/launch/ur_control.launch.py
@@ -330,6 +330,19 @@ def launch_setup(context, *args, **kwargs):
         arguments=["-d", rviz_config_file],
     )
 
+    trajectory_until_node = Node(
+        package="ur_robot_driver",
+        executable="trajectory_until_node",
+        name="trajectory_until_node",
+        output="screen",
+        parameters=[
+            {
+                "motion_controller_uri": f"/{initial_joint_controller.perform(context)}/follow_joint_trajectory",
+                "until_action_uri": "tool_contact_controller/detect_tool_contact",
+            },
+        ],
+    )
+
     # Spawn controllers
     def controller_spawner(controllers, active=True):
         inactive_flags = ["--inactive"] if not active else []
@@ -386,6 +399,7 @@ def controller_spawner(controllers, active=True):
         urscript_interface,
         robot_state_publisher_node,
         rviz_node,
+        trajectory_until_node,
     ] + controller_spawners
 
     return nodes_to_start
diff --git a/ur_robot_driver/src/trajectory_until_node.cpp b/ur_robot_driver/src/trajectory_until_node.cpp
diff --git a/ur_robot_driver/test/integration_test_trajectory_until.py b/ur_robot_driver/test/integration_test_trajectory_until.py
diff --git a/ur_robot_driver/test/test_common.py b/ur_robot_driver/test/test_common.py
