From 78020666bf22b69b4287259f023864dcc6f17780 Mon Sep 17 00:00:00 2001 From: "mergify[bot]" <37929162+mergify[bot]@users.noreply.github.com> Date: Wed, 25 Sep 2024 15:02:38 +0200 Subject: [PATCH 001/133] Auto-update pre-commit hooks (backport of #1094) --- .pre-commit-config.yaml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml index 7393b848f..72f0efc09 100644 --- a/.pre-commit-config.yaml +++ b/.pre-commit-config.yaml @@ -120,7 +120,7 @@ repos: # Docs - RestructuredText hooks - repo: https://github.com/PyCQA/doc8 - rev: 0.9.0a1 + rev: v1.1.2 hooks: - id: doc8 args: ['--max-line-length=100', '--ignore=D001'] From 72087b1a5822b10a9d642f32fe0069083553f3c1 Mon Sep 17 00:00:00 2001 From: "mergify[bot]" <37929162+mergify[bot]@users.noreply.github.com> Date: Tue, 1 Oct 2024 12:16:07 +0200 Subject: [PATCH 002/133] Assure the description is loaded as string (backport of #1107) If this isn't explicitly specified, the description string might be interpreted as a yaml content, which leads to problems, obviously. --- ur_robot_driver/launch/ur_control.launch.py | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/ur_robot_driver/launch/ur_control.launch.py b/ur_robot_driver/launch/ur_control.launch.py index bbcdd5fd0..9c1a036a7 100644 --- a/ur_robot_driver/launch/ur_control.launch.py +++ b/ur_robot_driver/launch/ur_control.launch.py @@ -30,7 +30,7 @@ # Author: Denis Stogl from launch_ros.actions import Node -from launch_ros.parameter_descriptions import ParameterFile +from launch_ros.parameter_descriptions import ParameterFile, ParameterValue from launch_ros.substitutions import FindPackageShare from launch import LaunchDescription @@ -200,7 +200,9 @@ def launch_setup(context, *args, **kwargs): " ", ] ) - robot_description = {"robot_description": robot_description_content} + robot_description = { + "robot_description": ParameterValue(value=robot_description_content, value_type=str) + } initial_joint_controllers = PathJoinSubstitution( [FindPackageShare(runtime_config_package), "config", controllers_file] From 75c37f1f6eb3c1ae2541df7d0e4177d651db7b2d Mon Sep 17 00:00:00 2001 From: "mergify[bot]" <37929162+mergify[bot]@users.noreply.github.com> Date: Mon, 7 Oct 2024 10:26:21 +0200 Subject: [PATCH 003/133] Auto-update pre-commit hooks (backport of #1118) --- .pre-commit-config.yaml | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml index 72f0efc09..cc6a89647 100644 --- a/.pre-commit-config.yaml +++ b/.pre-commit-config.yaml @@ -15,7 +15,7 @@ repos: # Standard hooks - repo: https://github.com/pre-commit/pre-commit-hooks - rev: v3.4.0 + rev: v5.0.0 hooks: - id: check-added-large-files - id: check-ast @@ -69,7 +69,7 @@ repos: - id: ament_cppcheck name: ament_cppcheck description: Static code analysis of C/C++ files. - stages: [commit] + stages: [pre-commit] entry: env AMENT_CPPCHECK_ALLOW_SLOW_VERSIONS=1 ament_cppcheck language: system files: \.(h\+\+|h|hh|hxx|hpp|cuh|c|cc|cpp|cu|c\+\+|cxx|tpp|txx)$ @@ -80,7 +80,7 @@ repos: - id: ament_cpplint name: ament_cpplint description: Static code analysis of C/C++ files. - stages: [commit] + stages: [pre-commit] entry: ament_cpplint language: system files: \.(h\+\+|h|hh|hxx|hpp|cuh|c|cc|cpp|cu|c\+\+|cxx|tpp|txx)$ @@ -102,7 +102,7 @@ repos: - id: ament_lint_cmake name: ament_lint_cmake description: Check format of CMakeLists.txt files. - stages: [commit] + stages: [pre-commit] entry: ament_lint_cmake language: system files: CMakeLists.txt$ @@ -113,7 +113,7 @@ repos: - id: ament_copyright name: ament_copyright description: Check if copyright notice is available in all files. - stages: [commit] + stages: [pre-commit] entry: ament_copyright language: system args: ['--exclude', 'ur_robot_driver/doc/conf.py', 'ur_calibration/doc/conf.py'] From 6be649f6e10aa034e1259bb5fd89deeab923d632 Mon Sep 17 00:00:00 2001 From: "mergify[bot]" <37929162+mergify[bot]@users.noreply.github.com> Date: Tue, 8 Oct 2024 12:48:59 +0200 Subject: [PATCH 004/133] Service to get software version of robot (backport of #964) * Implemented get_version service * Implemented test of get_version service and integrated it with the test of tool contact * Update ur_robot_driver/test/test_common.py Co-authored-by: Felix Exner (fexner) * Renamed service to "GetRobotSoftwareVersion" everywhere. Also moved the version information from being stored in the command interface to be stored in the state interface * Remove tool contact from test. * Implemented test of get_version service and integrated it with the test of tool contact * Renamed service to "GetRobotSoftwareVersion" everywhere. Also moved the version information from being stored in the command interface to be stored in the state interface * Remove tool contact from test. * Create new URConfigurationController And moved the get_robot_software_version service in to it. * Make configuration controller thread safe Also minor cleanup and add testing of the robot software version service * Use ptr-safe RealTimeBoxBestEffort the RealTimeBox used before is not really real-time safe and the way it was implemented there was unnecessary data allocation in both, the activate method and the service callback. Using the RealTimeBoxBestEffort makes allocating additional memory unnecessary and makes things really thread-safe. * Added back files that were mistakenly deleted --- ur_controllers/CMakeLists.txt | 9 +- ur_controllers/controller_plugins.xml | 5 + .../ur_configuration_controller.hpp | 105 ++++++++++++++ .../src/ur_configuration_controller.cpp | 128 ++++++++++++++++++ ...r_configuration_controller_parameters.yaml | 6 + ur_robot_driver/config/ur_controllers.yaml | 6 + .../ur_robot_driver/hardware_interface.hpp | 4 + ur_robot_driver/launch/ur_control.launch.py | 2 + ur_robot_driver/src/hardware_interface.cpp | 20 +++ ur_robot_driver/test/robot_driver.py | 7 + ur_robot_driver/test/test_common.py | 15 +- 11 files changed, 304 insertions(+), 3 deletions(-) create mode 100644 ur_controllers/include/ur_controllers/ur_configuration_controller.hpp create mode 100644 ur_controllers/src/ur_configuration_controller.cpp create mode 100644 ur_controllers/src/ur_configuration_controller_parameters.yaml diff --git a/ur_controllers/CMakeLists.txt b/ur_controllers/CMakeLists.txt index a2b72e599..5ea7bbe86 100644 --- a/ur_controllers/CMakeLists.txt +++ b/ur_controllers/CMakeLists.txt @@ -54,10 +54,16 @@ generate_parameter_library( src/scaled_joint_trajectory_controller_parameters.yaml ) +generate_parameter_library( + ur_configuration_controller_parameters + src/ur_configuration_controller_parameters.yaml +) + add_library(${PROJECT_NAME} SHARED src/scaled_joint_trajectory_controller.cpp src/speed_scaling_state_broadcaster.cpp - src/gpio_controller.cpp) + src/gpio_controller.cpp + src/ur_configuration_controller.cpp) target_include_directories(${PROJECT_NAME} PRIVATE include @@ -66,6 +72,7 @@ target_link_libraries(${PROJECT_NAME} gpio_controller_parameters speed_scaling_state_broadcaster_parameters scaled_joint_trajectory_controller_parameters + ur_configuration_controller_parameters ) ament_target_dependencies(${PROJECT_NAME} ${THIS_PACKAGE_INCLUDE_DEPENDS} diff --git a/ur_controllers/controller_plugins.xml b/ur_controllers/controller_plugins.xml index f0058ab55..fa4b63987 100644 --- a/ur_controllers/controller_plugins.xml +++ b/ur_controllers/controller_plugins.xml @@ -14,4 +14,9 @@ This controller publishes the Tool IO. + + + Controller used to get and change the configuration of the robot + + diff --git a/ur_controllers/include/ur_controllers/ur_configuration_controller.hpp b/ur_controllers/include/ur_controllers/ur_configuration_controller.hpp new file mode 100644 index 000000000..3775ee72c --- /dev/null +++ b/ur_controllers/include/ur_controllers/ur_configuration_controller.hpp @@ -0,0 +1,105 @@ +// 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 jala@universal-robots.com + * \date 2024-07-11 + * + * + * + * + */ +//---------------------------------------------------------------------- + +#ifndef UR_CONTROLLERS__UR_CONFIGURATION_CONTROLLER_HPP_ +#define UR_CONTROLLERS__UR_CONFIGURATION_CONTROLLER_HPP_ + +// TODO(fmauch): Currently, the realtime_box_best_effort doesn't include this +#include +#include // NOLINT + +#include + +#include + +#include "ur_msgs/srv/get_robot_software_version.hpp" +#include "ur_configuration_controller_parameters.hpp" + +namespace ur_controllers +{ + +// Struct to hold version information +struct VersionInformation +{ + uint32_t major = 0, minor = 0, build = 0, bugfix = 0; +}; + +// Enum for indexing into state interfaces. +enum StateInterfaces +{ + ROBOT_VERSION_MAJOR = 0, + ROBOT_VERSION_MINOR = 1, + ROBOT_VERSION_BUILD = 2, + ROBOT_VERSION_BUGFIX = 3, +}; + +class URConfigurationController : public controller_interface::ControllerInterface +{ +public: + controller_interface::InterfaceConfiguration command_interface_configuration() const override; + + controller_interface::InterfaceConfiguration state_interface_configuration() const override; + + controller_interface::return_type update(const rclcpp::Time& time, const rclcpp::Duration& period) override; + + CallbackReturn on_configure(const rclcpp_lifecycle::State& previous_state) override; + + CallbackReturn on_activate(const rclcpp_lifecycle::State& previous_state) override; + + CallbackReturn on_deactivate(const rclcpp_lifecycle::State& previous_state) override; + + CallbackReturn on_init() override; + +private: + realtime_tools::RealtimeBoxBestEffort> robot_software_version_{ + std::make_shared() + }; + + rclcpp::Service::SharedPtr get_robot_software_version_srv_; + + bool getRobotSoftwareVersion(ur_msgs::srv::GetRobotSoftwareVersion::Request::SharedPtr req, + ur_msgs::srv::GetRobotSoftwareVersion::Response::SharedPtr resp); + + std::shared_ptr param_listener_; + ur_configuration_controller::Params params_; +}; +} // namespace ur_controllers + +#endif // UR_CONTROLLERS__UR_CONFIGURATION_CONTROLLER_HPP_ diff --git a/ur_controllers/src/ur_configuration_controller.cpp b/ur_controllers/src/ur_configuration_controller.cpp new file mode 100644 index 000000000..a6ec2d24b --- /dev/null +++ b/ur_controllers/src/ur_configuration_controller.cpp @@ -0,0 +1,128 @@ +// 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 jala@universal-robots.com + * \date 2024-07-11 + * + * + * + * + */ +//---------------------------------------------------------------------- + +#include +#include +namespace ur_controllers +{ + +controller_interface::CallbackReturn URConfigurationController::on_init() +{ + return controller_interface::CallbackReturn::SUCCESS; +} + +controller_interface::CallbackReturn +URConfigurationController::on_configure(const rclcpp_lifecycle::State& /* previous_state */) +{ + param_listener_ = std::make_shared(get_node()); + params_ = param_listener_->get_params(); + + get_robot_software_version_srv_ = get_node()->create_service( + "~/get_robot_software_version", std::bind(&URConfigurationController::getRobotSoftwareVersion, this, + std::placeholders::_1, std::placeholders::_2)); + + return controller_interface::CallbackReturn::SUCCESS; +} + +controller_interface::InterfaceConfiguration URConfigurationController::command_interface_configuration() const +{ + // No command interfaces currently + controller_interface::InterfaceConfiguration config; + config.type = controller_interface::interface_configuration_type::INDIVIDUAL; + + return config; +} + +controller_interface::InterfaceConfiguration URConfigurationController::state_interface_configuration() const +{ + controller_interface::InterfaceConfiguration config; + config.type = controller_interface::interface_configuration_type::INDIVIDUAL; + + const std::string tf_prefix = params_.tf_prefix; + + config.names.emplace_back(tf_prefix + "get_robot_software_version/get_version_major"); + config.names.emplace_back(tf_prefix + "get_robot_software_version/get_version_minor"); + config.names.emplace_back(tf_prefix + "get_robot_software_version/get_version_build"); + config.names.emplace_back(tf_prefix + "get_robot_software_version/get_version_bugfix"); + + return config; +} + +controller_interface::return_type URConfigurationController::update(const rclcpp::Time& /* time */, + const rclcpp::Duration& /* period */) +{ + return controller_interface::return_type::OK; +} + +controller_interface::CallbackReturn +URConfigurationController::on_activate(const rclcpp_lifecycle::State& /* previous_state */) +{ + robot_software_version_.set([this](const std::shared_ptr ptr) { + ptr->major = state_interfaces_[StateInterfaces::ROBOT_VERSION_MAJOR].get_value(); + ptr->minor = state_interfaces_[StateInterfaces::ROBOT_VERSION_MINOR].get_value(); + ptr->build = state_interfaces_[StateInterfaces::ROBOT_VERSION_BUILD].get_value(); + ptr->bugfix = state_interfaces_[StateInterfaces::ROBOT_VERSION_BUGFIX].get_value(); + }); + return controller_interface::CallbackReturn::SUCCESS; +} + +controller_interface::CallbackReturn +URConfigurationController::on_deactivate(const rclcpp_lifecycle::State& /* previous_state */) +{ + return controller_interface::CallbackReturn::SUCCESS; +} + +bool URConfigurationController::getRobotSoftwareVersion( + ur_msgs::srv::GetRobotSoftwareVersion::Request::SharedPtr /*req*/, + ur_msgs::srv::GetRobotSoftwareVersion::Response::SharedPtr resp) +{ + std::shared_ptr temp; + return robot_software_version_.tryGet([resp](const std::shared_ptr ptr) { + resp->major = ptr->major; + resp->minor = ptr->minor; + resp->build = ptr->build; + resp->bugfix = ptr->bugfix; + }); +} +} // namespace ur_controllers + +#include "pluginlib/class_list_macros.hpp" + +PLUGINLIB_EXPORT_CLASS(ur_controllers::URConfigurationController, controller_interface::ControllerInterface) diff --git a/ur_controllers/src/ur_configuration_controller_parameters.yaml b/ur_controllers/src/ur_configuration_controller_parameters.yaml new file mode 100644 index 000000000..4cbdf8aa1 --- /dev/null +++ b/ur_controllers/src/ur_configuration_controller_parameters.yaml @@ -0,0 +1,6 @@ +ur_configuration_controller: + tf_prefix: { + type: string, + default_value: "", + description: "URDF prefix of the corresponding arm" + } diff --git a/ur_robot_driver/config/ur_controllers.yaml b/ur_robot_driver/config/ur_controllers.yaml index a512dc1ca..544dd1fbd 100644 --- a/ur_robot_driver/config/ur_controllers.yaml +++ b/ur_robot_driver/config/ur_controllers.yaml @@ -24,6 +24,8 @@ controller_manager: forward_position_controller: type: position_controllers/JointGroupPositionController + ur_configuration_controller: + type: ur_controllers/URConfigurationController speed_scaling_state_broadcaster: ros__parameters: @@ -34,6 +36,10 @@ io_and_status_controller: ros__parameters: tf_prefix: "$(var tf_prefix)" +ur_configuration_controller: + ros__parameters: + tf_prefix: "$(var tf_prefix)" + force_torque_sensor_broadcaster: ros__parameters: sensor_name: $(var tf_prefix)tcp_fts_sensor diff --git a/ur_robot_driver/include/ur_robot_driver/hardware_interface.hpp b/ur_robot_driver/include/ur_robot_driver/hardware_interface.hpp index 67e27a222..6fc227766 100644 --- a/ur_robot_driver/include/ur_robot_driver/hardware_interface.hpp +++ b/ur_robot_driver/include/ur_robot_driver/hardware_interface.hpp @@ -191,6 +191,10 @@ class URPositionHardwareInterface : public hardware_interface::SystemInterface bool initialized_; double system_interface_initialized_; bool async_thread_shutdown_; + double get_robot_software_version_major_; + double get_robot_software_version_minor_; + double get_robot_software_version_bugfix_; + double get_robot_software_version_build_; // payload stuff urcl::vector3d_t payload_center_of_gravity_; diff --git a/ur_robot_driver/launch/ur_control.launch.py b/ur_robot_driver/launch/ur_control.launch.py index 9c1a036a7..053ed4996 100644 --- a/ur_robot_driver/launch/ur_control.launch.py +++ b/ur_robot_driver/launch/ur_control.launch.py @@ -295,6 +295,7 @@ def launch_setup(context, *args, **kwargs): "force_torque_sensor_broadcaster", "joint_state_broadcaster", "speed_scaling_state_broadcaster", + "ur_configuration_controller", ] }, ], @@ -337,6 +338,7 @@ def controller_spawner(controllers, active=True): "io_and_status_controller", "speed_scaling_state_broadcaster", "force_torque_sensor_broadcaster", + "ur_configuration_controller", ] controllers_inactive = ["forward_position_controller"] diff --git a/ur_robot_driver/src/hardware_interface.cpp b/ur_robot_driver/src/hardware_interface.cpp index 6404e69fc..b682bfd9b 100644 --- a/ur_robot_driver/src/hardware_interface.cpp +++ b/ur_robot_driver/src/hardware_interface.cpp @@ -44,6 +44,7 @@ #include "ur_client_library/exceptions.h" #include "ur_client_library/ur/tool_communication.h" +#include "ur_client_library/ur/version_information.h" #include "rclcpp/rclcpp.hpp" #include "hardware_interface/types/hardware_interface_type_values.hpp" @@ -231,6 +232,18 @@ std::vector URPositionHardwareInterface::exp state_interfaces.emplace_back( hardware_interface::StateInterface(tf_prefix + "gpio", "program_running", &robot_program_running_copy_)); + state_interfaces.emplace_back(hardware_interface::StateInterface( + tf_prefix + "get_robot_software_version", "get_version_major", &get_robot_software_version_major_)); + + state_interfaces.emplace_back(hardware_interface::StateInterface( + tf_prefix + "get_robot_software_version", "get_version_minor", &get_robot_software_version_minor_)); + + state_interfaces.emplace_back(hardware_interface::StateInterface( + tf_prefix + "get_robot_software_version", "get_version_bugfix", &get_robot_software_version_bugfix_)); + + state_interfaces.emplace_back(hardware_interface::StateInterface( + tf_prefix + "get_robot_software_version", "get_version_build", &get_robot_software_version_build_)); + return state_interfaces; } @@ -454,6 +467,13 @@ URPositionHardwareInterface::on_configure(const rclcpp_lifecycle::State& previou "README.md] for details."); } + // Export version information to state interfaces + urcl::VersionInformation version_info = ur_driver_->getVersion(); + get_robot_software_version_major_ = version_info.major; + get_robot_software_version_minor_ = version_info.minor; + get_robot_software_version_build_ = version_info.build; + get_robot_software_version_bugfix_ = version_info.bugfix; + async_thread_ = std::make_shared(&URPositionHardwareInterface::asyncThread, this); RCLCPP_INFO(rclcpp::get_logger("URPositionHardwareInterface"), "System successfully started!"); diff --git a/ur_robot_driver/test/robot_driver.py b/ur_robot_driver/test/robot_driver.py index 96c859947..88fcffce3 100644 --- a/ur_robot_driver/test/robot_driver.py +++ b/ur_robot_driver/test/robot_driver.py @@ -49,6 +49,7 @@ ControllerManagerInterface, DashboardInterface, IoStatusInterface, + ConfigurationInterface, generate_driver_test_description, ) @@ -92,6 +93,7 @@ def init_robot(self): self._dashboard_interface = DashboardInterface(self.node) self._controller_manager_interface = ControllerManagerInterface(self.node) self._io_status_controller_interface = IoStatusInterface(self.node) + self._configuration_controller_interface = ConfigurationInterface(self.node) self._scaled_follow_joint_trajectory = ActionInterface( self.node, @@ -108,6 +110,11 @@ def setUp(self): # Test functions # + def test_get_robot_software_version(self): + self.assertNotEqual( + self._configuration_controller_interface.get_robot_software_version().major, 0 + ) + def test_start_scaled_jtc_controller(self): self.assertTrue( self._controller_manager_interface.switch_controller( diff --git a/ur_robot_driver/test/test_common.py b/ur_robot_driver/test/test_common.py index f1eac75af..8db7b6835 100644 --- a/ur_robot_driver/test/test_common.py +++ b/ur_robot_driver/test/test_common.py @@ -52,7 +52,7 @@ IsProgramRunning, Load, ) -from ur_msgs.srv import SetIO +from ur_msgs.srv import SetIO, GetRobotSoftwareVersion TIMEOUT_WAIT_SERVICE = 10 TIMEOUT_WAIT_SERVICE_INITIAL = 120 # If we download the docker image simultaneously to the tests, it can take quite some time until the dashboard server is reachable and usable. @@ -240,7 +240,18 @@ class IoStatusInterface( _ServiceInterface, namespace="/io_and_status_controller", initial_services={"set_io": SetIO}, - services={"resend_robot_program": Trigger}, + services={ + "resend_robot_program": Trigger, + }, +): + pass + + +class ConfigurationInterface( + _ServiceInterface, + namespace="/ur_configuration_controller", + initial_services={"get_robot_software_version": GetRobotSoftwareVersion}, + services={}, ): pass From aca2a0e0baff7fcf2269f68f5a9e73e7ccb6f568 Mon Sep 17 00:00:00 2001 From: "Felix Exner (fexner)" Date: Tue, 8 Oct 2024 13:43:00 +0200 Subject: [PATCH 005/133] Update ur_msgs upstream branch (#1126) --- Universal_Robots_ROS2_Driver.humble.repos | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Universal_Robots_ROS2_Driver.humble.repos b/Universal_Robots_ROS2_Driver.humble.repos index 61ee8ef3a..14de188d5 100644 --- a/Universal_Robots_ROS2_Driver.humble.repos +++ b/Universal_Robots_ROS2_Driver.humble.repos @@ -10,7 +10,7 @@ repositories: ur_msgs: type: git url: https://github.com/ros-industrial/ur_msgs.git - version: foxy-devel + version: humble-devel ros2_control: type: git url: https://github.com/ros-controls/ros2_control.git From 1a5c861f57bac8b187efc1d140180a0a86ba64d1 Mon Sep 17 00:00:00 2001 From: "Felix Exner (fexner)" Date: Thu, 10 Oct 2024 11:42:44 +0200 Subject: [PATCH 006/133] [moveit] Disable execution_duration_monitoring by default (#1133) * [moveit] Disable execution_duration_monitoring by default * Add note about disabling TEM in docs * Update documentation --- ur_moveit_config/launch/ur_moveit.launch.py | 2 ++ ur_robot_driver/doc/usage.rst | 20 ++++++++++++++++++++ 2 files changed, 22 insertions(+) diff --git a/ur_moveit_config/launch/ur_moveit.launch.py b/ur_moveit_config/launch/ur_moveit.launch.py index 4226dfa52..dba83fc74 100644 --- a/ur_moveit_config/launch/ur_moveit.launch.py +++ b/ur_moveit_config/launch/ur_moveit.launch.py @@ -188,6 +188,8 @@ def launch_setup(context, *args, **kwargs): "trajectory_execution.allowed_execution_duration_scaling": 1.2, "trajectory_execution.allowed_goal_duration_margin": 0.5, "trajectory_execution.allowed_start_tolerance": 0.01, + # Execution time monitoring can be incompatible with the scaled JTC + "trajectory_execution.execution_duration_monitoring": False, } planning_scene_monitor_parameters = { diff --git a/ur_robot_driver/doc/usage.rst b/ur_robot_driver/doc/usage.rst index 014c0fd23..208177d8d 100644 --- a/ur_robot_driver/doc/usage.rst +++ b/ur_robot_driver/doc/usage.rst @@ -191,6 +191,26 @@ To test the driver with the example MoveIt-setup, first start the driver as desc Now you should be able to use the MoveIt Plugin in rviz2 to plan and execute trajectories with the robot as explained `here `_. +.. note:: + The MoveIt configuration provided here has Trajectory Execution Monitoring (TEM) *disabled*, as the + Scaled Joint Trajectory Controller may cause trajectories to be executed at a lower velocity + than they were originally planned by MoveIt. MoveIt's TEM however is not aware of this + deliberate slow-down due to scaling and will in most cases unnecessarily (and unexpectedly) + abort goals. + + Until this incompatibility is resolved, the default value for ``execution_duration_monitoring`` + is set to ``false``. Users who wish to temporarily (re)enable TEM at runtime (for use with + other, non-scaling controllers) can do so using the ROS 2 parameter services supported by + MoveIt. + + .. literalinclude:: ../../ur_moveit_config/launch/ur_moveit.launch.py + :language: python + :start-at: trajectory_execution = + :end-at: execution_duration_monitoring": False + :append: } + :dedent: 4 + :caption: ur_moveit_config/launch/ur_moveit.launch.py + Fake hardware on ROS2 Galactic ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ From 60b09db5ddcd22a476d56765ff84632836ff6ba8 Mon Sep 17 00:00:00 2001 From: "mergify[bot]" <37929162+mergify[bot]@users.noreply.github.com> Date: Wed, 16 Oct 2024 09:30:49 +0200 Subject: [PATCH 007/133] README: Center family photo (backport pf #1122) Increased size of family photo (cherry picked from commit 980e7fbf9940870e1d9b35944c2a84a51eb0dc39) Co-authored-by: URJala <159417921+URJala@users.noreply.github.com> --- README.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/README.md b/README.md index 2d50dff45..432604d11 100644 --- a/README.md +++ b/README.md @@ -1,7 +1,7 @@ # Universal Robots ROS2 Driver Universal Robots has become a dominant supplier of lightweight, robotic manipulators for industry, as well as for scientific research and education. -
Universal Robot family
+
Universal Robot family
This is one of the very first ROS2 manipulator drivers. Some of the new features are enabled by ROS2 and include decreased latency, improved security, and more flexibility regarding middleware configuration. The package contains launch files to quickly get started using the driver as a standalone version or in combination with MoveIt2 From 255355d7876f1cec996f957107c548f9848cf7ed Mon Sep 17 00:00:00 2001 From: "mergify[bot]" <37929162+mergify[bot]@users.noreply.github.com> Date: Wed, 16 Oct 2024 09:32:15 +0200 Subject: [PATCH 008/133] Auto-update pre-commit hooks (backport of #1147) --- .pre-commit-config.yaml | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml index cc6a89647..c83afa27e 100644 --- a/.pre-commit-config.yaml +++ b/.pre-commit-config.yaml @@ -33,13 +33,13 @@ repos: # Python hooks - repo: https://github.com/asottile/pyupgrade - rev: v2.7.2 + rev: v3.18.0 hooks: - id: pyupgrade args: [--py36-plus] - repo: https://github.com/psf/black - rev: 22.3.0 + rev: 24.10.0 hooks: - id: black args: ["--line-length=100"] From eea73a0f45857f7678b02e20ae90e175e15209f8 Mon Sep 17 00:00:00 2001 From: "mergify[bot]" <37929162+mergify[bot]@users.noreply.github.com> Date: Tue, 22 Oct 2024 13:56:34 +0200 Subject: [PATCH 009/133] [moveit] Properly handle use_sim_time (#1146) (#1159) Pass use_sim_time also to RViz and use non-scaled JTC when use_sim_time is active. (cherry picked from commit 9c33a93c4f0c8650f5fd868f9960753f3f4fc133) Co-authored-by: Felix Exner (fexner) --- ur_moveit_config/launch/ur_moveit.launch.py | 15 +++++++++++++-- 1 file changed, 13 insertions(+), 2 deletions(-) diff --git a/ur_moveit_config/launch/ur_moveit.launch.py b/ur_moveit_config/launch/ur_moveit.launch.py index dba83fc74..b1858d519 100644 --- a/ur_moveit_config/launch/ur_moveit.launch.py +++ b/ur_moveit_config/launch/ur_moveit.launch.py @@ -38,7 +38,13 @@ from launch import LaunchDescription from launch.actions import DeclareLaunchArgument, OpaqueFunction from launch.conditions import IfCondition -from launch.substitutions import Command, FindExecutable, LaunchConfiguration, PathJoinSubstitution +from launch.substitutions import ( + Command, + FindExecutable, + LaunchConfiguration, + PathJoinSubstitution, + OrSubstitution, +) def launch_setup(context, *args, **kwargs): @@ -173,7 +179,9 @@ def launch_setup(context, *args, **kwargs): # Trajectory Execution Configuration controllers_yaml = load_yaml("ur_moveit_config", "config/controllers.yaml") # the scaled_joint_trajectory_controller does not work on fake hardware - change_controllers = context.perform_substitution(use_fake_hardware) + change_controllers = context.perform_substitution( + OrSubstitution(use_fake_hardware, use_sim_time) + ) if change_controllers == "true": controllers_yaml["scaled_joint_trajectory_controller"]["default"] = False controllers_yaml["joint_trajectory_controller"]["default"] = True @@ -242,6 +250,9 @@ def launch_setup(context, *args, **kwargs): robot_description_kinematics, robot_description_planning, warehouse_ros_config, + { + "use_sim_time": use_sim_time, + }, ], ) From 81877d4d9876953e905b2c392625cb1e91cdeaa5 Mon Sep 17 00:00:00 2001 From: "mergify[bot]" <37929162+mergify[bot]@users.noreply.github.com> Date: Sat, 26 Oct 2024 14:27:22 +0200 Subject: [PATCH 010/133] Fix component lifecycle (backport of #1098) * Correctly reset async_thread_shutdown when starting the async thread * Do not use a global log handler object --- .../include/ur_robot_driver/hardware_interface.hpp | 2 +- ur_robot_driver/src/hardware_interface.cpp | 1 + ur_robot_driver/src/urcl_log_handler.cpp | 6 +++--- 3 files changed, 5 insertions(+), 4 deletions(-) diff --git a/ur_robot_driver/include/ur_robot_driver/hardware_interface.hpp b/ur_robot_driver/include/ur_robot_driver/hardware_interface.hpp index 6fc227766..f7167e793 100644 --- a/ur_robot_driver/include/ur_robot_driver/hardware_interface.hpp +++ b/ur_robot_driver/include/ur_robot_driver/hardware_interface.hpp @@ -190,7 +190,7 @@ class URPositionHardwareInterface : public hardware_interface::SystemInterface bool first_pass_; bool initialized_; double system_interface_initialized_; - bool async_thread_shutdown_; + std::atomic_bool async_thread_shutdown_; double get_robot_software_version_major_; double get_robot_software_version_minor_; double get_robot_software_version_bugfix_; diff --git a/ur_robot_driver/src/hardware_interface.cpp b/ur_robot_driver/src/hardware_interface.cpp index b682bfd9b..2852df1b4 100644 --- a/ur_robot_driver/src/hardware_interface.cpp +++ b/ur_robot_driver/src/hardware_interface.cpp @@ -532,6 +532,7 @@ void URPositionHardwareInterface::readBitsetData(const std::unique_ptr g_log_handler(new UrclLogHandler); UrclLogHandler::UrclLogHandler() = default; @@ -82,10 +81,11 @@ void UrclLogHandler::log(const char* file, int line, urcl::LogLevel loglevel, co void registerUrclLogHandler(const std::string& tf_prefix) { if (g_registered == false) { - g_log_handler->setTFPrefix(tf_prefix); + std::unique_ptr log_handler(new UrclLogHandler); + log_handler->setTFPrefix(tf_prefix); // Log level is decided by ROS2 log level urcl::setLogLevel(urcl::LogLevel::DEBUG); - urcl::registerLogHandler(std::move(g_log_handler)); + urcl::registerLogHandler(std::move(log_handler)); g_registered = true; } } From ffabf585b6f005b4815a67237933de6c85c47e7b Mon Sep 17 00:00:00 2001 From: "mergify[bot]" <37929162+mergify[bot]@users.noreply.github.com> Date: Sun, 27 Oct 2024 20:46:07 +0100 Subject: [PATCH 011/133] Auto-update pre-commit hooks (backport of #1164) --- .pre-commit-config.yaml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml index c83afa27e..67ecafab9 100644 --- a/.pre-commit-config.yaml +++ b/.pre-commit-config.yaml @@ -33,7 +33,7 @@ repos: # Python hooks - repo: https://github.com/asottile/pyupgrade - rev: v3.18.0 + rev: v3.19.0 hooks: - id: pyupgrade args: [--py36-plus] From 88275ccc2362f1eb6995a8d1bd5d3ee7ecf69f6c Mon Sep 17 00:00:00 2001 From: "mergify[bot]" <37929162+mergify[bot]@users.noreply.github.com> Date: Mon, 28 Oct 2024 16:34:13 +0100 Subject: [PATCH 012/133] Allow setting the analog output domain when setting an analog output (backport of #1123) --- ur_controllers/doc/index.rst | 137 ++++++++++++++++++ .../ur_controllers/gpio_controller.hpp | 6 + ur_controllers/src/gpio_controller.cpp | 47 +++++- .../ur_robot_driver/hardware_interface.hpp | 1 + ur_robot_driver/src/hardware_interface.cpp | 12 +- 5 files changed, 201 insertions(+), 2 deletions(-) create mode 100644 ur_controllers/doc/index.rst diff --git a/ur_controllers/doc/index.rst b/ur_controllers/doc/index.rst new file mode 100644 index 000000000..bc4d7c147 --- /dev/null +++ b/ur_controllers/doc/index.rst @@ -0,0 +1,137 @@ +ur_controllers +============== + +This package contains controllers and hardware interface for ``ros2_controllers`` that are special to the UR +robot family. Currently this contains: + + +* A **speed_scaling_state_broadcaster** that publishes the current execution speed as reported by + the robot to a topic interface. Values are floating points between 0 and 1. +* A **scaled_joint_trajectory_controller** that is similar to the *joint_trajectory_controller*\ , + but it uses the speed scaling reported to align progress of the trajectory between the robot and controller. +* A **io_and_status_controller** that allows setting I/O ports, controlling some UR-specific + functionality and publishes status information about the robot. + +About this package +------------------ + +This package contains controllers not being available in the default ``ros2_controllers`` set. They are +created to support more features offered by the UR robot family. Some of these controllers are +example implementations for certain features and are intended to be generalized and merged +into the default ``ros2_controllers`` controller set at some future point. + +Controller description +---------------------- + +This packages offers a couple of specific controllers that will be explained in the following +sections. + +.. _speed_scaling_state_broadcaster: + +ur_controllers/SpeedScalingStateBroadcaster +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +This controller publishes the current actual execution speed as reported by the robot. Values are +floating points between 0 and 1. + +In the `ur_robot_driver +`_ +this is calculated by multiplying the two `RTDE +`_ data +fields ``speed_scaling`` (which should be equal to the value shown by the speed slider position on the +teach pendant) and ``target_speed_fraction`` (Which is the fraction to which execution gets slowed +down by the controller). + +.. _scaled_jtc: + +ur_controlers/ScaledJointTrajectoryController +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +These controllers work similar to the well-known +`joint_trajectory_controller `_. + +However, they are extended to handle the robot's execution speed specifically. Because the default +``joint_trajectory_controller`` would interpolate the trajectory with the configured time constraints (ie: always assume maximum velocity and acceleration supported by the robot), +this could lead to significant path deviation due to multiple reasons: + + +* The speed slider on the robot might not be at 100%, so motion commands sent from ROS would + effectively get scaled down resulting in a slower execution. +* The robot could scale down motions based on configured safety limits resulting in a slower motion + than expected and therefore not reaching the desired target in a control cycle. +* Motions might not be executed at all, e.g. because the robot is E-stopped or in a protective stop +* Motion commands sent to the robot might not be interpreted, e.g. because there is no + `external_control `_ + program node running on the robot controller. +* The program interpreting motion commands could be paused. + +The following plot illustrates the problem: + +.. image:: traj_without_speed_scaling.png + :target: traj_without_speed_scaling.png + :alt: Trajectory execution with default trajectory controller + + +The graph shows a trajectory with one joint being moved to a target point and back to its starting +point. As the joint's speed is limited to a very low setting on the teach pendant, speed scaling +(black line) activates and limits the joint speed (green line). As a result, the target +trajectory (light blue) doesn't get executed by the robot, but instead the pink trajectory is executed. +The vertical distance between the light blue line and the pink line is the path error in each +control cycle. We can see that the path deviation gets above 300 degrees at some point and the +target point at -6 radians never gets reached. + +All of the cases mentioned above are addressed by the scaled trajectory versions. Trajectory execution +can be transparently scaled down using the speed slider on the teach pendant without leading to +additional path deviations. Pausing the program or hitting the E-stop effectively leads to +``speed_scaling`` being 0 meaning the trajectory will not be continued until the program is continued. +This way, trajectory executions can be explicitly paused and continued. + +With the scaled version of the trajectory controller the example motion shown in the previous diagram becomes: + +.. image:: traj_with_speed_scaling.png + :target: traj_with_speed_scaling.png + :alt: Trajectory execution with scaled_joint_trajectory_controller + + +The deviation between trajectory interpolation on the ROS side and actual robot execution stays minimal and the +robot reaches the intermediate setpoint instead of returning "too early" as in the example above. + +Under the hood this is implemented by proceeding the trajectory not by a full time step but only by +the fraction determined by the current speed scaling. If speed scaling is currently at 50% then +interpolation of the current control cycle will start half a time step after the beginning of the +previous control cycle. + +.. _io_and_status_controller: + +ur_controllers/GPIOController +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +This controller allows setting I/O ports, controlling some UR-specific functionality and publishes +status information about the robot. + +Published topics +"""""""""""""""" + +* ``~/io_states [ur_msgs/msg/IOStates]``: Status of all I/O ports +* ``~/robot_mode [ur_dashboard_msgs/msg/RobotMode]``: The current robot mode (e.g. ``POWER_OFF``, + ``IDLE``, ``RUNNING``) +* ``~/robot_program_running [std_msgs/msg/Bool]``: Publishes whether **the External Control + program** is running or not. If this is ``false`` no commands can be sent to the robot. +* ``~/safety_mode [ur_dashboard_msgs/msg/SafetyMode]``: The robot's current safety mode (e.g. + ``PROTECTIVE_STOP``, ``ROBOT_EMERGENCY_STOP``, ``NORMAL``) +* ``~/tool_data [ur_msgs/msg/ToolDataMsg]``: Information about the robot's tool configuration + +Advertised services +""""""""""""""""""" + +* ``~/hand_back_control [std_srvs/srv/Trigger]``: Calling this service will make the robot program + exit the *External Control* program node and continue with the rest of the program. +* ``~/resend_robot_program [std_srvs/srv/Trigger]``: When :ref:`headless_mode` is used, this + service can be used to restart the *External Control* program on the robot. +* ``~/set_io [ur_msgs/srv/SetIO]``: Set an output pin on the robot. +* ``~/set_analog_output [ur_msgs/srv/SetAnalogOutput]``: Set an analog output on the robot. This + also allows specifying the domain. +* ``~/set_payload [ur_msgs/srv/SetPayload]``: Change the robot's payload on-the-fly. +* ``~/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. diff --git a/ur_controllers/include/ur_controllers/gpio_controller.hpp b/ur_controllers/include/ur_controllers/gpio_controller.hpp index c20bd680e..b06b5139a 100644 --- a/ur_controllers/include/ur_controllers/gpio_controller.hpp +++ b/ur_controllers/include/ur_controllers/gpio_controller.hpp @@ -51,6 +51,7 @@ #include "ur_dashboard_msgs/msg/robot_mode.hpp" #include "ur_dashboard_msgs/msg/safety_mode.hpp" #include "ur_msgs/srv/set_io.hpp" +#include "ur_msgs/srv/set_analog_output.hpp" #include "ur_msgs/srv/set_speed_slider_fraction.hpp" #include "ur_msgs/srv/set_payload.hpp" #include "rclcpp/time.hpp" @@ -79,6 +80,7 @@ enum CommandInterfaces ZERO_FTSENSOR_ASYNC_SUCCESS = 32, HAND_BACK_CONTROL_CMD = 33, HAND_BACK_CONTROL_ASYNC_SUCCESS = 34, + ANALOG_OUTPUTS_DOMAIN = 35, }; enum StateInterfaces @@ -122,6 +124,9 @@ class GPIOController : public controller_interface::ControllerInterface private: bool setIO(ur_msgs::srv::SetIO::Request::SharedPtr req, ur_msgs::srv::SetIO::Response::SharedPtr resp); + bool setAnalogOutput(ur_msgs::srv::SetAnalogOutput::Request::SharedPtr req, + ur_msgs::srv::SetAnalogOutput::Response::SharedPtr resp); + bool setSpeedSlider(ur_msgs::srv::SetSpeedSliderFraction::Request::SharedPtr req, ur_msgs::srv::SetSpeedSliderFraction::Response::SharedPtr resp); @@ -161,6 +166,7 @@ class GPIOController : public controller_interface::ControllerInterface rclcpp::Service::SharedPtr hand_back_control_srv_; rclcpp::Service::SharedPtr set_speed_slider_srv_; rclcpp::Service::SharedPtr set_io_srv_; + rclcpp::Service::SharedPtr set_analog_output_srv_; rclcpp::Service::SharedPtr set_payload_srv_; rclcpp::Service::SharedPtr tare_sensor_srv_; diff --git a/ur_controllers/src/gpio_controller.cpp b/ur_controllers/src/gpio_controller.cpp index d5e6d16ee..188542d0e 100644 --- a/ur_controllers/src/gpio_controller.cpp +++ b/ur_controllers/src/gpio_controller.cpp @@ -97,6 +97,8 @@ controller_interface::InterfaceConfiguration GPIOController::command_interface_c config.names.emplace_back(tf_prefix + "hand_back_control/hand_back_control_cmd"); config.names.emplace_back(tf_prefix + "hand_back_control/hand_back_control_async_success"); + config.names.emplace_back(tf_prefix + "gpio/analog_output_domain_cmd"); + return config; } @@ -293,6 +295,9 @@ ur_controllers::GPIOController::on_activate(const rclcpp_lifecycle::State& /*pre program_state_pub_ = get_node()->create_publisher("~/robot_program_running", qos_latched); set_io_srv_ = get_node()->create_service( "~/set_io", std::bind(&GPIOController::setIO, this, std::placeholders::_1, std::placeholders::_2)); + set_analog_output_srv_ = get_node()->create_service( + "~/set_analog_output", + std::bind(&GPIOController::setAnalogOutput, this, std::placeholders::_1, std::placeholders::_2)); set_speed_slider_srv_ = get_node()->create_service( "~/set_speed_slider", @@ -357,7 +362,7 @@ bool GPIOController::setIO(ur_msgs::srv::SetIO::Request::SharedPtr req, ur_msgs: command_interfaces_[CommandInterfaces::IO_ASYNC_SUCCESS].set_value(ASYNC_WAITING); command_interfaces_[CommandInterfaces::ANALOG_OUTPUTS_CMD + req->pin].set_value(static_cast(req->state)); - RCLCPP_INFO(get_node()->get_logger(), "Setting analog output '%d' to state: '%1.0f'.", req->pin, req->state); + RCLCPP_INFO(get_node()->get_logger(), "Setting analog output '%d' to state: '%f'.", req->pin, req->state); if (!waitForAsyncCommand([&]() { return command_interfaces_[CommandInterfaces::IO_ASYNC_SUCCESS].get_value(); })) { RCLCPP_WARN(get_node()->get_logger(), "Could not verify that io was set. (This might happen when using the " @@ -385,6 +390,46 @@ bool GPIOController::setIO(ur_msgs::srv::SetIO::Request::SharedPtr req, ur_msgs: } } +bool GPIOController::setAnalogOutput(ur_msgs::srv::SetAnalogOutput::Request::SharedPtr req, + ur_msgs::srv::SetAnalogOutput::Response::SharedPtr resp) +{ + std::string domain_string = "UNKNOWN"; + switch (req->data.domain) { + case ur_msgs::msg::Analog::CURRENT: + domain_string = "CURRENT"; + break; + case ur_msgs::msg::Analog::VOLTAGE: + domain_string = "VOLTAGE"; + break; + default: + RCLCPP_ERROR(get_node()->get_logger(), "Domain must be either 0 (CURRENT) or 1 (VOLTAGE)"); + resp->success = false; + return false; + } + + if (req->data.pin < 0 || req->data.pin > 1) { + RCLCPP_ERROR(get_node()->get_logger(), "Invalid pin selected. Only pins 0 and 1 are allowed."); + resp->success = false; + return false; + } + + command_interfaces_[CommandInterfaces::IO_ASYNC_SUCCESS].set_value(ASYNC_WAITING); + command_interfaces_[CommandInterfaces::ANALOG_OUTPUTS_CMD + req->data.pin].set_value( + static_cast(req->data.state)); + command_interfaces_[CommandInterfaces::ANALOG_OUTPUTS_DOMAIN].set_value(static_cast(req->data.domain)); + + RCLCPP_INFO(get_node()->get_logger(), "Setting analog output '%d' to state: '%f' in domain %s.", req->data.pin, + req->data.state, domain_string.c_str()); + + if (!waitForAsyncCommand([&]() { return command_interfaces_[CommandInterfaces::IO_ASYNC_SUCCESS].get_value(); })) { + RCLCPP_WARN(get_node()->get_logger(), "Could not verify that io was set. (This might happen when using the " + "mocked interface)"); + } + + resp->success = static_cast(command_interfaces_[IO_ASYNC_SUCCESS].get_value()); + return resp->success; +} + bool GPIOController::setSpeedSlider(ur_msgs::srv::SetSpeedSliderFraction::Request::SharedPtr req, ur_msgs::srv::SetSpeedSliderFraction::Response::SharedPtr resp) { diff --git a/ur_robot_driver/include/ur_robot_driver/hardware_interface.hpp b/ur_robot_driver/include/ur_robot_driver/hardware_interface.hpp index f7167e793..51d9d48e8 100644 --- a/ur_robot_driver/include/ur_robot_driver/hardware_interface.hpp +++ b/ur_robot_driver/include/ur_robot_driver/hardware_interface.hpp @@ -177,6 +177,7 @@ class URPositionHardwareInterface : public hardware_interface::SystemInterface // asynchronous commands std::array standard_dig_out_bits_cmd_; std::array standard_analog_output_cmd_; + double analog_output_domain_cmd_; double tool_voltage_cmd_; double io_async_success_; double target_speed_fraction_cmd_; diff --git a/ur_robot_driver/src/hardware_interface.cpp b/ur_robot_driver/src/hardware_interface.cpp index 2852df1b4..f99bcc270 100644 --- a/ur_robot_driver/src/hardware_interface.cpp +++ b/ur_robot_driver/src/hardware_interface.cpp @@ -301,6 +301,8 @@ std::vector URPositionHardwareInterface::e command_interfaces.emplace_back(hardware_interface::CommandInterface( tf_prefix + "gpio", "standard_analog_output_cmd_" + std::to_string(i), &standard_analog_output_cmd_[i])); } + command_interfaces.emplace_back( + hardware_interface::CommandInterface(tf_prefix + "gpio", "analog_output_domain_cmd", &analog_output_domain_cmd_)); command_interfaces.emplace_back( hardware_interface::CommandInterface(tf_prefix + "gpio", "tool_voltage_cmd", &tool_voltage_cmd_)); @@ -677,6 +679,8 @@ void URPositionHardwareInterface::initAsyncIO() standard_analog_output_cmd_[i] = NO_NEW_CMD_; } + analog_output_domain_cmd_ = NO_NEW_CMD_; + tool_voltage_cmd_ = NO_NEW_CMD_; payload_mass_ = NO_NEW_CMD_; @@ -706,7 +710,13 @@ void URPositionHardwareInterface::checkAsyncIO() for (size_t i = 0; i < 2; ++i) { if (!std::isnan(standard_analog_output_cmd_[i]) && ur_driver_ != nullptr) { - io_async_success_ = ur_driver_->getRTDEWriter().sendStandardAnalogOutput(i, standard_analog_output_cmd_[i]); + urcl::AnalogOutputType domain = urcl::AnalogOutputType::SET_ON_TEACH_PENDANT; + if (!std::isnan(analog_output_domain_cmd_) && ur_driver_ != nullptr) { + domain = static_cast(analog_output_domain_cmd_); + analog_output_domain_cmd_ = NO_NEW_CMD_; + } + io_async_success_ = + ur_driver_->getRTDEWriter().sendStandardAnalogOutput(i, standard_analog_output_cmd_[i], domain); standard_analog_output_cmd_[i] = NO_NEW_CMD_; } } From f1350b9685e69fc47840fbaca099254d3e51e7a4 Mon Sep 17 00:00:00 2001 From: Felix Exner Date: Mon, 28 Oct 2024 17:05:25 +0100 Subject: [PATCH 013/133] Update changelogs for upcoming release --- ur/CHANGELOG.rst | 3 +++ ur_bringup/CHANGELOG.rst | 3 +++ ur_calibration/CHANGELOG.rst | 3 +++ ur_controllers/CHANGELOG.rst | 6 ++++++ ur_dashboard_msgs/CHANGELOG.rst | 3 +++ ur_moveit_config/CHANGELOG.rst | 7 +++++++ ur_robot_driver/CHANGELOG.rst | 9 +++++++++ 7 files changed, 34 insertions(+) diff --git a/ur/CHANGELOG.rst b/ur/CHANGELOG.rst index 9bfd506ca..f4dd0f84b 100644 --- a/ur/CHANGELOG.rst +++ b/ur/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package ur ^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 2.2.15 (2024-07-26) ------------------- diff --git a/ur_bringup/CHANGELOG.rst b/ur_bringup/CHANGELOG.rst index ad09d48af..741af087f 100644 --- a/ur_bringup/CHANGELOG.rst +++ b/ur_bringup/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package ur_bringup ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 2.2.15 (2024-07-26) ------------------- diff --git a/ur_calibration/CHANGELOG.rst b/ur_calibration/CHANGELOG.rst index 953daf0bf..671daf3f0 100644 --- a/ur_calibration/CHANGELOG.rst +++ b/ur_calibration/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package ur_calibration ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 2.2.15 (2024-07-26) ------------------- diff --git a/ur_controllers/CHANGELOG.rst b/ur_controllers/CHANGELOG.rst index 2d3ad37b6..5531b362a 100644 --- a/ur_controllers/CHANGELOG.rst +++ b/ur_controllers/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package ur_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Allow setting the analog output domain when setting an analog output (backport of `#1123 `_) +* Service to get software version of robot (backport of `#964 `_) +* Contributors: mergify[bot], Felix Enxer, Jacob Larsen + 2.2.15 (2024-07-26) ------------------- * Updated scaled JTC to latest upstream updates (`#1067 `_) diff --git a/ur_dashboard_msgs/CHANGELOG.rst b/ur_dashboard_msgs/CHANGELOG.rst index 95a696584..ad4c4f258 100644 --- a/ur_dashboard_msgs/CHANGELOG.rst +++ b/ur_dashboard_msgs/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package ur_dashboard_msgs ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 2.2.15 (2024-07-26) ------------------- diff --git a/ur_moveit_config/CHANGELOG.rst b/ur_moveit_config/CHANGELOG.rst index 0ee305cef..5e682bd18 100644 --- a/ur_moveit_config/CHANGELOG.rst +++ b/ur_moveit_config/CHANGELOG.rst @@ -2,6 +2,13 @@ Changelog for package ur_moveit_config ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Properly handle use_sim_time (`#1146 `_) (`#1159 `_) +* Disable execution_duration_monitoring by default (`#1133 `_) +* Added option to publish SRDF file. +* Contributors: Felix Exner, mergify[bot], v-marsh + 2.2.15 (2024-07-26) ------------------- diff --git a/ur_robot_driver/CHANGELOG.rst b/ur_robot_driver/CHANGELOG.rst index 45aa60b5b..e4743d97a 100644 --- a/ur_robot_driver/CHANGELOG.rst +++ b/ur_robot_driver/CHANGELOG.rst @@ -1,3 +1,12 @@ +Forthcoming +----------- +* Allow setting the analog output domain when setting an analog output (backport of `#1123 `_) +* Fix component lifecycle (backport of `#1098 `_) +* [moveit] Disable execution_duration_monitoring by default (`#1133 `_) +* Service to get software version of robot (backport of `#964 `_) +* Assure the description is loaded as string (backport of `#1107 `_) +* Contributors: Felix Exner (fexner), mergify[bot], Jacob Larsen + 2.2.15 (2024-07-26) ------------------- * Fix passing launch_dashobard_client launch argument (backport of `#1057 `_) From 28cac065ef049a7c708dbfb5168bd4cf30cc5219 Mon Sep 17 00:00:00 2001 From: Felix Exner Date: Mon, 28 Oct 2024 17:05:36 +0100 Subject: [PATCH 014/133] 2.2.16 --- ur/CHANGELOG.rst | 4 ++-- ur/package.xml | 2 +- ur_bringup/CHANGELOG.rst | 4 ++-- ur_bringup/package.xml | 2 +- ur_calibration/CHANGELOG.rst | 4 ++-- ur_calibration/package.xml | 2 +- ur_controllers/CHANGELOG.rst | 4 ++-- ur_controllers/package.xml | 2 +- ur_dashboard_msgs/CHANGELOG.rst | 4 ++-- ur_dashboard_msgs/package.xml | 2 +- ur_moveit_config/CHANGELOG.rst | 4 ++-- ur_moveit_config/package.xml | 2 +- ur_robot_driver/CHANGELOG.rst | 4 ++-- ur_robot_driver/package.xml | 2 +- 14 files changed, 21 insertions(+), 21 deletions(-) diff --git a/ur/CHANGELOG.rst b/ur/CHANGELOG.rst index f4dd0f84b..6297de836 100644 --- a/ur/CHANGELOG.rst +++ b/ur/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package ur ^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.2.16 (2024-10-28) +------------------- 2.2.15 (2024-07-26) ------------------- diff --git a/ur/package.xml b/ur/package.xml index c36b73c0c..3ab6f7d6b 100644 --- a/ur/package.xml +++ b/ur/package.xml @@ -2,7 +2,7 @@ ur - 2.2.15 + 2.2.16 Metapackage for universal robots Felix Exner Robert Wilbrandt diff --git a/ur_bringup/CHANGELOG.rst b/ur_bringup/CHANGELOG.rst index 741af087f..9008fa5ef 100644 --- a/ur_bringup/CHANGELOG.rst +++ b/ur_bringup/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package ur_bringup ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.2.16 (2024-10-28) +------------------- 2.2.15 (2024-07-26) ------------------- diff --git a/ur_bringup/package.xml b/ur_bringup/package.xml index 9aad69454..11ff3c710 100644 --- a/ur_bringup/package.xml +++ b/ur_bringup/package.xml @@ -2,7 +2,7 @@ ur_bringup - 2.2.15 + 2.2.16 Launch file and run-time configurations, e.g. controllers. Denis Stogl diff --git a/ur_calibration/CHANGELOG.rst b/ur_calibration/CHANGELOG.rst index 671daf3f0..85047f1e9 100644 --- a/ur_calibration/CHANGELOG.rst +++ b/ur_calibration/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package ur_calibration ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.2.16 (2024-10-28) +------------------- 2.2.15 (2024-07-26) ------------------- diff --git a/ur_calibration/package.xml b/ur_calibration/package.xml index 3d3cb6973..fb6b89630 100644 --- a/ur_calibration/package.xml +++ b/ur_calibration/package.xml @@ -1,7 +1,7 @@ ur_calibration - 2.2.15 + 2.2.16 Package for extracting the factory calibration from a UR robot and change it such that it can be used by ur_description to gain a correct URDF Felix Exner diff --git a/ur_controllers/CHANGELOG.rst b/ur_controllers/CHANGELOG.rst index 5531b362a..03c1c984f 100644 --- a/ur_controllers/CHANGELOG.rst +++ b/ur_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package ur_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.2.16 (2024-10-28) +------------------- * Allow setting the analog output domain when setting an analog output (backport of `#1123 `_) * Service to get software version of robot (backport of `#964 `_) * Contributors: mergify[bot], Felix Enxer, Jacob Larsen diff --git a/ur_controllers/package.xml b/ur_controllers/package.xml index e5b7197e8..ed62e6515 100644 --- a/ur_controllers/package.xml +++ b/ur_controllers/package.xml @@ -2,7 +2,7 @@ ur_controllers - 2.2.15 + 2.2.16 Provides controllers that use the speed scaling interface of Universal Robots. Denis Stogl diff --git a/ur_dashboard_msgs/CHANGELOG.rst b/ur_dashboard_msgs/CHANGELOG.rst index ad4c4f258..6e47280fe 100644 --- a/ur_dashboard_msgs/CHANGELOG.rst +++ b/ur_dashboard_msgs/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package ur_dashboard_msgs ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.2.16 (2024-10-28) +------------------- 2.2.15 (2024-07-26) ------------------- diff --git a/ur_dashboard_msgs/package.xml b/ur_dashboard_msgs/package.xml index ea0298e30..6684d7f9c 100644 --- a/ur_dashboard_msgs/package.xml +++ b/ur_dashboard_msgs/package.xml @@ -2,7 +2,7 @@ ur_dashboard_msgs - 2.2.15 + 2.2.16 Messages around the UR Dashboard server. Felix Exner diff --git a/ur_moveit_config/CHANGELOG.rst b/ur_moveit_config/CHANGELOG.rst index 5e682bd18..e5add7ed8 100644 --- a/ur_moveit_config/CHANGELOG.rst +++ b/ur_moveit_config/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package ur_moveit_config ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.2.16 (2024-10-28) +------------------- * Properly handle use_sim_time (`#1146 `_) (`#1159 `_) * Disable execution_duration_monitoring by default (`#1133 `_) * Added option to publish SRDF file. diff --git a/ur_moveit_config/package.xml b/ur_moveit_config/package.xml index 5c159c00d..4eea852ea 100644 --- a/ur_moveit_config/package.xml +++ b/ur_moveit_config/package.xml @@ -2,7 +2,7 @@ ur_moveit_config - 2.2.15 + 2.2.16 An example package with MoveIt2 configurations for UR robots. diff --git a/ur_robot_driver/CHANGELOG.rst b/ur_robot_driver/CHANGELOG.rst index e4743d97a..1ec669fd4 100644 --- a/ur_robot_driver/CHANGELOG.rst +++ b/ur_robot_driver/CHANGELOG.rst @@ -1,5 +1,5 @@ -Forthcoming ------------ +2.2.16 (2024-10-28) +------------------- * Allow setting the analog output domain when setting an analog output (backport of `#1123 `_) * Fix component lifecycle (backport of `#1098 `_) * [moveit] Disable execution_duration_monitoring by default (`#1133 `_) diff --git a/ur_robot_driver/package.xml b/ur_robot_driver/package.xml index 90924da23..9261a98f5 100644 --- a/ur_robot_driver/package.xml +++ b/ur_robot_driver/package.xml @@ -2,7 +2,7 @@ ur_robot_driver - 2.2.15 + 2.2.16 The new driver for Universal Robots UR3, UR5 and UR10 robots with CB3 controllers and the e-series. Denis Stogl From ecf02413650064474c8e1807ffb3a6f85d4b930e Mon Sep 17 00:00:00 2001 From: "mergify[bot]" <37929162+mergify[bot]@users.noreply.github.com> Date: Wed, 30 Oct 2024 09:20:54 +0100 Subject: [PATCH 015/133] [SJTC] Make scaling interface optional (#1145) (#1172) This way, the controller can be used on systems, where no scaling interface is available (e.g. GZ). The upstream version in ros2_controllers will have the same behavior. --- .../scaled_joint_trajectory_controller.hpp | 7 ++++- .../scaled_joint_trajectory_controller.cpp | 31 +++++++++++++++---- 2 files changed, 31 insertions(+), 7 deletions(-) diff --git a/ur_controllers/include/ur_controllers/scaled_joint_trajectory_controller.hpp b/ur_controllers/include/ur_controllers/scaled_joint_trajectory_controller.hpp index 75a450547..58bd12426 100644 --- a/ur_controllers/include/ur_controllers/scaled_joint_trajectory_controller.hpp +++ b/ur_controllers/include/ur_controllers/scaled_joint_trajectory_controller.hpp @@ -37,6 +37,8 @@ #ifndef UR_CONTROLLERS__SCALED_JOINT_TRAJECTORY_CONTROLLER_HPP_ #define UR_CONTROLLERS__SCALED_JOINT_TRAJECTORY_CONTROLLER_HPP_ +#include +#include #include "angles/angles.h" #include "joint_trajectory_controller/joint_trajectory_controller.hpp" #include "joint_trajectory_controller/trajectory.hpp" @@ -73,9 +75,12 @@ class ScaledJointTrajectoryController : public joint_trajectory_controller::Join }; private: - double scaling_factor_{}; + double scaling_factor_{ 1.0 }; realtime_tools::RealtimeBuffer time_data_; + std::optional> scaling_state_interface_ = + std::nullopt; + std::shared_ptr scaled_param_listener_; scaled_joint_trajectory_controller::Params scaled_params_; }; diff --git a/ur_controllers/src/scaled_joint_trajectory_controller.cpp b/ur_controllers/src/scaled_joint_trajectory_controller.cpp index d9009898b..9a97ab38a 100644 --- a/ur_controllers/src/scaled_joint_trajectory_controller.cpp +++ b/ur_controllers/src/scaled_joint_trajectory_controller.cpp @@ -50,6 +50,12 @@ controller_interface::CallbackReturn ScaledJointTrajectoryController::on_init() // Create the parameter listener and get the parameters scaled_param_listener_ = std::make_shared(get_node()); scaled_params_ = scaled_param_listener_->get_params(); + if (!scaled_params_.speed_scaling_interface_name.empty()) { + RCLCPP_INFO(get_node()->get_logger(), "Using scaling state from the hardware from interface %s.", + scaled_params_.speed_scaling_interface_name.c_str()); + } else { + RCLCPP_INFO(get_node()->get_logger(), "No scaling interface set. This controller will not use speed scaling."); + } return JointTrajectoryController::on_init(); } @@ -58,7 +64,10 @@ controller_interface::InterfaceConfiguration ScaledJointTrajectoryController::st { controller_interface::InterfaceConfiguration conf; conf = JointTrajectoryController::state_interface_configuration(); - conf.names.push_back(scaled_params_.speed_scaling_interface_name); + + if (!scaled_params_.speed_scaling_interface_name.empty()) { + conf.names.push_back(scaled_params_.speed_scaling_interface_name); + } return conf; } @@ -70,17 +79,27 @@ controller_interface::CallbackReturn ScaledJointTrajectoryController::on_activat time_data.period = rclcpp::Duration::from_nanoseconds(0); time_data.uptime = get_node()->now(); time_data_.initRT(time_data); + + // Set scaling interfaces + if (!scaled_params_.speed_scaling_interface_name.empty()) { + auto it = std::find_if(state_interfaces_.begin(), state_interfaces_.end(), [&](auto& interface) { + return (interface.get_name() == scaled_params_.speed_scaling_interface_name); + }); + if (it != state_interfaces_.end()) { + scaling_state_interface_ = *it; + } else { + RCLCPP_ERROR(get_node()->get_logger(), "Did not find speed scaling interface in state interfaces."); + } + } + return JointTrajectoryController::on_activate(state); } controller_interface::return_type ScaledJointTrajectoryController::update(const rclcpp::Time& time, const rclcpp::Duration& period) { - if (state_interfaces_.back().get_name() == scaled_params_.speed_scaling_interface_name) { - scaling_factor_ = state_interfaces_.back().get_value(); - } else { - RCLCPP_ERROR(get_node()->get_logger(), "Speed scaling interface (%s) not found in hardware interface.", - scaled_params_.speed_scaling_interface_name.c_str()); + if (scaling_state_interface_.has_value()) { + scaling_factor_ = scaling_state_interface_->get().get_value(); } if (get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE) { From 1b2b8468f6ebfb7c2e094097369e8bfcdd7e1ddd Mon Sep 17 00:00:00 2001 From: Felix Exner Date: Thu, 7 Nov 2024 09:37:32 +0100 Subject: [PATCH 016/133] Update Humble upstream workspace (#1176) * Update Universal_Robots_ROS2_Driver.humble.repos As the master version of kinematics_interface got API incompatible we should update this in our workflow. * Add realtime_tools to upstream workspace * Make non-released upstream workspace empty --- Universal_Robots_ROS2_Driver-not-released.humble.repos | 6 +----- Universal_Robots_ROS2_Driver.humble.repos | 6 +++++- 2 files changed, 6 insertions(+), 6 deletions(-) diff --git a/Universal_Robots_ROS2_Driver-not-released.humble.repos b/Universal_Robots_ROS2_Driver-not-released.humble.repos index 3c7db407d..fde9527c5 100644 --- a/Universal_Robots_ROS2_Driver-not-released.humble.repos +++ b/Universal_Robots_ROS2_Driver-not-released.humble.repos @@ -3,8 +3,4 @@ # requires a newer version than the one currently released to the target distributions. # Once Upstream packages are released and synced to the target distributions in the required # version, the entry in this file shall be removed again. -repositories: - Universal_Robots_ROS2_Description: - type: git - url: https://github.com/UniversalRobots/Universal_Robots_ROS2_Description.git - version: humble +repositories: [] diff --git a/Universal_Robots_ROS2_Driver.humble.repos b/Universal_Robots_ROS2_Driver.humble.repos index 14de188d5..ddbdbbfac 100644 --- a/Universal_Robots_ROS2_Driver.humble.repos +++ b/Universal_Robots_ROS2_Driver.humble.repos @@ -22,8 +22,12 @@ repositories: kinematics_interface: type: git url: https://github.com/ros-controls/kinematics_interface.git - version: master + version: humble control_msgs: type: git url: https://github.com/ros-controls/control_msgs.git version: humble + realtime_tools: + type: git + url: https://github.com/ros-controls/realtime_tools.git + version: master From d2876cb81700556e141d2f85159ee35e57565fc9 Mon Sep 17 00:00:00 2001 From: "mergify[bot]" <37929162+mergify[bot]@users.noreply.github.com> Date: Thu, 14 Nov 2024 16:20:42 +0100 Subject: [PATCH 017/133] Use pose_broadcaster to publish the TCP pose (backport of #1108) Adds a broadcaster for the robot's TCP pose. --- ur_robot_driver/config/ur_controllers.yaml | 10 +++++ .../ur_robot_driver/hardware_interface.hpp | 25 ++++++++++- ur_robot_driver/launch/ur_control.launch.py | 2 + ur_robot_driver/package.xml | 1 + ur_robot_driver/src/hardware_interface.cpp | 43 ++++++++++++------- 5 files changed, 65 insertions(+), 16 deletions(-) diff --git a/ur_robot_driver/config/ur_controllers.yaml b/ur_robot_driver/config/ur_controllers.yaml index 544dd1fbd..ceb56e5ef 100644 --- a/ur_robot_driver/config/ur_controllers.yaml +++ b/ur_robot_driver/config/ur_controllers.yaml @@ -24,6 +24,9 @@ controller_manager: forward_position_controller: type: position_controllers/JointGroupPositionController + tcp_pose_broadcaster: + type: pose_broadcaster/PoseBroadcaster + ur_configuration_controller: type: ur_controllers/URConfigurationController @@ -130,3 +133,10 @@ forward_position_controller: - $(var tf_prefix)wrist_1_joint - $(var tf_prefix)wrist_2_joint - $(var tf_prefix)wrist_3_joint + +tcp_pose_broadcaster: + ros__parameters: + frame_id: $(var tf_prefix)base + pose_name: $(var tf_prefix)tcp_pose + tf: + child_frame_id: $(var tf_prefix)tool0_controller diff --git a/ur_robot_driver/include/ur_robot_driver/hardware_interface.hpp b/ur_robot_driver/include/ur_robot_driver/hardware_interface.hpp index 51d9d48e8..515f19999 100644 --- a/ur_robot_driver/include/ur_robot_driver/hardware_interface.hpp +++ b/ur_robot_driver/include/ur_robot_driver/hardware_interface.hpp @@ -80,6 +80,28 @@ enum StoppingInterface STOP_VELOCITY }; +// We define our own quaternion to use it as a buffer, since we need to pass pointers to the state +// interfaces. +struct Quaternion +{ + Quaternion() : x(0), y(0), z(0), w(0) + { + } + + void set(const tf2::Quaternion& q) + { + x = q.x(); + y = q.y(); + z = q.z(); + w = q.w(); + } + + double x; + double y; + double z; + double w; +}; + /*! * \brief The HardwareInterface class handles the interface between the ROS system and the main * driver. It contains the read and write methods of the main control loop and registers various ROS @@ -144,6 +166,8 @@ class URPositionHardwareInterface : public hardware_interface::SystemInterface urcl::vector6d_t urcl_joint_efforts_; urcl::vector6d_t urcl_ft_sensor_measurements_; urcl::vector6d_t urcl_tcp_pose_; + tf2::Quaternion tcp_rotation_quat_; + Quaternion tcp_rotation_buffer; bool packet_read_; @@ -172,7 +196,6 @@ class URPositionHardwareInterface : public hardware_interface::SystemInterface // transform stuff tf2::Vector3 tcp_force_; tf2::Vector3 tcp_torque_; - geometry_msgs::msg::TransformStamped tcp_transform_; // asynchronous commands std::array standard_dig_out_bits_cmd_; diff --git a/ur_robot_driver/launch/ur_control.launch.py b/ur_robot_driver/launch/ur_control.launch.py index 053ed4996..970c30d2d 100644 --- a/ur_robot_driver/launch/ur_control.launch.py +++ b/ur_robot_driver/launch/ur_control.launch.py @@ -295,6 +295,7 @@ def launch_setup(context, *args, **kwargs): "force_torque_sensor_broadcaster", "joint_state_broadcaster", "speed_scaling_state_broadcaster", + "tcp_pose_broadcaster", "ur_configuration_controller", ] }, @@ -338,6 +339,7 @@ def controller_spawner(controllers, active=True): "io_and_status_controller", "speed_scaling_state_broadcaster", "force_torque_sensor_broadcaster", + "tcp_pose_broadcaster", "ur_configuration_controller", ] controllers_inactive = ["forward_position_controller"] diff --git a/ur_robot_driver/package.xml b/ur_robot_driver/package.xml index 9261a98f5..bc27de8de 100644 --- a/ur_robot_driver/package.xml +++ b/ur_robot_driver/package.xml @@ -50,6 +50,7 @@ joint_trajectory_controller launch launch_ros + pose_broadcaster position_controllers robot_state_publisher ros2_controllers_test_nodes diff --git a/ur_robot_driver/src/hardware_interface.cpp b/ur_robot_driver/src/hardware_interface.cpp index f99bcc270..e88eabb7b 100644 --- a/ur_robot_driver/src/hardware_interface.cpp +++ b/ur_robot_driver/src/hardware_interface.cpp @@ -170,9 +170,14 @@ std::vector URPositionHardwareInterface::exp &speed_scaling_combined_)); for (auto& sensor : info_.sensors) { - for (uint j = 0; j < sensor.state_interfaces.size(); ++j) { - state_interfaces.emplace_back(hardware_interface::StateInterface(sensor.name, sensor.state_interfaces[j].name, - &urcl_ft_sensor_measurements_[j])); + if (sensor.name == tf_prefix + "tcp_fts_sensor") { + const std::vector fts_names = { + "force.x", "force.y", "force.z", "torque.x", "torque.y", "torque.z" + }; + for (uint j = 0; j < 6; ++j) { + state_interfaces.emplace_back( + hardware_interface::StateInterface(sensor.name, fts_names[j], &urcl_ft_sensor_measurements_[j])); + } } } @@ -232,6 +237,21 @@ std::vector URPositionHardwareInterface::exp state_interfaces.emplace_back( hardware_interface::StateInterface(tf_prefix + "gpio", "program_running", &robot_program_running_copy_)); + state_interfaces.emplace_back( + hardware_interface::StateInterface(tf_prefix + "tcp_pose", "position.x", &urcl_tcp_pose_[0])); + state_interfaces.emplace_back( + hardware_interface::StateInterface(tf_prefix + "tcp_pose", "position.y", &urcl_tcp_pose_[1])); + state_interfaces.emplace_back( + hardware_interface::StateInterface(tf_prefix + "tcp_pose", "position.z", &urcl_tcp_pose_[2])); + state_interfaces.emplace_back( + hardware_interface::StateInterface(tf_prefix + "tcp_pose", "orientation.x", &tcp_rotation_buffer.x)); + state_interfaces.emplace_back( + hardware_interface::StateInterface(tf_prefix + "tcp_pose", "orientation.y", &tcp_rotation_buffer.y)); + state_interfaces.emplace_back( + hardware_interface::StateInterface(tf_prefix + "tcp_pose", "orientation.z", &tcp_rotation_buffer.z)); + state_interfaces.emplace_back( + hardware_interface::StateInterface(tf_prefix + "tcp_pose", "orientation.w", &tcp_rotation_buffer.w)); + state_interfaces.emplace_back(hardware_interface::StateInterface( tf_prefix + "get_robot_software_version", "get_version_major", &get_robot_software_version_major_)); @@ -796,10 +816,8 @@ void URPositionHardwareInterface::transformForceTorque() tcp_torque_.setValue(urcl_ft_sensor_measurements_[3], urcl_ft_sensor_measurements_[4], urcl_ft_sensor_measurements_[5]); - tf2::Quaternion rotation_quat; - tf2::fromMsg(tcp_transform_.transform.rotation, rotation_quat); - tcp_force_ = tf2::quatRotate(rotation_quat.inverse(), tcp_force_); - tcp_torque_ = tf2::quatRotate(rotation_quat.inverse(), tcp_torque_); + tcp_force_ = tf2::quatRotate(tcp_rotation_quat_.inverse(), tcp_force_); + tcp_torque_ = tf2::quatRotate(tcp_rotation_quat_.inverse(), tcp_torque_); urcl_ft_sensor_measurements_ = { tcp_force_.x(), tcp_force_.y(), tcp_force_.z(), tcp_torque_.x(), tcp_torque_.y(), tcp_torque_.z() }; @@ -812,17 +830,12 @@ void URPositionHardwareInterface::extractToolPose() std::sqrt(std::pow(urcl_tcp_pose_[3], 2) + std::pow(urcl_tcp_pose_[4], 2) + std::pow(urcl_tcp_pose_[5], 2)); tf2::Vector3 rotation_vec(urcl_tcp_pose_[3], urcl_tcp_pose_[4], urcl_tcp_pose_[5]); - tf2::Quaternion rotation; if (tcp_angle > 1e-16) { - rotation.setRotation(rotation_vec.normalized(), tcp_angle); + tcp_rotation_quat_.setRotation(rotation_vec.normalized(), tcp_angle); } else { - rotation.setValue(0.0, 0.0, 0.0, 1.0); // default Quaternion is 0,0,0,0 which is invalid + tcp_rotation_quat_.setValue(0.0, 0.0, 0.0, 1.0); // default Quaternion is 0,0,0,0 which is invalid } - tcp_transform_.transform.translation.x = urcl_tcp_pose_[0]; - tcp_transform_.transform.translation.y = urcl_tcp_pose_[1]; - tcp_transform_.transform.translation.z = urcl_tcp_pose_[2]; - - tcp_transform_.transform.rotation = tf2::toMsg(rotation); + tcp_rotation_buffer.set(tcp_rotation_quat_); } hardware_interface::return_type URPositionHardwareInterface::prepare_command_mode_switch( From 6f074bb5c9247560660d1c793da86263e7b77376 Mon Sep 17 00:00:00 2001 From: "mergify[bot]" <37929162+mergify[bot]@users.noreply.github.com> Date: Thu, 28 Nov 2024 13:28:51 +0100 Subject: [PATCH 018/133] Forward trajectory controller (backport of #944) * 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 --- ur_bringup/config/ur_controllers.yaml | 115 ++-- ur_controllers/CMakeLists.txt | 14 + ur_controllers/controller_plugins.xml | 5 + ur_controllers/doc/index.rst | 129 ++++ .../passthrough_trajectory_controller.hpp | 184 ++++++ ur_controllers/package.xml | 4 + .../src/passthrough_trajectory_controller.cpp | 624 ++++++++++++++++++ ...ough_trajectory_controller_parameters.yaml | 31 + ur_robot_driver/CMakeLists.txt | 5 + ur_robot_driver/config/ur_controllers.yaml | 18 + .../ur_robot_driver/hardware_interface.hpp | 24 +- ur_robot_driver/launch/ur10.launch.py | 1 + ur_robot_driver/launch/ur10e.launch.py | 1 + ur_robot_driver/launch/ur16e.launch.py | 1 + ur_robot_driver/launch/ur20.launch.py | 1 + ur_robot_driver/launch/ur3.launch.py | 1 + ur_robot_driver/launch/ur30.launch.py | 1 + ur_robot_driver/launch/ur3e.launch.py | 1 + ur_robot_driver/launch/ur5.launch.py | 1 + ur_robot_driver/launch/ur5e.launch.py | 1 + ur_robot_driver/launch/ur_control.launch.py | 47 +- ur_robot_driver/scripts/example_move.py | 219 ++++++ ur_robot_driver/src/hardware_interface.cpp | 236 ++++++- .../integration_test_controller_switch.py | 287 ++++++++ ur_robot_driver/test/robot_driver.py | 149 ++++- ur_robot_driver/test/test_common.py | 22 +- 26 files changed, 2013 insertions(+), 109 deletions(-) create mode 100644 ur_controllers/include/ur_controllers/passthrough_trajectory_controller.hpp create mode 100644 ur_controllers/src/passthrough_trajectory_controller.cpp create mode 100644 ur_controllers/src/passthrough_trajectory_controller_parameters.yaml create mode 100755 ur_robot_driver/scripts/example_move.py create mode 100644 ur_robot_driver/test/integration_test_controller_switch.py diff --git a/ur_bringup/config/ur_controllers.yaml b/ur_bringup/config/ur_controllers.yaml index 1d9c6eb4d..8c7e30b96 100644 --- a/ur_bringup/config/ur_controllers.yaml +++ b/ur_bringup/config/ur_controllers.yaml @@ -24,15 +24,31 @@ controller_manager: forward_position_controller: type: position_controllers/JointGroupPositionController + tcp_pose_broadcaster: + type: pose_broadcaster/PoseBroadcaster + + passthrough_trajectory_controller: + type: ur_controllers/PassthroughTrajectoryController + + ur_configuration_controller: + type: ur_controllers/URConfigurationController speed_scaling_state_broadcaster: ros__parameters: state_publish_rate: 100.0 + tf_prefix: "$(var tf_prefix)" + +io_and_status_controller: + ros__parameters: + tf_prefix: "$(var tf_prefix)" +ur_configuration_controller: + ros__parameters: + tf_prefix: "$(var tf_prefix)" force_torque_sensor_broadcaster: ros__parameters: - sensor_name: tcp_fts_sensor + sensor_name: $(var tf_prefix)tcp_fts_sensor state_interface_names: - force.x - force.y @@ -40,19 +56,19 @@ force_torque_sensor_broadcaster: - torque.x - torque.y - torque.z - frame_id: tool0 + frame_id: $(var tf_prefix)tool0 topic_name: ft_data joint_trajectory_controller: ros__parameters: joints: - - shoulder_pan_joint - - shoulder_lift_joint - - elbow_joint - - wrist_1_joint - - wrist_2_joint - - wrist_3_joint + - $(var tf_prefix)shoulder_pan_joint + - $(var tf_prefix)shoulder_lift_joint + - $(var tf_prefix)elbow_joint + - $(var tf_prefix)wrist_1_joint + - $(var tf_prefix)wrist_2_joint + - $(var tf_prefix)wrist_3_joint command_interfaces: - position state_interfaces: @@ -64,23 +80,23 @@ joint_trajectory_controller: constraints: stopped_velocity_tolerance: 0.2 goal_time: 0.0 - shoulder_pan_joint: { trajectory: 0.2, goal: 0.1 } - shoulder_lift_joint: { trajectory: 0.2, goal: 0.1 } - elbow_joint: { trajectory: 0.2, goal: 0.1 } - wrist_1_joint: { trajectory: 0.2, goal: 0.1 } - wrist_2_joint: { trajectory: 0.2, goal: 0.1 } - wrist_3_joint: { trajectory: 0.2, goal: 0.1 } + $(var tf_prefix)shoulder_pan_joint: { trajectory: 0.2, goal: 0.1 } + $(var tf_prefix)shoulder_lift_joint: { trajectory: 0.2, goal: 0.1 } + $(var tf_prefix)elbow_joint: { trajectory: 0.2, goal: 0.1 } + $(var tf_prefix)wrist_1_joint: { trajectory: 0.2, goal: 0.1 } + $(var tf_prefix)wrist_2_joint: { trajectory: 0.2, goal: 0.1 } + $(var tf_prefix)wrist_3_joint: { trajectory: 0.2, goal: 0.1 } scaled_joint_trajectory_controller: ros__parameters: joints: - - shoulder_pan_joint - - shoulder_lift_joint - - elbow_joint - - wrist_1_joint - - wrist_2_joint - - wrist_3_joint + - $(var tf_prefix)shoulder_pan_joint + - $(var tf_prefix)shoulder_lift_joint + - $(var tf_prefix)elbow_joint + - $(var tf_prefix)wrist_1_joint + - $(var tf_prefix)wrist_2_joint + - $(var tf_prefix)wrist_3_joint command_interfaces: - position state_interfaces: @@ -92,30 +108,53 @@ scaled_joint_trajectory_controller: constraints: stopped_velocity_tolerance: 0.2 goal_time: 0.0 - shoulder_pan_joint: { trajectory: 0.2, goal: 0.1 } - shoulder_lift_joint: { trajectory: 0.2, goal: 0.1 } - elbow_joint: { trajectory: 0.2, goal: 0.1 } - wrist_1_joint: { trajectory: 0.2, goal: 0.1 } - wrist_2_joint: { trajectory: 0.2, goal: 0.1 } - wrist_3_joint: { trajectory: 0.2, goal: 0.1 } + $(var tf_prefix)shoulder_pan_joint: { trajectory: 0.2, goal: 0.1 } + $(var tf_prefix)shoulder_lift_joint: { trajectory: 0.2, goal: 0.1 } + $(var tf_prefix)elbow_joint: { trajectory: 0.2, goal: 0.1 } + $(var tf_prefix)wrist_1_joint: { trajectory: 0.2, goal: 0.1 } + $(var tf_prefix)wrist_2_joint: { trajectory: 0.2, goal: 0.1 } + $(var tf_prefix)wrist_3_joint: { trajectory: 0.2, goal: 0.1 } + speed_scaling_interface_name: $(var tf_prefix)speed_scaling/speed_scaling_factor + +passthrough_trajectory_controller: + ros__parameters: + tf_prefix: "$(var tf_prefix)" + joints: + - $(var tf_prefix)shoulder_pan_joint + - $(var tf_prefix)shoulder_lift_joint + - $(var tf_prefix)elbow_joint + - $(var tf_prefix)wrist_1_joint + - $(var tf_prefix)wrist_2_joint + - $(var tf_prefix)wrist_3_joint + state_interfaces: + - position + - velocity + speed_scaling_interface_name: $(var tf_prefix)speed_scaling/speed_scaling_factor forward_velocity_controller: ros__parameters: joints: - - shoulder_pan_joint - - shoulder_lift_joint - - elbow_joint - - wrist_1_joint - - wrist_2_joint - - wrist_3_joint + - $(var tf_prefix)shoulder_pan_joint + - $(var tf_prefix)shoulder_lift_joint + - $(var tf_prefix)elbow_joint + - $(var tf_prefix)wrist_1_joint + - $(var tf_prefix)wrist_2_joint + - $(var tf_prefix)wrist_3_joint interface_name: velocity forward_position_controller: ros__parameters: joints: - - shoulder_pan_joint - - shoulder_lift_joint - - elbow_joint - - wrist_1_joint - - wrist_2_joint - - wrist_3_joint + - $(var tf_prefix)shoulder_pan_joint + - $(var tf_prefix)shoulder_lift_joint + - $(var tf_prefix)elbow_joint + - $(var tf_prefix)wrist_1_joint + - $(var tf_prefix)wrist_2_joint + - $(var tf_prefix)wrist_3_joint + +tcp_pose_broadcaster: + ros__parameters: + frame_id: $(var tf_prefix)base + pose_name: $(var tf_prefix)tcp_pose + tf: + child_frame_id: $(var tf_prefix)tool0_controller diff --git a/ur_controllers/CMakeLists.txt b/ur_controllers/CMakeLists.txt index 5ea7bbe86..e94be4f0f 100644 --- a/ur_controllers/CMakeLists.txt +++ b/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} diff --git a/ur_controllers/controller_plugins.xml b/ur_controllers/controller_plugins.xml index fa4b63987..c784b35d0 100644 --- a/ur_controllers/controller_plugins.xml +++ b/ur_controllers/controller_plugins.xml @@ -14,6 +14,11 @@ This controller publishes the Tool IO. + + + This controller forwards a joint-based trajectory to the robot controller for interpolation. + + Controller used to get and change the configuration of the robot diff --git a/ur_controllers/doc/index.rst b/ur_controllers/doc/index.rst index bc4d7c147..a488d07e5 100644 --- a/ur_controllers/doc/index.rst +++ b/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 `_. + +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 | | Joint names to listen to | ++----------------------------------+--------------+----------------------------------------+------------------------------------------------------------------------------------------------------------------+ +| ``state_interfaces`` (required) | string_array | | 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 | | 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 + + + + + + + + + + + + + + + + + + + + + + + + + +.. 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. diff --git a/ur_controllers/include/ur_controllers/passthrough_trajectory_controller.hpp b/ur_controllers/include/ur_controllers/passthrough_trajectory_controller.hpp new file mode 100644 index 000000000..a3c91d10f --- /dev/null +++ b/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 jala@universal-robots.com + * \date 2024-03-11 + * + * + * + * + */ +//---------------------------------------------------------------------- + +#ifndef UR_CONTROLLERS__PASSTHROUGH_TRAJECTORY_CONTROLLER_HPP_ +#define UR_CONTROLLERS__PASSTHROUGH_TRAJECTORY_CONTROLLER_HPP_ + +#include + +#include +#include + +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include + +#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; + using RealtimeGoalHandlePtr = std::shared_ptr; + using RealtimeGoalHandleBuffer = realtime_tools::RealtimeBuffer; + + 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> 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 create_joint_mapping(const std::vector& joint_names) const; + + std::shared_ptr passthrough_param_listener_; + passthrough_trajectory_controller::Params passthrough_params_; + + rclcpp_action::Server::SharedPtr send_trajectory_action_server_; + + rclcpp_action::GoalResponse + goal_received_callback(const rclcpp_action::GoalUUID& uuid, + std::shared_ptr goal); + + rclcpp_action::CancelResponse goal_cancelled_callback( + const std::shared_ptr> goal_handle); + + void goal_accepted_callback( + std::shared_ptr> goal_handle); + + realtime_tools::RealtimeBuffer> joint_names_; + std::vector state_interface_types_; + + std::vector joint_state_interface_names_; + std::vector> joint_position_state_interface_; + std::vector> joint_velocity_state_interface_; + std::vector> joint_acceleration_state_interface_; + + bool check_goal_tolerances(std::shared_ptr goal); + bool check_goal_positions(std::shared_ptr goal); + bool check_goal_velocities(std::shared_ptr goal); + bool check_goal_accelerations(std::shared_ptr goal); + + trajectory_msgs::msg::JointTrajectory active_joint_traj_; + // std::vector path_tolerance_; + realtime_tools::RealtimeBuffer> goal_tolerance_; + realtime_tools::RealtimeBuffer goal_time_tolerance_{ rclcpp::Duration(0, 0) }; + + std::atomic current_index_; + std::atomic 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 number_of_joints_; + static constexpr double NO_VAL = std::numeric_limits::quiet_NaN(); + + std::optional> scaling_state_interface_; + std::optional> abort_command_interface_; + std::optional> transfer_command_interface_; + std::optional> time_from_start_command_interface_; + + rclcpp::Clock::SharedPtr clock_; +}; +} // namespace ur_controllers +#endif // UR_CONTROLLERS__PASSTHROUGH_TRAJECTORY_CONTROLLER_HPP_ diff --git a/ur_controllers/package.xml b/ur_controllers/package.xml index ed62e6515..e8f7c8da4 100644 --- a/ur_controllers/package.xml +++ b/ur_controllers/package.xml @@ -32,6 +32,10 @@ std_srvs ur_dashboard_msgs ur_msgs + control_msgs + trajectory_msgs + action_msgs + ament_cmake diff --git a/ur_controllers/src/passthrough_trajectory_controller.cpp b/ur_controllers/src/passthrough_trajectory_controller.cpp new file mode 100644 index 000000000..95f726664 --- /dev/null +++ b/ur_controllers/src/passthrough_trajectory_controller.cpp @@ -0,0 +1,624 @@ +// 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 jala@universal-robots.com + * \date 2024-03-11 + * + * + * + * + */ +//---------------------------------------------------------------------- + +#include +#include +#include + +#include +#include +#include +#include + +#include "ur_controllers/passthrough_trajectory_controller.hpp" + +namespace ur_controllers +{ + +double duration_to_double(const builtin_interfaces::msg::Duration& duration) +{ + return duration.sec + (duration.nanosec / 1000000000.0); +} + +controller_interface::CallbackReturn PassthroughTrajectoryController::on_init() +{ + passthrough_param_listener_ = std::make_shared(get_node()); + passthrough_params_ = passthrough_param_listener_->get_params(); + current_index_ = 0; + auto joint_names = passthrough_params_.joints; + joint_names_.writeFromNonRT(joint_names); + number_of_joints_ = joint_names.size(); + state_interface_types_ = passthrough_params_.state_interfaces; + scaling_factor_ = 1.0; + clock_ = get_node()->get_clock(); + return controller_interface::CallbackReturn::SUCCESS; +} + +controller_interface::CallbackReturn +PassthroughTrajectoryController::on_configure(const rclcpp_lifecycle::State& previous_state) +{ + start_action_server(); + trajectory_active_ = false; + + joint_state_interface_names_.clear(); + + joint_state_interface_names_.reserve(number_of_joints_ * state_interface_types_.size()); + + auto joint_names_internal = joint_names_.readFromRT(); + for (const auto& joint_name : *joint_names_internal) { + for (const auto& interface_type : state_interface_types_) { + joint_state_interface_names_.emplace_back(joint_name + "/" + interface_type); + } + } + + return ControllerInterface::on_configure(previous_state); +} + +void PassthroughTrajectoryController::start_action_server(void) +{ + send_trajectory_action_server_ = rclcpp_action::create_server( + get_node(), std::string(get_node()->get_name()) + "/follow_joint_trajectory", + std::bind(&PassthroughTrajectoryController::goal_received_callback, this, std::placeholders::_1, + std::placeholders::_2), + std::bind(&PassthroughTrajectoryController::goal_cancelled_callback, this, std::placeholders::_1), + std::bind(&PassthroughTrajectoryController::goal_accepted_callback, this, std::placeholders::_1)); + return; +} + +controller_interface::InterfaceConfiguration PassthroughTrajectoryController::state_interface_configuration() const +{ + controller_interface::InterfaceConfiguration conf; + conf.type = controller_interface::interface_configuration_type::INDIVIDUAL; + + std::copy(joint_state_interface_names_.cbegin(), joint_state_interface_names_.cend(), std::back_inserter(conf.names)); + + conf.names.push_back(passthrough_params_.speed_scaling_interface_name); + + return conf; +} + +controller_interface::InterfaceConfiguration PassthroughTrajectoryController::command_interface_configuration() const +{ + controller_interface::InterfaceConfiguration config; + config.type = controller_interface::interface_configuration_type::INDIVIDUAL; + + const std::string tf_prefix = passthrough_params_.tf_prefix; + + for (size_t i = 0; i < number_of_joints_; ++i) { + config.names.emplace_back(tf_prefix + "trajectory_passthrough/setpoint_positions_" + std::to_string(i)); + config.names.emplace_back(tf_prefix + "trajectory_passthrough/setpoint_velocities_" + std::to_string(i)); + config.names.emplace_back(tf_prefix + "trajectory_passthrough/setpoint_accelerations_" + std::to_string(i)); + } + + 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"); + + return config; +} + +controller_interface::CallbackReturn PassthroughTrajectoryController::on_activate(const rclcpp_lifecycle::State& state) +{ + // clear out vectors in case of restart + joint_position_state_interface_.clear(); + joint_velocity_state_interface_.clear(); + joint_acceleration_state_interface_.clear(); + + for (auto& interface_name : joint_state_interface_names_) { + auto interface_it = std::find_if(state_interfaces_.begin(), state_interfaces_.end(), + [&](auto& interface) { return (interface.get_name() == interface_name); }); + if (interface_it != state_interfaces_.end()) { + if (interface_it->get_interface_name() == "position") { + joint_position_state_interface_.emplace_back(*interface_it); + } else if (interface_it->get_interface_name() == "velocity") { + joint_velocity_state_interface_.emplace_back(*interface_it); + } else if (interface_it->get_interface_name() == "acceleration") { + joint_acceleration_state_interface_.emplace_back(*interface_it); + } + } + } + + auto it = std::find_if(state_interfaces_.begin(), state_interfaces_.end(), [&](auto& interface) { + return (interface.get_name() == passthrough_params_.speed_scaling_interface_name); + }); + if (it != state_interfaces_.end()) { + scaling_state_interface_ = *it; + } else { + RCLCPP_ERROR(get_node()->get_logger(), "Did not find speed scaling interface in state interfaces."); + return controller_interface::CallbackReturn::ERROR; + } + + { + const std::string interface_name = passthrough_params_.tf_prefix + "trajectory_passthrough/" + "abort"; + auto it = std::find_if(command_interfaces_.begin(), command_interfaces_.end(), + [&](auto& interface) { return (interface.get_name() == interface_name); }); + if (it != command_interfaces_.end()) { + abort_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"; + auto it = std::find_if(command_interfaces_.begin(), command_interfaces_.end(), + [&](auto& interface) { return (interface.get_name() == interface_name); }); + if (it != command_interfaces_.end()) { + transfer_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 interface_name = tf_prefix + "trajectory_passthrough/time_from_start"; + auto it = std::find_if(command_interfaces_.begin(), command_interfaces_.end(), + [&](auto& interface) { return (interface.get_name() == interface_name); }); + if (it != command_interfaces_.end()) { + time_from_start_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; + } + } + + return ControllerInterface::on_activate(state); +} + +controller_interface::CallbackReturn PassthroughTrajectoryController::on_deactivate(const rclcpp_lifecycle::State&) +{ + abort_command_interface_->get().set_value(1.0); + if (trajectory_active_) { + const auto active_goal = *rt_active_goal_.readFromRT(); + std::shared_ptr result = + std::make_shared(); + result->set__error_string("Aborting current goal, since the controller is being deactivated."); + active_goal->setAborted(result); + rt_active_goal_.writeFromNonRT(RealtimeGoalHandlePtr()); + end_goal(); + } + return CallbackReturn::SUCCESS; +} + +controller_interface::return_type PassthroughTrajectoryController::update(const rclcpp::Time& /*time*/, + const rclcpp::Duration& period) +{ + const auto active_goal = *rt_active_goal_.readFromRT(); + + const auto current_transfer_state = transfer_command_interface_->get().get_value(); + + if (active_goal && trajectory_active_) { + if (current_transfer_state != TRANSFER_STATE_IDLE) { + // Check if the trajectory has been aborted from the hardware interface. E.g. the robot was stopped on the teach + // pendant. + if (abort_command_interface_->get().get_value() == 1.0 && current_index_ > 0) { + RCLCPP_INFO(get_node()->get_logger(), "Trajectory aborted by hardware, aborting action."); + std::shared_ptr result = + std::make_shared(); + active_goal->setAborted(result); + end_goal(); + return controller_interface::return_type::OK; + } + } + + active_joint_traj_ = active_goal->gh_->get_goal()->trajectory; + + if (current_index_ == 0 && current_transfer_state == TRANSFER_STATE_IDLE) { + 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)); + transfer_command_interface_->get().set_value(TRANSFER_STATE_WAITING_FOR_POINT); + } + auto active_goal_time_tol = goal_time_tolerance_.readFromRT(); + auto joint_mapping = joint_trajectory_mapping_.readFromRT(); + + // Write a new point to the command interface, if the previous point has been read by the hardware interface. + if (current_transfer_state == TRANSFER_STATE_WAITING_FOR_POINT) { + if (current_index_ < active_joint_traj_.points.size()) { + // Write the time_from_start parameter. + time_from_start_command_interface_->get().set_value( + duration_to_double(active_joint_traj_.points[current_index_].time_from_start)); + + // Write the positions for each joint of the robot + auto joint_names_internal = joint_names_.readFromRT(); + // We've added the joint interfaces matching the order of the joint names so we can safely access + // them by the index. + for (size_t i = 0; i < number_of_joints_; i++) { + command_interfaces_[i * 3].set_value( + active_joint_traj_.points[current_index_].positions[joint_mapping->at(joint_names_internal->at(i))]); + // Optionally, also write velocities and accelerations for each joint. + if (active_joint_traj_.points[current_index_].velocities.size() > 0) { + command_interfaces_[i * 3 + 1].set_value( + active_joint_traj_.points[current_index_].velocities[joint_mapping->at(joint_names_internal->at(i))]); + if (active_joint_traj_.points[current_index_].accelerations.size() > 0) { + command_interfaces_[i * 3 + 2].set_value( + active_joint_traj_.points[current_index_] + .accelerations[joint_mapping->at(joint_names_internal->at(i))]); + } else { + command_interfaces_[i * 3 + 2].set_value(NO_VAL); + } + } else { + command_interfaces_[i * 3 + 1].set_value(NO_VAL); + command_interfaces_[i * 3 + 2].set_value(NO_VAL); + } + } + // Tell hardware interface that this point is ready to be read. + transfer_command_interface_->get().set_value(TRANSFER_STATE_TRANSFERRING); + current_index_++; + // Check if all points have been written to the hardware interface. + } else if (current_index_ == active_joint_traj_.points.size()) { + transfer_command_interface_->get().set_value(TRANSFER_STATE_TRANSFER_DONE); + } else { + RCLCPP_ERROR(get_node()->get_logger(), "Hardware waiting for trajectory point while none is present!"); + } + + // When the trajectory is finished, report the goal as successful to the client. + } else if (current_transfer_state == TRANSFER_STATE_DONE) { + auto result = active_goal->preallocated_result_; + // Check if the actual position complies with the tolerances given. + if (!check_goal_tolerance()) { + result->error_code = control_msgs::action::FollowJointTrajectory::Result::GOAL_TOLERANCE_VIOLATED; + result->error_string = "Robot not within tolerances at end of trajectory."; + active_goal->setAborted(result); + end_goal(); + RCLCPP_ERROR(get_node()->get_logger(), "Trajectory failed, goal tolerances not met."); + } else if (active_goal_time_tol->nanoseconds() > 0 && + active_trajectory_elapsed_time_ > (max_trajectory_time_ + *active_goal_time_tol)) { + // Check if the goal time tolerance was complied with. + result->error_code = control_msgs::action::FollowJointTrajectory::Result::GOAL_TOLERANCE_VIOLATED; + result->error_string = + "Goal not reached within time tolerance. Missed goal time by " + + std::to_string((active_trajectory_elapsed_time_ - max_trajectory_time_ - *active_goal_time_tol).seconds()) + + " seconds."; + active_goal->setAborted(result); + end_goal(); + } else { + result->error_code = control_msgs::action::FollowJointTrajectory::Result::SUCCESSFUL; + result->error_string = "Trajectory executed successfully in " + + std::to_string(active_trajectory_elapsed_time_.seconds()) + + " (scaled) seconds! The real time needed for execution could be longer."; + active_goal->setSucceeded(result); + end_goal(); + RCLCPP_INFO(get_node()->get_logger(), "%s", result->error_string.c_str()); + } + } else if (current_transfer_state == TRANSFER_STATE_IN_MOTION) { + // Keep track of how long the trajectory has been executing, if it takes too long, send a warning. + if (scaling_state_interface_.has_value()) { + scaling_factor_ = scaling_state_interface_->get().get_value(); + } + + active_trajectory_elapsed_time_ = active_trajectory_elapsed_time_ + (period * scaling_factor_); + + // RCLCPP_INFO(get_node()->get_logger(), "Elapsed trajectory time: %f. Scaling factor: %f, period: %f", + // active_trajectory_elapsed_time_.seconds(), scaling_factor_, period.seconds()); + + if (active_trajectory_elapsed_time_ > (max_trajectory_time_ + *active_goal_time_tol) && trajectory_active_) { + RCLCPP_WARN_THROTTLE(get_node()->get_logger(), *clock_, 1000, + "Trajectory should be finished by now. You may want to cancel this goal, if it is not."); + } + } + } else if (current_transfer_state != TRANSFER_STATE_IDLE && current_transfer_state != TRANSFER_STATE_DONE) { + // No goal is active, but we are not in IDLE, either. We have been canceled. + abort_command_interface_->get().set_value(1.0); + + } else if (current_transfer_state == TRANSFER_STATE_DONE) { + // We have been informed about the finished trajectory. Let's reset things. + transfer_command_interface_->get().set_value(TRANSFER_STATE_IDLE); + abort_command_interface_->get().set_value(0.0); + } + + return controller_interface::return_type::OK; +} + +rclcpp_action::GoalResponse PassthroughTrajectoryController::goal_received_callback( + const rclcpp_action::GoalUUID& /*uuid*/, + std::shared_ptr goal) +{ + RCLCPP_INFO(get_node()->get_logger(), "Received new trajectory."); + // Precondition: Running controller + if (get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE) { + RCLCPP_ERROR(get_node()->get_logger(), "Can't accept new trajectories. Controller is not running."); + return rclcpp_action::GoalResponse::REJECT; + } + + if (trajectory_active_) { + RCLCPP_ERROR(get_node()->get_logger(), "Can't accept new trajectory. A trajectory is already executing."); + return rclcpp_action::GoalResponse::REJECT; + } + + // Check that all parts of the trajectory are valid. + if (!check_goal_positions(goal)) { + RCLCPP_ERROR(get_node()->get_logger(), "Trajectory rejected"); + return rclcpp_action::GoalResponse::REJECT; + } + if (!check_goal_velocities(goal)) { + RCLCPP_ERROR(get_node()->get_logger(), "Trajectory rejected"); + return rclcpp_action::GoalResponse::REJECT; + } + if (!check_goal_accelerations(goal)) { + RCLCPP_ERROR(get_node()->get_logger(), "Trajectory rejected"); + return rclcpp_action::GoalResponse::REJECT; + } + if (!check_goal_tolerances(goal)) { + RCLCPP_ERROR(get_node()->get_logger(), "Trajectory rejected"); + return rclcpp_action::GoalResponse::REJECT; + } + + return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; +} + +bool PassthroughTrajectoryController::check_goal_tolerances( + std::shared_ptr goal) +{ + auto& tolerances = goal->goal_tolerance; + auto joint_names_internal = joint_names_.readFromRT(); + if (!tolerances.empty()) { + for (auto& tol : tolerances) { + auto found_it = std::find(joint_names_internal->begin(), joint_names_internal->end(), tol.name); + if (found_it == joint_names_internal->end()) { + RCLCPP_ERROR(get_node()->get_logger(), + "Tolerance for joint '%s' given. This joint is not known to this controller.", tol.name.c_str()); + return false; + } + } + if (tolerances.size() != number_of_joints_) { + RCLCPP_ERROR(get_node()->get_logger(), "Tolerances for %lu joints given. This controller knows %lu joints.", + tolerances.size(), number_of_joints_.load()); + return false; + } + } + return true; +} + +bool PassthroughTrajectoryController::check_goal_positions( + std::shared_ptr goal) +{ + for (uint32_t i = 0; i < goal->trajectory.points.size(); i++) { + if (goal->trajectory.points[i].positions.size() != number_of_joints_) { + std::string msg; + msg = "Can't accept new trajectory. All trajectory points must have positions for all joints of the robot. (" + + std::to_string(number_of_joints_) + " joint positions per point)"; + RCLCPP_ERROR(get_node()->get_logger(), "%s", msg.c_str()); + msg = "Point nr " + std::to_string(i + 1) + + " has: " + std::to_string(goal->trajectory.points[i].positions.size()) + " positions."; + RCLCPP_ERROR(get_node()->get_logger(), "%s", msg.c_str()); + return false; + } + } + return true; +} + +bool PassthroughTrajectoryController::check_goal_velocities( + std::shared_ptr goal) +{ + for (uint32_t i = 0; i < goal->trajectory.points.size(); i++) { + if (goal->trajectory.points[i].velocities.size() != number_of_joints_ && + goal->trajectory.points[i].velocities.size() != 0) { + std::string msg; + msg = "Can't accept new trajectory. All trajectory points must either not have velocities or have them for all " + "joints of the robot. (" + + std::to_string(number_of_joints_) + " joint velocities per point)"; + RCLCPP_ERROR(get_node()->get_logger(), "%s", msg.c_str()); + msg = "Point nr " + std::to_string(i + 1) + + " has: " + std::to_string(goal->trajectory.points[i].velocities.size()) + " velocities."; + RCLCPP_ERROR(get_node()->get_logger(), "%s", msg.c_str()); + return false; + } + if (goal->trajectory.points[i].velocities.size() != goal->trajectory.points[0].velocities.size()) { + std::string msg; + msg = "Can't accept new trajectory. All trajectory points must have velocities for all joints of the robot. " + "(" + + std::to_string(number_of_joints_) + " joint velocities per point)"; + RCLCPP_ERROR(get_node()->get_logger(), "%s", msg.c_str()); + msg = "Point nr " + std::to_string(i) + " has: " + std::to_string(goal->trajectory.points[i].velocities.size()) + + " velocities."; + RCLCPP_ERROR(get_node()->get_logger(), "%s", msg.c_str()); + return false; + } + } + return true; +} + +bool PassthroughTrajectoryController::check_goal_accelerations( + std::shared_ptr goal) +{ + for (uint32_t i = 0; i < goal->trajectory.points.size(); i++) { + if (goal->trajectory.points[i].accelerations.size() != 0 && + goal->trajectory.points[i].accelerations.size() != number_of_joints_) { + std::string msg; + msg = "Can't accept new trajectory. All trajectory points must either not have accelerations or have them for " + "all joints of the robot. (" + + std::to_string(number_of_joints_) + " joint accelerations per point)"; + RCLCPP_ERROR(get_node()->get_logger(), "%s", msg.c_str()); + msg = "Point nr " + std::to_string(i) + + " has: " + std::to_string(goal->trajectory.points[i].accelerations.size()) + " accelerations."; + RCLCPP_ERROR(get_node()->get_logger(), "%s", msg.c_str()); + return false; + } + if (goal->trajectory.points[i].accelerations.size() != goal->trajectory.points[0].accelerations.size()) { + std::string msg; + msg = "Can't accept new trajectory. All trajectory points must have accelerations for all joints of the " + "robot. " + "(" + + std::to_string(number_of_joints_) + " joint accelerations per point)"; + RCLCPP_ERROR(get_node()->get_logger(), "%s", msg.c_str()); + msg = "Point nr " + std::to_string(i) + + " has: " + std::to_string(goal->trajectory.points[i].accelerations.size()) + " accelerations."; + RCLCPP_ERROR(get_node()->get_logger(), "%s", msg.c_str()); + return false; + } + } + return true; +} + +rclcpp_action::CancelResponse PassthroughTrajectoryController::goal_cancelled_callback( + const std::shared_ptr> goal_handle) +{ + // Check that cancel request refers to currently active goal (if any) + const auto active_goal = *rt_active_goal_.readFromNonRT(); + if (active_goal && active_goal->gh_ == goal_handle) { + RCLCPP_INFO(get_node()->get_logger(), "Cancelling active trajectory requested."); + + // Mark the current goal as canceled + auto result = std::make_shared(); + active_goal->setCanceled(result); + rt_active_goal_.writeFromNonRT(RealtimeGoalHandlePtr()); + trajectory_active_ = false; + } + return rclcpp_action::CancelResponse::ACCEPT; +} + +// Action goal was accepted, initialise values for a new trajectory. +void PassthroughTrajectoryController::goal_accepted_callback( + std::shared_ptr> goal_handle) +{ + RCLCPP_INFO_STREAM(get_node()->get_logger(), "Accepted new trajectory with " + << goal_handle->get_goal()->trajectory.points.size() << " points."); + current_index_ = 0; + + // TODO(fexner): Merge goal tolerances with default tolerances + + joint_trajectory_mapping_.writeFromNonRT(create_joint_mapping(goal_handle->get_goal()->trajectory.joint_names)); + + // sort goal tolerances to match internal joint order + std::vector goal_tolerances; + if (!goal_handle->get_goal()->goal_tolerance.empty()) { + auto joint_names_internal = joint_names_.readFromRT(); + std::stringstream ss; + ss << "Using goal tolerances\n"; + for (auto& joint_name : *joint_names_internal) { + auto found_it = + std::find_if(goal_handle->get_goal()->goal_tolerance.begin(), goal_handle->get_goal()->goal_tolerance.end(), + [&joint_name](auto& tol) { return tol.name == joint_name; }); + if (found_it != goal_handle->get_goal()->goal_tolerance.end()) { + goal_tolerances.push_back(*found_it); + ss << joint_name << " -- position: " << found_it->position << ", velocity: " << found_it->velocity + << ", acceleration: " << found_it->acceleration << std::endl; + } + } + RCLCPP_INFO_STREAM(get_node()->get_logger(), ss.str()); + } + goal_tolerance_.writeFromNonRT(goal_tolerances); + goal_time_tolerance_.writeFromNonRT(goal_handle->get_goal()->goal_time_tolerance); + RCLCPP_INFO_STREAM(get_node()->get_logger(), + "Goal time tolerance: " << duration_to_double(goal_handle->get_goal()->goal_time_tolerance) + << " sec"); + + // Action handling will be done from the timer callback to avoid those things in the realtime + // thread. First, we delete the existing (if any) timer by resetting the pointer and then create a new + // one. + // + RealtimeGoalHandlePtr rt_goal = std::make_shared(goal_handle); + rt_goal->execute(); + rt_active_goal_.writeFromNonRT(rt_goal); + goal_handle_timer_.reset(); + goal_handle_timer_ = get_node()->create_wall_timer(action_monitor_period_.to_chrono(), + std::bind(&RealtimeGoalHandle::runNonRealtime, rt_goal)); + trajectory_active_ = true; + return; +} + +bool PassthroughTrajectoryController::check_goal_tolerance() +{ + auto goal_tolerance = goal_tolerance_.readFromRT(); + auto joint_mapping = joint_trajectory_mapping_.readFromRT(); + auto joint_names_internal = joint_names_.readFromRT(); + if (goal_tolerance->empty()) { + return true; + } + + for (size_t i = 0; i < number_of_joints_; ++i) { + const std::string joint_name = joint_names_internal->at(i); + const auto& joint_tol = goal_tolerance->at(i); + const auto& setpoint = active_joint_traj_.points.back().positions[joint_mapping->at(joint_name)]; + const double joint_pos = joint_position_state_interface_[i].get().get_value(); + if (std::abs(joint_pos - setpoint) > joint_tol.position) { + // RCLCPP_ERROR( + // get_node()->get_logger(), "Joint %s should be at position %f, but is at position %f, where tolerance is %f", + // joint_position_state_interface_[i].get().get_name().c_str(), setpoint, joint_pos, joint_tol.position); + return false; + } + + if (!active_joint_traj_.points.back().velocities.empty()) { + const double joint_vel = joint_velocity_state_interface_[i].get().get_value(); + const auto& expected_vel = active_joint_traj_.points.back().velocities[joint_mapping->at(joint_name)]; + if (std::abs(joint_vel - expected_vel) > joint_tol.velocity) { + return false; + } + } + if (!active_joint_traj_.points.back().accelerations.empty()) { + const double joint_vel = joint_acceleration_state_interface_[i].get().get_value(); + const auto& expected_vel = active_joint_traj_.points.back().accelerations[joint_mapping->at(joint_name)]; + if (std::abs(joint_vel - expected_vel) > joint_tol.acceleration) { + return false; + } + } + } + + return true; +} + +void PassthroughTrajectoryController::end_goal() +{ + trajectory_active_ = false; + transfer_command_interface_->get().set_value(TRANSFER_STATE_IDLE); +} + +std::unordered_map +PassthroughTrajectoryController::create_joint_mapping(const std::vector& joint_names) const +{ + std::unordered_map joint_mapping; + auto joint_names_internal = joint_names_.readFromNonRT(); + for (auto& joint_name : *joint_names_internal) { + auto found_it = std::find(joint_names.begin(), joint_names.end(), joint_name); + if (found_it != joint_names.end()) { + joint_mapping.insert({ joint_name, found_it - joint_names.begin() }); + } + } + return joint_mapping; +} +} // namespace ur_controllers + +#include "pluginlib/class_list_macros.hpp" + +PLUGINLIB_EXPORT_CLASS(ur_controllers::PassthroughTrajectoryController, controller_interface::ControllerInterface) diff --git a/ur_controllers/src/passthrough_trajectory_controller_parameters.yaml b/ur_controllers/src/passthrough_trajectory_controller_parameters.yaml new file mode 100644 index 000000000..db8844338 --- /dev/null +++ b/ur_controllers/src/passthrough_trajectory_controller_parameters.yaml @@ -0,0 +1,31 @@ +--- +passthrough_trajectory_controller: + speed_scaling_interface_name: { + type: string, + default_value: "speed_scaling/speed_scaling_factor", + description: "Fully qualified name of the speed scaling interface name" + } + tf_prefix: { + type: string, + default_value: "", + description: "Urdf prefix of the corresponding arm" + } + joints: { + type: string_array, + default_value: [], + description: "Joint names to claim and listen to", + read_only: true, + validation: { + unique<>: null, + } + } + state_interfaces: { + type: string_array, + default_value: [], + description: "State interfaces provided by the hardware for all joints.", + read_only: true, + validation: { + unique<>: null, + subset_of<>: [["position", "velocity", "acceleration"]], + } + } diff --git a/ur_robot_driver/CMakeLists.txt b/ur_robot_driver/CMakeLists.txt index 6577e8e87..26f533b3f 100644 --- a/ur_robot_driver/CMakeLists.txt +++ b/ur_robot_driver/CMakeLists.txt @@ -156,6 +156,7 @@ ament_python_install_package(${PROJECT_NAME}) # Install Python executables install(PROGRAMS scripts/tool_communication.py + scripts/example_move.py DESTINATION lib/${PROJECT_NAME} ) @@ -192,6 +193,10 @@ if(BUILD_TESTING) TIMEOUT 800 ) + add_launch_test(test/integration_test_controller_switch.py + TIMEOUT + 800 + ) add_launch_test(test/urscript_interface.py TIMEOUT 500 diff --git a/ur_robot_driver/config/ur_controllers.yaml b/ur_robot_driver/config/ur_controllers.yaml index ceb56e5ef..8c7e30b96 100644 --- a/ur_robot_driver/config/ur_controllers.yaml +++ b/ur_robot_driver/config/ur_controllers.yaml @@ -27,6 +27,9 @@ controller_manager: tcp_pose_broadcaster: type: pose_broadcaster/PoseBroadcaster + passthrough_trajectory_controller: + type: ur_controllers/PassthroughTrajectoryController + ur_configuration_controller: type: ur_controllers/URConfigurationController @@ -113,6 +116,21 @@ scaled_joint_trajectory_controller: $(var tf_prefix)wrist_3_joint: { trajectory: 0.2, goal: 0.1 } speed_scaling_interface_name: $(var tf_prefix)speed_scaling/speed_scaling_factor +passthrough_trajectory_controller: + ros__parameters: + tf_prefix: "$(var tf_prefix)" + joints: + - $(var tf_prefix)shoulder_pan_joint + - $(var tf_prefix)shoulder_lift_joint + - $(var tf_prefix)elbow_joint + - $(var tf_prefix)wrist_1_joint + - $(var tf_prefix)wrist_2_joint + - $(var tf_prefix)wrist_3_joint + state_interfaces: + - position + - velocity + speed_scaling_interface_name: $(var tf_prefix)speed_scaling/speed_scaling_factor + forward_velocity_controller: ros__parameters: joints: diff --git a/ur_robot_driver/include/ur_robot_driver/hardware_interface.hpp b/ur_robot_driver/include/ur_robot_driver/hardware_interface.hpp index 515f19999..428456113 100644 --- a/ur_robot_driver/include/ur_robot_driver/hardware_interface.hpp +++ b/ur_robot_driver/include/ur_robot_driver/hardware_interface.hpp @@ -77,7 +77,8 @@ enum StoppingInterface { NONE, STOP_POSITION, - STOP_VELOCITY + STOP_VELOCITY, + STOP_PASSTHROUGH }; // We define our own quaternion to use it as a buffer, since we need to pass pointers to the state @@ -157,6 +158,10 @@ class URPositionHardwareInterface : public hardware_interface::SystemInterface void updateNonDoubleValues(); void extractToolPose(); void transformForceTorque(); + void check_passthrough_trajectory_controller(); + void trajectory_done_callback(urcl::control::TrajectoryResult result); + bool has_accelerations(std::vector> accelerations); + bool has_velocities(std::vector> velocities); urcl::vector6d_t urcl_position_commands_; urcl::vector6d_t urcl_position_commands_old_; @@ -220,6 +225,14 @@ class URPositionHardwareInterface : public hardware_interface::SystemInterface double get_robot_software_version_bugfix_; double get_robot_software_version_build_; + // Passthrough trajectory controller interface values + double passthrough_trajectory_transfer_state_; + double passthrough_trajectory_abort_; + bool passthrough_trajectory_controller_running_; + urcl::vector6d_t passthrough_trajectory_positions_; + urcl::vector6d_t passthrough_trajectory_velocities_; + urcl::vector6d_t passthrough_trajectory_accelerations_; + double passthrough_trajectory_time_from_start_; // payload stuff urcl::vector3d_t payload_center_of_gravity_; double payload_mass_; @@ -241,6 +254,13 @@ class URPositionHardwareInterface : public hardware_interface::SystemInterface bool non_blocking_read_; double robot_program_running_copy_; + /* Vectors used to store the trajectory received from the passthrough trajectory controller. The whole trajectory is + * received before it is sent to the robot. */ + std::vector> trajectory_joint_positions_; + std::vector> trajectory_joint_velocities_; + std::vector> trajectory_joint_accelerations_; + std::vector trajectory_times_; + PausingState pausing_state_; double pausing_ramp_up_increment_; @@ -256,6 +276,8 @@ class URPositionHardwareInterface : public hardware_interface::SystemInterface std::atomic_bool rtde_comm_has_been_started_ = false; urcl::RobotReceiveTimeout receive_timeout_ = urcl::RobotReceiveTimeout::millisec(20); + + const std::string PASSTHROUGH_GPIO = "trajectory_passthrough"; }; } // namespace ur_robot_driver diff --git a/ur_robot_driver/launch/ur10.launch.py b/ur_robot_driver/launch/ur10.launch.py index 28710f631..31c4fc82c 100644 --- a/ur_robot_driver/launch/ur10.launch.py +++ b/ur_robot_driver/launch/ur10.launch.py @@ -69,6 +69,7 @@ def generate_launch_description(): "joint_trajectory_controller", "forward_velocity_controller", "forward_position_controller", + "passthrough_trajectory_controller", ], ) ) diff --git a/ur_robot_driver/launch/ur10e.launch.py b/ur_robot_driver/launch/ur10e.launch.py index 09daf55c1..7009abfd2 100644 --- a/ur_robot_driver/launch/ur10e.launch.py +++ b/ur_robot_driver/launch/ur10e.launch.py @@ -69,6 +69,7 @@ def generate_launch_description(): "joint_trajectory_controller", "forward_velocity_controller", "forward_position_controller", + "passthrough_trajectory_controller", ], ) ) diff --git a/ur_robot_driver/launch/ur16e.launch.py b/ur_robot_driver/launch/ur16e.launch.py index 208c97e4a..02a56fd59 100644 --- a/ur_robot_driver/launch/ur16e.launch.py +++ b/ur_robot_driver/launch/ur16e.launch.py @@ -69,6 +69,7 @@ def generate_launch_description(): "joint_trajectory_controller", "forward_velocity_controller", "forward_position_controller", + "passthrough_trajectory_controller", ], ) ) diff --git a/ur_robot_driver/launch/ur20.launch.py b/ur_robot_driver/launch/ur20.launch.py index 32e4c3677..3f9ad3a91 100644 --- a/ur_robot_driver/launch/ur20.launch.py +++ b/ur_robot_driver/launch/ur20.launch.py @@ -69,6 +69,7 @@ def generate_launch_description(): "joint_trajectory_controller", "forward_velocity_controller", "forward_position_controller", + "passthrough_trajectory_controller", ], ) ) diff --git a/ur_robot_driver/launch/ur3.launch.py b/ur_robot_driver/launch/ur3.launch.py index 6a28969b6..feaa54c56 100644 --- a/ur_robot_driver/launch/ur3.launch.py +++ b/ur_robot_driver/launch/ur3.launch.py @@ -69,6 +69,7 @@ def generate_launch_description(): "joint_trajectory_controller", "forward_velocity_controller", "forward_position_controller", + "passthrough_trajectory_controller", ], ) ) diff --git a/ur_robot_driver/launch/ur30.launch.py b/ur_robot_driver/launch/ur30.launch.py index 04b0d1e10..3822f5e2c 100644 --- a/ur_robot_driver/launch/ur30.launch.py +++ b/ur_robot_driver/launch/ur30.launch.py @@ -69,6 +69,7 @@ def generate_launch_description(): "joint_trajectory_controller", "forward_velocity_controller", "forward_position_controller", + "passthrough_trajectory_controller", ], ) ) diff --git a/ur_robot_driver/launch/ur3e.launch.py b/ur_robot_driver/launch/ur3e.launch.py index 10d2eefd8..cbb3b2a5e 100644 --- a/ur_robot_driver/launch/ur3e.launch.py +++ b/ur_robot_driver/launch/ur3e.launch.py @@ -69,6 +69,7 @@ def generate_launch_description(): "joint_trajectory_controller", "forward_velocity_controller", "forward_position_controller", + "passthrough_trajectory_controller", ], ) ) diff --git a/ur_robot_driver/launch/ur5.launch.py b/ur_robot_driver/launch/ur5.launch.py index fdda097e0..6281bbf90 100644 --- a/ur_robot_driver/launch/ur5.launch.py +++ b/ur_robot_driver/launch/ur5.launch.py @@ -69,6 +69,7 @@ def generate_launch_description(): "joint_trajectory_controller", "forward_velocity_controller", "forward_position_controller", + "passthrough_trajectory_controller", ], ) ) diff --git a/ur_robot_driver/launch/ur5e.launch.py b/ur_robot_driver/launch/ur5e.launch.py index f42e66c48..372bf2ef3 100644 --- a/ur_robot_driver/launch/ur5e.launch.py +++ b/ur_robot_driver/launch/ur5e.launch.py @@ -69,6 +69,7 @@ def generate_launch_description(): "joint_trajectory_controller", "forward_velocity_controller", "forward_position_controller", + "passthrough_trajectory_controller", ], ) ) diff --git a/ur_robot_driver/launch/ur_control.launch.py b/ur_robot_driver/launch/ur_control.launch.py index 970c30d2d..f98d7a880 100644 --- a/ur_robot_driver/launch/ur_control.launch.py +++ b/ur_robot_driver/launch/ur_control.launch.py @@ -342,39 +342,21 @@ def controller_spawner(controllers, active=True): "tcp_pose_broadcaster", "ur_configuration_controller", ] - controllers_inactive = ["forward_position_controller"] + controllers_inactive = [ + "scaled_joint_trajectory_controller", + "joint_trajectory_controller", + "forward_velocity_controller", + "forward_position_controller", + "passthrough_trajectory_controller", + ] + if activate_joint_controller.perform(context) == "true": + controllers_active.append(initial_joint_controller.perform(context)) + controllers_inactive.remove(initial_joint_controller.perform(context)) controller_spawners = [controller_spawner(controllers_active)] + [ controller_spawner(controllers_inactive, active=False) ] - # There may be other controllers of the joints, but this is the initially-started one - initial_joint_controller_spawner_started = Node( - package="controller_manager", - executable="spawner", - arguments=[ - initial_joint_controller, - "-c", - "/controller_manager", - "--controller-manager-timeout", - controller_spawner_timeout, - ], - condition=IfCondition(activate_joint_controller), - ) - initial_joint_controller_spawner_stopped = Node( - package="controller_manager", - executable="spawner", - arguments=[ - initial_joint_controller, - "-c", - "/controller_manager", - "--controller-manager-timeout", - controller_spawner_timeout, - "--inactive", - ], - condition=UnlessCondition(activate_joint_controller), - ) - nodes_to_start = [ control_node, ur_control_node, @@ -384,8 +366,6 @@ def controller_spawner(controllers, active=True): urscript_interface, robot_state_publisher_node, rviz_node, - initial_joint_controller_spawner_stopped, - initial_joint_controller_spawner_started, ] + controller_spawners return nodes_to_start @@ -514,6 +494,13 @@ def generate_launch_description(): DeclareLaunchArgument( "initial_joint_controller", default_value="scaled_joint_trajectory_controller", + choices=[ + "scaled_joint_trajectory_controller", + "joint_trajectory_controller", + "forward_velocity_controller", + "forward_position_controller", + "passthrough_trajectory_controller", + ], description="Initially loaded robot controller.", ) ) diff --git a/ur_robot_driver/scripts/example_move.py b/ur_robot_driver/scripts/example_move.py new file mode 100755 index 000000000..c77836a72 --- /dev/null +++ b/ur_robot_driver/scripts/example_move.py @@ -0,0 +1,219 @@ +#!/usr/bin/env python3 +# Copyright (c) 2024 FZI Forschungszentrum Informatik +# +# 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. + +# +# Author: Felix Exner + +# This is an example of how to interface the robot without any additional ROS components. For +# real-life applications, we do recommend to use something like MoveIt! + +import time + +import rclpy +from rclpy.action import ActionClient + +from builtin_interfaces.msg import Duration +from action_msgs.msg import GoalStatus +from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint +from control_msgs.action import FollowJointTrajectory +from control_msgs.msg import JointTolerance + +TRAJECTORIES = { + "traj0": [ + { + "positions": [0.043128, -1.28824, 1.37179, -1.82208, -1.63632, -0.18], + "velocities": [0.0, 0.0, 0.0, 0.0, 0.0, 0.0], + "time_from_start": Duration(sec=4, nanosec=0), + }, + { + "positions": [-0.195016, -1.70093, 0.902027, -0.944217, -1.52982, -0.195171], + "velocities": [0.0, 0.0, 0.0, 0.0, 0.0, 0.0], + "time_from_start": Duration(sec=8, nanosec=0), + }, + ], + "traj1": [ + { + "positions": [-0.195016, -1.70094, 0.902027, -0.944217, -1.52982, -0.195171], + "velocities": [0.0, 0.0, 0.0, 0.0, 0.0, 0.0], + "time_from_start": Duration(sec=0, nanosec=0), + }, + { + "positions": [0.30493, -0.982258, 0.955637, -1.48215, -1.72737, 0.204445], + "velocities": [0.0, 0.0, 0.0, 0.0, 0.0, 0.0], + "time_from_start": Duration(sec=8, nanosec=0), + }, + ], +} + + +class JTCClient(rclpy.node.Node): + """Small test client for the jtc.""" + + def __init__(self): + super().__init__("jtc_client") + self.declare_parameter("controller_name", "scaled_joint_trajectory_controller") + self.declare_parameter( + "joints", + [ + "shoulder_pan_joint", + "shoulder_lift_joint", + "elbow_joint", + "wrist_1_joint", + "wrist_2_joint", + "wrist_3_joint", + ], + ) + + controller_name = self.get_parameter("controller_name").value + "/follow_joint_trajectory" + self.joints = self.get_parameter("joints").value + + if self.joints is None or len(self.joints) == 0: + raise Exception('"joints" parameter is required') + + self._action_client = ActionClient(self, FollowJointTrajectory, controller_name) + self.get_logger().info(f"Waiting for action server on {controller_name}") + self._action_client.wait_for_server() + + self.parse_trajectories() + self.i = 0 + self._send_goal_future = None + self._get_result_future = None + self.execute_next_trajectory() + + def parse_trajectories(self): + self.goals = {} + + for traj_name in TRAJECTORIES: + goal = JointTrajectory() + goal.joint_names = self.joints + for pt in TRAJECTORIES[traj_name]: + point = JointTrajectoryPoint() + point.positions = pt["positions"] + point.velocities = pt["velocities"] + point.time_from_start = pt["time_from_start"] + goal.points.append(point) + + self.goals[traj_name] = goal + + def execute_next_trajectory(self): + if self.i >= len(self.goals): + self.get_logger().info("Done with all trajectories") + raise SystemExit + traj_name = list(self.goals)[self.i] + self.i = self.i + 1 + if traj_name: + self.execute_trajectory(traj_name) + + def execute_trajectory(self, traj_name): + self.get_logger().info(f"Executing trajectory {traj_name}") + goal = FollowJointTrajectory.Goal() + goal.trajectory = self.goals[traj_name] + + goal.goal_time_tolerance = Duration(sec=0, nanosec=500000000) + goal.goal_tolerance = [ + JointTolerance(position=0.01, velocity=0.01, name=self.joints[i]) for i in range(6) + ] + + self._send_goal_future = self._action_client.send_goal_async(goal) + self._send_goal_future.add_done_callback(self.goal_response_callback) + + def goal_response_callback(self, future): + goal_handle = future.result() + if not goal_handle.accepted: + self.get_logger().error("Goal rejected :(") + raise RuntimeError("Goal rejected :(") + + self.get_logger().debug("Goal accepted :)") + + self._get_result_future = goal_handle.get_result_async() + self._get_result_future.add_done_callback(self.get_result_callback) + + def get_result_callback(self, future): + result = future.result().result + status = future.result().status + self.get_logger().info(f"Done with result: {self.status_to_str(status)}") + if status == GoalStatus.STATUS_SUCCEEDED: + time.sleep(2) + self.execute_next_trajectory() + else: + if result.error_code != FollowJointTrajectory.Result.SUCCESSFUL: + self.get_logger().error( + f"Done with result: {self.error_code_to_str(result.error_code)}" + ) + raise RuntimeError("Executing trajectory failed. " + result.error_string) + + @staticmethod + def error_code_to_str(error_code): + if error_code == FollowJointTrajectory.Result.SUCCESSFUL: + return "SUCCESSFUL" + if error_code == FollowJointTrajectory.Result.INVALID_GOAL: + return "INVALID_GOAL" + if error_code == FollowJointTrajectory.Result.INVALID_JOINTS: + return "INVALID_JOINTS" + if error_code == FollowJointTrajectory.Result.OLD_HEADER_TIMESTAMP: + return "OLD_HEADER_TIMESTAMP" + if error_code == FollowJointTrajectory.Result.PATH_TOLERANCE_VIOLATED: + return "PATH_TOLERANCE_VIOLATED" + if error_code == FollowJointTrajectory.Result.GOAL_TOLERANCE_VIOLATED: + return "GOAL_TOLERANCE_VIOLATED" + + @staticmethod + def status_to_str(error_code): + if error_code == GoalStatus.STATUS_UNKNOWN: + return "UNKNOWN" + if error_code == GoalStatus.STATUS_ACCEPTED: + return "ACCEPTED" + if error_code == GoalStatus.STATUS_EXECUTING: + return "EXECUTING" + if error_code == GoalStatus.STATUS_CANCELING: + return "CANCELING" + if error_code == GoalStatus.STATUS_SUCCEEDED: + return "SUCCEEDED" + if error_code == GoalStatus.STATUS_CANCELED: + return "CANCELED" + if error_code == GoalStatus.STATUS_ABORTED: + return "ABORTED" + + +def main(args=None): + rclpy.init(args=args) + + jtc_client = JTCClient() + try: + rclpy.spin(jtc_client) + except RuntimeError as err: + jtc_client.get_logger().error(str(err)) + except SystemExit: + rclpy.logging.get_logger("jtc_client").info("Done") + + rclpy.shutdown() + + +if __name__ == "__main__": + main() diff --git a/ur_robot_driver/src/hardware_interface.cpp b/ur_robot_driver/src/hardware_interface.cpp index e88eabb7b..30285f9a2 100644 --- a/ur_robot_driver/src/hardware_interface.cpp +++ b/ur_robot_driver/src/hardware_interface.cpp @@ -46,7 +46,7 @@ #include "ur_client_library/ur/tool_communication.h" #include "ur_client_library/ur/version_information.h" -#include "rclcpp/rclcpp.hpp" +#include #include "hardware_interface/types/hardware_interface_type_values.hpp" #include "ur_robot_driver/hardware_interface.hpp" #include "ur_robot_driver/urcl_log_handler.hpp" @@ -86,6 +86,7 @@ URPositionHardwareInterface::on_init(const hardware_interface::HardwareInfo& sys start_modes_ = {}; position_controller_running_ = false; velocity_controller_running_ = false; + passthrough_trajectory_controller_running_ = false; runtime_state_ = static_cast(rtde::RUNTIME_STATE::STOPPED); pausing_state_ = PausingState::RUNNING; pausing_ramp_up_increment_ = 0.01; @@ -94,6 +95,11 @@ URPositionHardwareInterface::on_init(const hardware_interface::HardwareInfo& sys initialized_ = false; async_thread_shutdown_ = false; system_interface_initialized_ = 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(); for (const hardware_interface::ComponentInfo& joint : info_.joints) { if (joint.command_interfaces.size() != 2) { @@ -144,7 +150,6 @@ URPositionHardwareInterface::on_init(const hardware_interface::HardwareInfo& sys return hardware_interface::CallbackReturn::ERROR; } } - return hardware_interface::CallbackReturn::SUCCESS; } @@ -333,6 +338,32 @@ std::vector URPositionHardwareInterface::e command_interfaces.emplace_back(hardware_interface::CommandInterface( tf_prefix + "zero_ftsensor", "zero_ftsensor_async_success", &zero_ftsensor_async_success_)); + command_interfaces.emplace_back(hardware_interface::CommandInterface(tf_prefix + PASSTHROUGH_GPIO, "transfer_state", + &passthrough_trajectory_transfer_state_)); + + command_interfaces.emplace_back(hardware_interface::CommandInterface(tf_prefix + PASSTHROUGH_GPIO, "time_from_start", + &passthrough_trajectory_time_from_start_)); + command_interfaces.emplace_back( + hardware_interface::CommandInterface(tf_prefix + PASSTHROUGH_GPIO, "abort", &passthrough_trajectory_abort_)); + + 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), + &passthrough_trajectory_positions_[i])); + } + + for (size_t i = 0; i < 6; ++i) { + command_interfaces.emplace_back(hardware_interface::CommandInterface(tf_prefix + PASSTHROUGH_GPIO, + "setpoint_velocities_" + std::to_string(i), + &passthrough_trajectory_velocities_[i])); + } + + for (size_t i = 0; i < 6; ++i) { + command_interfaces.emplace_back(hardware_interface::CommandInterface(tf_prefix + PASSTHROUGH_GPIO, + "setpoint_accelerations_" + std::to_string(i), + &passthrough_trajectory_accelerations_[i])); + } + return command_interfaces; } @@ -500,6 +531,9 @@ URPositionHardwareInterface::on_configure(const rclcpp_lifecycle::State& previou RCLCPP_INFO(rclcpp::get_logger("URPositionHardwareInterface"), "System successfully started!"); + ur_driver_->registerTrajectoryDoneCallback( + std::bind(&URPositionHardwareInterface::trajectory_done_callback, this, std::placeholders::_1)); + return hardware_interface::CallbackReturn::SUCCESS; } @@ -674,6 +708,9 @@ hardware_interface::return_type URPositionHardwareInterface::write(const rclcpp: } else if (velocity_controller_running_) { ur_driver_->writeJointCommand(urcl_velocity_commands_, urcl::comm::ControlMode::MODE_SPEEDJ, receive_timeout_); + } else if (passthrough_trajectory_controller_running_) { + ur_driver_->writeTrajectoryControlMessage(urcl::control::TrajectoryControlMessage::TRAJECTORY_NOOP); + check_passthrough_trajectory_controller(); } else { ur_driver_->writeKeepalive(); } @@ -843,30 +880,53 @@ hardware_interface::return_type URPositionHardwareInterface::prepare_command_mod { hardware_interface::return_type ret_val = hardware_interface::return_type::OK; - start_modes_.clear(); + start_modes_ = std::vector(info_.joints.size(), "UNDEFINED"); stop_modes_.clear(); + std::vector control_modes(info_.joints.size()); + const std::string tf_prefix = info_.hardware_parameters.at("tf_prefix"); + + // Assess current state + for (auto i = 0u; i < info_.joints.size(); i++) { + if (position_controller_running_) { + control_modes[i] = hardware_interface::HW_IF_POSITION; + } + if (velocity_controller_running_) { + control_modes[i] = hardware_interface::HW_IF_VELOCITY; + } + if (passthrough_trajectory_controller_running_) { + control_modes[i] = PASSTHROUGH_GPIO; + } + } + + if (!std::all_of(start_modes_.begin() + 1, start_modes_.end(), + [&](const std::string& other) { return other == start_modes_[0]; })) { + RCLCPP_ERROR(rclcpp::get_logger("URPositionHardwareInterface"), "Start modes of all joints have to be the same."); + return hardware_interface::return_type::ERROR; + } // Starting interfaces - // add start interface per joint in tmp var for later check + // If a joint has been reserved already, raise an error. + // Modes that are not directly mapped to a single joint such as force_mode reserve all joints. for (const auto& key : start_interfaces) { for (auto i = 0u; i < info_.joints.size(); i++) { if (key == info_.joints[i].name + "/" + hardware_interface::HW_IF_POSITION) { - start_modes_.push_back(hardware_interface::HW_IF_POSITION); - } - if (key == info_.joints[i].name + "/" + hardware_interface::HW_IF_VELOCITY) { - start_modes_.push_back(hardware_interface::HW_IF_VELOCITY); + if (start_modes_[i] != "UNDEFINED") { + return hardware_interface::return_type::ERROR; + } + start_modes_[i] = hardware_interface::HW_IF_POSITION; + } else if (key == info_.joints[i].name + "/" + hardware_interface::HW_IF_VELOCITY) { + if (start_modes_[i] != "UNDEFINED") { + return hardware_interface::return_type::ERROR; + } + start_modes_[i] = hardware_interface::HW_IF_VELOCITY; + } else if (key == tf_prefix + PASSTHROUGH_GPIO + "/setpoint_positions_" + std::to_string(i)) { + if (start_modes_[i] != "UNDEFINED") { + return hardware_interface::return_type::ERROR; + } + start_modes_[i] = PASSTHROUGH_GPIO; } } } - // set new mode to all interfaces at the same time - if (start_modes_.size() != 0 && start_modes_.size() != 6) { - ret_val = hardware_interface::return_type::ERROR; - } - - // all start interfaces must be the same - can't mix position and velocity control - if (start_modes_.size() != 0 && !std::equal(start_modes_.begin() + 1, start_modes_.end(), start_modes_.begin())) { - ret_val = hardware_interface::return_type::ERROR; - } // Stopping interfaces // add stop interface per joint in tmp var for later check @@ -874,15 +934,55 @@ hardware_interface::return_type URPositionHardwareInterface::prepare_command_mod for (auto i = 0u; i < info_.joints.size(); i++) { if (key == info_.joints[i].name + "/" + hardware_interface::HW_IF_POSITION) { stop_modes_.push_back(StoppingInterface::STOP_POSITION); + if (control_modes[i] == hardware_interface::HW_IF_POSITION) { + control_modes[i] = "UNDEFINED"; + } } if (key == info_.joints[i].name + "/" + hardware_interface::HW_IF_VELOCITY) { stop_modes_.push_back(StoppingInterface::STOP_VELOCITY); + if (control_modes[i] == hardware_interface::HW_IF_VELOCITY) { + control_modes[i] = "UNDEFINED"; + } + } + if (key == tf_prefix + PASSTHROUGH_GPIO + "/setpoint_positions_" + std::to_string(i)) { + stop_modes_.push_back(StoppingInterface::STOP_PASSTHROUGH); + if (control_modes[i] == PASSTHROUGH_GPIO) { + control_modes[i] = "UNDEFINED"; + } } } } - // stop all interfaces at the same time - if (stop_modes_.size() != 0 && - (stop_modes_.size() != 6 || !std::equal(stop_modes_.begin() + 1, stop_modes_.end(), stop_modes_.begin()))) { + + // Do not start conflicting controllers + if (std::any_of(start_modes_.begin(), start_modes_.end(), + [this](auto& item) { return (item == PASSTHROUGH_GPIO); }) && + (std::any_of(start_modes_.begin(), start_modes_.end(), + [](auto& item) { + return (item == hardware_interface::HW_IF_VELOCITY || item == hardware_interface::HW_IF_POSITION); + }) || + std::any_of(control_modes.begin(), control_modes.end(), [this](auto& item) { + return (item == hardware_interface::HW_IF_VELOCITY || item == hardware_interface::HW_IF_POSITION || + item == PASSTHROUGH_GPIO); + }))) { + ret_val = hardware_interface::return_type::ERROR; + } + if (std::any_of(start_modes_.begin(), start_modes_.end(), + [](auto& item) { return (item == hardware_interface::HW_IF_POSITION); }) && + (std::any_of( + start_modes_.begin(), start_modes_.end(), + [this](auto& item) { return (item == hardware_interface::HW_IF_VELOCITY || item == PASSTHROUGH_GPIO); }) || + std::any_of(control_modes.begin(), control_modes.end(), [this](auto& item) { + return (item == hardware_interface::HW_IF_VELOCITY || item == hardware_interface::HW_IF_POSITION || + item == PASSTHROUGH_GPIO); + }))) { + ret_val = hardware_interface::return_type::ERROR; + } + if (std::any_of(start_modes_.begin(), start_modes_.end(), + [](auto& item) { return (item == hardware_interface::HW_IF_VELOCITY); }) && + std::any_of(start_modes_.begin(), start_modes_.end(), [this](auto& item) { + return (item == hardware_interface::HW_IF_VELOCITY || item == hardware_interface::HW_IF_POSITION || + item == PASSTHROUGH_GPIO); + })) { ret_val = hardware_interface::return_type::ERROR; } @@ -903,19 +1003,34 @@ hardware_interface::return_type URPositionHardwareInterface::perform_command_mod std::find(stop_modes_.begin(), stop_modes_.end(), StoppingInterface::STOP_VELOCITY) != stop_modes_.end()) { velocity_controller_running_ = false; urcl_velocity_commands_ = { { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 } }; + } else if (stop_modes_.size() != 0 && std::find(stop_modes_.begin(), stop_modes_.end(), + StoppingInterface::STOP_PASSTHROUGH) != stop_modes_.end()) { + passthrough_trajectory_controller_running_ = false; + passthrough_trajectory_abort_ = 1.0; + trajectory_joint_positions_.clear(); + trajectory_joint_accelerations_.clear(); + trajectory_joint_velocities_.clear(); } if (start_modes_.size() != 0 && std::find(start_modes_.begin(), start_modes_.end(), hardware_interface::HW_IF_POSITION) != start_modes_.end()) { velocity_controller_running_ = false; + passthrough_trajectory_controller_running_ = false; urcl_position_commands_ = urcl_position_commands_old_ = urcl_joint_positions_; position_controller_running_ = true; } else if (start_modes_.size() != 0 && std::find(start_modes_.begin(), start_modes_.end(), hardware_interface::HW_IF_VELOCITY) != start_modes_.end()) { position_controller_running_ = false; + passthrough_trajectory_controller_running_ = false; urcl_velocity_commands_ = { { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 } }; velocity_controller_running_ = true; + } else if (start_modes_.size() != 0 && + std::find(start_modes_.begin(), start_modes_.end(), PASSTHROUGH_GPIO) != start_modes_.end()) { + velocity_controller_running_ = false; + position_controller_running_ = false; + passthrough_trajectory_controller_running_ = true; + passthrough_trajectory_abort_ = 0.0; } start_modes_.clear(); @@ -923,6 +1038,87 @@ hardware_interface::return_type URPositionHardwareInterface::perform_command_mod return ret_val; } + +void URPositionHardwareInterface::check_passthrough_trajectory_controller() +{ + static double last_time = 0.0; + // 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_ == 2.0) { + passthrough_trajectory_abort_ = 0.0; + trajectory_joint_positions_.push_back(passthrough_trajectory_positions_); + + trajectory_times_.push_back(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_); + } + passthrough_trajectory_transfer_state_ = 1.0; + /* When all points have been read, write them to the physical robot controller.*/ + } 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()); + /* 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]); + } + } + 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; + } +} + +void URPositionHardwareInterface::trajectory_done_callback(urcl::control::TrajectoryResult result) +{ + if (result == urcl::control::TrajectoryResult::TRAJECTORY_RESULT_FAILURE) { + passthrough_trajectory_abort_ = 1.0; + } else { + passthrough_trajectory_abort_ = 0.0; + } + passthrough_trajectory_transfer_state_ = 5.0; + return; +} + +bool URPositionHardwareInterface::has_velocities(std::vector> velocities) +{ + return (velocities.size() > 0); +} + +bool URPositionHardwareInterface::has_accelerations(std::vector> accelerations) +{ + return (accelerations.size() > 0); +} + } // namespace ur_robot_driver #include "pluginlib/class_list_macros.hpp" diff --git a/ur_robot_driver/test/integration_test_controller_switch.py b/ur_robot_driver/test/integration_test_controller_switch.py new file mode 100644 index 000000000..20ea69190 --- /dev/null +++ b/ur_robot_driver/test/integration_test_controller_switch.py @@ -0,0 +1,287 @@ +#!/usr/bin/env python +# Copyright 2019, 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 os +import sys +import time +import unittest + +import pytest + +import launch_testing +import rclpy +from rclpy.node import Node + +from controller_manager_msgs.srv import SwitchController + +sys.path.append(os.path.dirname(__file__)) +from test_common import ( # noqa: E402 + ControllerManagerInterface, + DashboardInterface, + IoStatusInterface, + generate_driver_test_description, +) + + +@pytest.mark.launch_test +@launch_testing.parametrize( + "tf_prefix", + [(""), ("my_ur_")], +) +def generate_test_description(tf_prefix): + return generate_driver_test_description(tf_prefix=tf_prefix) + + +class RobotDriverTest(unittest.TestCase): + @classmethod + def setUpClass(cls): + # Initialize the ROS context + rclpy.init() + cls.node = Node("robot_driver_test") + time.sleep(1) + cls.init_robot(cls) + + @classmethod + def tearDownClass(cls): + # Shutdown the ROS context + cls.node.destroy_node() + rclpy.shutdown() + + def init_robot(self): + self._dashboard_interface = DashboardInterface(self.node) + self._controller_manager_interface = ControllerManagerInterface(self.node) + self._io_status_controller_interface = IoStatusInterface(self.node) + + def setUp(self): + self._dashboard_interface.start_robot() + time.sleep(1) + self.assertTrue(self._io_status_controller_interface.resend_robot_program().success) + + def test_activating_multiple_controllers_same_interface_fails(self): + # Deactivate all writing controllers + self.assertTrue( + self._controller_manager_interface.switch_controller( + strictness=SwitchController.Request.BEST_EFFORT, + deactivate_controllers=[ + "scaled_joint_trajectory_controller", + "joint_trajectory_controller", + "forward_position_controller", + "forward_velocity_controller", + "passthrough_trajectory_controller", + ], + ).ok + ) + + # Activating different motion controllers should not be possible + self.assertFalse( + self._controller_manager_interface.switch_controller( + strictness=SwitchController.Request.STRICT, + activate_controllers=[ + "scaled_joint_trajectory_controller", + "joint_trajectory_controller", + ], + ).ok + ) + self.assertFalse( + self._controller_manager_interface.switch_controller( + strictness=SwitchController.Request.STRICT, + activate_controllers=[ + "scaled_joint_trajectory_controller", + "forward_position_controller", + ], + ).ok + ) + self.assertFalse( + self._controller_manager_interface.switch_controller( + strictness=SwitchController.Request.STRICT, + activate_controllers=[ + "scaled_joint_trajectory_controller", + "forward_position_controller", + ], + ).ok + ) + + def test_activating_multiple_controllers_different_interface_fails(self): + # Deactivate all writing controllers + self.assertTrue( + self._controller_manager_interface.switch_controller( + strictness=SwitchController.Request.BEST_EFFORT, + deactivate_controllers=[ + "scaled_joint_trajectory_controller", + "joint_trajectory_controller", + "forward_position_controller", + "forward_velocity_controller", + "passthrough_trajectory_controller", + ], + ).ok + ) + self.assertFalse( + self._controller_manager_interface.switch_controller( + strictness=SwitchController.Request.STRICT, + activate_controllers=[ + "scaled_joint_trajectory_controller", + "forward_velocity_controller", + ], + ).ok + ) + self.assertFalse( + self._controller_manager_interface.switch_controller( + strictness=SwitchController.Request.STRICT, + activate_controllers=[ + "scaled_joint_trajectory_controller", + "passthrough_trajectory_controller", + ], + ).ok + ) + self.assertFalse( + self._controller_manager_interface.switch_controller( + strictness=SwitchController.Request.STRICT, + activate_controllers=[ + "forward_velocity_controller", + "passthrough_trajectory_controller", + ], + ).ok + ) + self.assertFalse( + self._controller_manager_interface.switch_controller( + strictness=SwitchController.Request.STRICT, + activate_controllers=[ + "forward_position_controller", + "forward_velocity_controller", + ], + ).ok + ) + + def test_activating_controller_with_running_position_controller_fails(self): + # Having a position-based controller active, no other controller should be able to + # activate. + self.assertTrue( + self._controller_manager_interface.switch_controller( + strictness=SwitchController.Request.BEST_EFFORT, + activate_controllers=[ + "scaled_joint_trajectory_controller", + ], + deactivate_controllers=[ + "joint_trajectory_controller", + "forward_position_controller", + "forward_velocity_controller", + "passthrough_trajectory_controller", + ], + ).ok + ) + self.assertFalse( + self._controller_manager_interface.switch_controller( + strictness=SwitchController.Request.STRICT, + activate_controllers=[ + "forward_position_controller", + ], + ).ok + ) + self.assertFalse( + self._controller_manager_interface.switch_controller( + strictness=SwitchController.Request.STRICT, + activate_controllers=[ + "forward_velocity_controller", + ], + ).ok + ) + self.assertFalse( + self._controller_manager_interface.switch_controller( + strictness=SwitchController.Request.STRICT, + activate_controllers=[ + "passthrough_trajectory_controller", + ], + ).ok + ) + # Stop controller again + self.assertTrue( + self._controller_manager_interface.switch_controller( + strictness=SwitchController.Request.STRICT, + deactivate_controllers=[ + "scaled_joint_trajectory_controller", + ], + ).ok + ) + + def test_activating_controller_with_running_passthrough_trajectory_controller_fails(self): + # Having a position-based controller active, no other controller should be able to + # activate. + self.assertTrue( + self._controller_manager_interface.switch_controller( + strictness=SwitchController.Request.BEST_EFFORT, + activate_controllers=["passthrough_trajectory_controller"], + deactivate_controllers=[ + "scaled_joint_trajectory_controller", + "joint_trajectory_controller", + "forward_position_controller", + "forward_velocity_controller", + ], + ).ok + ) + self.assertFalse( + self._controller_manager_interface.switch_controller( + strictness=SwitchController.Request.STRICT, + activate_controllers=[ + "scaled_joint_trajectory_controller", + ], + ).ok + ) + self.assertFalse( + self._controller_manager_interface.switch_controller( + strictness=SwitchController.Request.STRICT, + activate_controllers=[ + "forward_position_controller", + ], + ).ok + ) + self.assertFalse( + self._controller_manager_interface.switch_controller( + strictness=SwitchController.Request.STRICT, + activate_controllers=[ + "forward_velocity_controller", + ], + ).ok + ) + self.assertFalse( + self._controller_manager_interface.switch_controller( + strictness=SwitchController.Request.STRICT, + activate_controllers=[ + "joint_trajectory_controller", + ], + ).ok + ) + # Stop the controller again + self.assertTrue( + self._controller_manager_interface.switch_controller( + strictness=SwitchController.Request.STRICT, + deactivate_controllers=[ + "passthrough_trajectory_controller", + ], + ).ok + ) diff --git a/ur_robot_driver/test/robot_driver.py b/ur_robot_driver/test/robot_driver.py index 88fcffce3..6350ea78d 100644 --- a/ur_robot_driver/test/robot_driver.py +++ b/ur_robot_driver/test/robot_driver.py @@ -1,4 +1,4 @@ -#!/usr/bin/env python +#!/usr/bin/env python3 # Copyright 2019, FZI Forschungszentrum Informatik # # Redistribution and use in source and binary forms, with or without @@ -37,6 +37,7 @@ import rclpy from builtin_interfaces.msg import Duration from control_msgs.action import FollowJointTrajectory +from control_msgs.msg import JointTolerance from controller_manager_msgs.srv import SwitchController from rclpy.node import Node from sensor_msgs.msg import JointState @@ -56,9 +57,9 @@ TIMEOUT_EXECUTE_TRAJECTORY = 30 ROBOT_JOINTS = [ - "elbow_joint", - "shoulder_lift_joint", "shoulder_pan_joint", + "shoulder_lift_joint", + "elbow_joint", "wrist_1_joint", "wrist_2_joint", "wrist_3_joint", @@ -100,6 +101,11 @@ def init_robot(self): "/scaled_joint_trajectory_controller/follow_joint_trajectory", FollowJointTrajectory, ) + self._passthrough_forward_joint_trajectory = ActionInterface( + self.node, + "/passthrough_trajectory_controller/follow_joint_trajectory", + FollowJointTrajectory, + ) def setUp(self): self._dashboard_interface.start_robot() @@ -123,6 +129,22 @@ def test_start_scaled_jtc_controller(self): ).ok ) + def test_start_passthrough_controller(self): + self.assertTrue( + self._controller_manager_interface.switch_controller( + strictness=SwitchController.Request.BEST_EFFORT, + activate_controllers=["passthrough_trajectory_controller"], + deactivate_controllers=["scaled_joint_trajectory_controller"], + ).ok + ) + self.assertTrue( + self._controller_manager_interface.switch_controller( + strictness=SwitchController.Request.BEST_EFFORT, + deactivate_controllers=["passthrough_trajectory_controller"], + activate_controllers=["scaled_joint_trajectory_controller"], + ).ok + ) + def test_set_io(self): """Test to set an IO and check whether it has been set.""" # Create io callback to verify result @@ -265,7 +287,10 @@ def test_trajectory_scaled_aborts_on_violation(self, tf_prefix): Duration(sec=6, nanosec=50000000), [-1.0 for j in ROBOT_JOINTS], ), # physically unfeasible - (Duration(sec=8, nanosec=0), [-1.5 for j in ROBOT_JOINTS]), # physically unfeasible + ( + Duration(sec=8, nanosec=0), + [-1.5 for j in ROBOT_JOINTS], + ), # physically unfeasible ] trajectory = JointTrajectory( @@ -318,17 +343,105 @@ def js_cb(msg): ) ) - # TODO: uncomment when JTC starts taking into account goal_time_tolerance from goal message - # see https://github.com/ros-controls/ros2_controllers/issues/249 - # Now do the same again, but with a goal time constraint - # self.node.get_logger().info("Sending scaled goal with time restrictions") - # - # goal.goal_time_tolerance = Duration(nanosec=10000000) - # goal_response = self.call_action("/scaled_joint_trajectory_controller/follow_joint_trajectory", goal) - # - # self.assertEqual(goal_response.accepted, True) - # - # if goal_response.accepted: - # result = self.get_result("/scaled_joint_trajectory_controller/follow_joint_trajectory", goal_response, TIMEOUT_EXECUTE_TRAJECTORY) - # self.assertEqual(result.error_code, FollowJointTrajectory.Result.GOAL_TOLERANCE_VIOLATED) - # self.node.get_logger().info("Received result GOAL_TOLERANCE_VIOLATED") + # TODO: uncomment when JTC starts taking into account goal_time_tolerance from goal message + # see https://github.com/ros-controls/ros2_controllers/issues/249 + # Now do the same again, but with a goal time constraint + # self.node.get_logger().info("Sending scaled goal with time restrictions") + # + # goal.goal_time_tolerance = Duration(nanosec=10000000) + # goal_response = self.call_action("/scaled_joint_trajectory_controller/follow_joint_trajectory", goal) + # + # self.assertEqual(goal_response.accepted, True) + # + # if goal_response.accepted: + # result = self.get_result("/scaled_joint_trajectory_controller/follow_joint_trajectory", goal_response, TIMEOUT_EXECUTE_TRAJECTORY) + # self.assertEqual(result.error_code, FollowJointTrajectory.Result.GOAL_TOLERANCE_VIOLATED) + # self.node.get_logger().info("Received result GOAL_TOLERANCE_VIOLATED") + + def test_passthrough_trajectory(self, tf_prefix): + self.assertTrue( + self._controller_manager_interface.switch_controller( + strictness=SwitchController.Request.BEST_EFFORT, + activate_controllers=["passthrough_trajectory_controller"], + deactivate_controllers=["scaled_joint_trajectory_controller"], + ).ok + ) + waypts = [ + [-1.58, -1.692, -1.4311, -0.0174, 1.5882, 0.0349], + [-3, -1.692, -1.4311, -0.0174, 1.5882, 0.0349], + [-1.58, -1.692, -1.4311, -0.0174, 1.5882, 0.0349], + ] + time_vec = [ + Duration(sec=4, nanosec=0), + Duration(sec=8, nanosec=0), + Duration(sec=12, nanosec=0), + ] + goal_tolerance = [ + JointTolerance(position=0.01, name=tf_prefix + ROBOT_JOINTS[i]) + for i in range(len(ROBOT_JOINTS)) + ] + goal_time_tolerance = Duration(sec=1, nanosec=0) + test_trajectory = zip(time_vec, waypts) + trajectory = JointTrajectory( + points=[ + JointTrajectoryPoint(positions=pos, time_from_start=times) + for (times, pos) in test_trajectory + ], + joint_names=[tf_prefix + ROBOT_JOINTS[i] for i in range(len(ROBOT_JOINTS))], + ) + goal_handle = self._passthrough_forward_joint_trajectory.send_goal( + trajectory=trajectory, + goal_time_tolerance=goal_time_tolerance, + goal_tolerance=goal_tolerance, + ) + self.assertTrue(goal_handle.accepted) + if goal_handle.accepted: + result = self._passthrough_forward_joint_trajectory.get_result( + goal_handle, TIMEOUT_EXECUTE_TRAJECTORY + ) + self.assertEqual(result.error_code, FollowJointTrajectory.Result.SUCCESSFUL) + # Test impossible goal tolerance, should fail. + goal_tolerance = [ + JointTolerance(position=0.000000001, name=tf_prefix + ROBOT_JOINTS[i]) + for i in range(len(ROBOT_JOINTS)) + ] + goal_handle = self._passthrough_forward_joint_trajectory.send_goal( + trajectory=trajectory, + goal_time_tolerance=goal_time_tolerance, + goal_tolerance=goal_tolerance, + ) + self.assertTrue(goal_handle.accepted) + if goal_handle.accepted: + result = self._passthrough_forward_joint_trajectory.get_result( + goal_handle, TIMEOUT_EXECUTE_TRAJECTORY + ) + self.assertEqual( + result.error_code, FollowJointTrajectory.Result.GOAL_TOLERANCE_VIOLATED + ) + + # Test impossible goal time + goal_tolerance = [ + JointTolerance(position=0.01, name=tf_prefix + ROBOT_JOINTS[i]) for i in range(6) + ] + goal_time_tolerance.sec = 0 + goal_time_tolerance.nanosec = 10 + goal_handle = self._passthrough_forward_joint_trajectory.send_goal( + trajectory=trajectory, + goal_time_tolerance=goal_time_tolerance, + goal_tolerance=goal_tolerance, + ) + self.assertTrue(goal_handle.accepted) + if goal_handle.accepted: + result = self._passthrough_forward_joint_trajectory.get_result( + goal_handle, TIMEOUT_EXECUTE_TRAJECTORY + ) + self.assertEqual( + result.error_code, FollowJointTrajectory.Result.GOAL_TOLERANCE_VIOLATED + ) + self.assertTrue( + self._controller_manager_interface.switch_controller( + strictness=SwitchController.Request.BEST_EFFORT, + deactivate_controllers=["passthrough_trajectory_controller"], + activate_controllers=["scaled_joint_trajectory_controller"], + ).ok + ) diff --git a/ur_robot_driver/test/test_common.py b/ur_robot_driver/test/test_common.py index 8db7b6835..a8a1a8ffb 100644 --- a/ur_robot_driver/test/test_common.py +++ b/ur_robot_driver/test/test_common.py @@ -29,7 +29,12 @@ import time import rclpy -from controller_manager_msgs.srv import ListControllers, SwitchController +from controller_manager_msgs.srv import ( + ListControllers, + SwitchController, + LoadController, + UnloadController, +) from launch import LaunchDescription from launch.actions import ( DeclareLaunchArgument, @@ -58,6 +63,15 @@ TIMEOUT_WAIT_SERVICE_INITIAL = 120 # If we download the docker image simultaneously to the tests, it can take quite some time until the dashboard server is reachable and usable. TIMEOUT_WAIT_ACTION = 10 +ROBOT_JOINTS = [ + "elbow_joint", + "shoulder_lift_joint", + "shoulder_pan_joint", + "wrist_1_joint", + "wrist_2_joint", + "wrist_3_joint", +] + def _wait_for_service(node, srv_name, srv_type, timeout): client = node.create_client(srv_type, srv_name) @@ -223,7 +237,11 @@ def _check_call(self, result): class ControllerManagerInterface( _ServiceInterface, namespace="/controller_manager", - initial_services={"switch_controller": SwitchController}, + initial_services={ + "switch_controller": SwitchController, + "load_controller": LoadController, + "unload_controller": UnloadController, + }, services={"list_controllers": ListControllers}, ): def wait_for_controller(self, controller_name, target_state="active"): From 56c08dd813f373d9b7bf5442a8a229742a5c8acc Mon Sep 17 00:00:00 2001 From: "mergify[bot]" <37929162+mergify[bot]@users.noreply.github.com> Date: Tue, 3 Dec 2024 07:50:33 +0100 Subject: [PATCH 019/133] Initialize segments in constructor of DHRobot in calibration.hpp (backport of #1197) Also add missing #include (cherry picked from commit d8517e745419879dd59c20acb15278998226212d) Co-authored-by: Benjamin <871576+b-bremer@users.noreply.github.com> --- ur_calibration/include/ur_calibration/calibration.hpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/ur_calibration/include/ur_calibration/calibration.hpp b/ur_calibration/include/ur_calibration/calibration.hpp index f6fdec8f0..6cfcc3f6b 100644 --- a/ur_calibration/include/ur_calibration/calibration.hpp +++ b/ur_calibration/include/ur_calibration/calibration.hpp @@ -44,6 +44,7 @@ #ifndef UR_CALIBRATION__CALIBRATION_HPP_ #define UR_CALIBRATION__CALIBRATION_HPP_ +#include #include #include #include @@ -111,7 +112,7 @@ struct DHRobot /*! * \brief Create a new robot representation giving a set of \ref DHSegment objects */ - explicit DHRobot(const std::vector& segments) + explicit DHRobot(const std::vector& segments) : segments_(segments) { delta_theta_correction2_ = 0; delta_theta_correction3_ = 0; From 4188f035c8d383f8d2fbb6a30ccddc694e20b0f5 Mon Sep 17 00:00:00 2001 From: Felix Exner Date: Wed, 4 Dec 2024 09:12:22 +0100 Subject: [PATCH 020/133] update branch for realtime_tools (#1200) realtime_tools branched out for humble, so we should update that in our repos file. --- Universal_Robots_ROS2_Driver.humble.repos | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Universal_Robots_ROS2_Driver.humble.repos b/Universal_Robots_ROS2_Driver.humble.repos index ddbdbbfac..b63138002 100644 --- a/Universal_Robots_ROS2_Driver.humble.repos +++ b/Universal_Robots_ROS2_Driver.humble.repos @@ -30,4 +30,4 @@ repositories: realtime_tools: type: git url: https://github.com/ros-controls/realtime_tools.git - version: master + version: humble From c1f75d42da292b60bae45d97152e47714d50087b Mon Sep 17 00:00:00 2001 From: "mergify[bot]" <37929162+mergify[bot]@users.noreply.github.com> Date: Mon, 16 Dec 2024 14:28:45 +0100 Subject: [PATCH 021/133] Update package maintainers (backport of #1203) This commit updates the package maintainers as the project's governance has changed a bit. --- ur/package.xml | 7 +++++-- ur_bringup/package.xml | 7 ++++--- ur_calibration/package.xml | 5 +++-- ur_controllers/package.xml | 8 +++++--- ur_dashboard_msgs/package.xml | 5 +++-- ur_moveit_config/package.xml | 9 ++++++--- ur_robot_driver/package.xml | 8 +++++--- 7 files changed, 31 insertions(+), 18 deletions(-) diff --git a/ur/package.xml b/ur/package.xml index 3ab6f7d6b..b37aef93b 100644 --- a/ur/package.xml +++ b/ur/package.xml @@ -4,8 +4,11 @@ ur 2.2.16 Metapackage for universal robots - Felix Exner - Robert Wilbrandt + + Felix Exner + Rune Søe-Knudsen + Universal Robots A/S + BSD-3-Clause ament_cmake diff --git a/ur_bringup/package.xml b/ur_bringup/package.xml index 11ff3c710..4f23de068 100644 --- a/ur_bringup/package.xml +++ b/ur_bringup/package.xml @@ -5,9 +5,9 @@ 2.2.16 Launch file and run-time configurations, e.g. controllers. - Denis Stogl - Felix Exner - Robert Wilbrandt + Felix Exner + Rune Søe-Knudsen + Universal Robots A/S BSD-3-Clause @@ -15,6 +15,7 @@ https://github.com/UniversalRobots/Universal_Robots_ROS2_Driver Lovro Ivanov + Denis Stogl Denis Stogl ament_cmake diff --git a/ur_calibration/package.xml b/ur_calibration/package.xml index fb6b89630..425b503d3 100644 --- a/ur_calibration/package.xml +++ b/ur_calibration/package.xml @@ -4,8 +4,9 @@ 2.2.16 Package for extracting the factory calibration from a UR robot and change it such that it can be used by ur_description to gain a correct URDF - Felix Exner - Robert Wilbrandt + Felix Exner + Rune Søe-Knudsen + Universal Robots A/S BSD-3-Clause diff --git a/ur_controllers/package.xml b/ur_controllers/package.xml index e8f7c8da4..8e5d999b9 100644 --- a/ur_controllers/package.xml +++ b/ur_controllers/package.xml @@ -5,18 +5,20 @@ 2.2.16 Provides controllers that use the speed scaling interface of Universal Robots. - Denis Stogl - Felix Exner - Robert Wilbrandt + Felix Exner + Rune Søe-Knudsen + Universal Robots A/S BSD-3-Clause https://github.com/UniversalRobots/Universal_Robots_ROS2_Driver/issues https://github.com/UniversalRobots/Universal_Robots_ROS2_Driver + Denis Stogl Marvin Große Besselmann Lovro Ivanov Andy Zelenak + Vincenzo Di Pentima ament_cmake diff --git a/ur_dashboard_msgs/package.xml b/ur_dashboard_msgs/package.xml index 6684d7f9c..286ed8486 100644 --- a/ur_dashboard_msgs/package.xml +++ b/ur_dashboard_msgs/package.xml @@ -5,8 +5,9 @@ 2.2.16 Messages around the UR Dashboard server. - Felix Exner - Robert Wilbrandt + Felix Exner + Rune Søe-Knudsen + Universal Robots A/S BSD-3-Clause diff --git a/ur_moveit_config/package.xml b/ur_moveit_config/package.xml index 4eea852ea..15f9ef071 100644 --- a/ur_moveit_config/package.xml +++ b/ur_moveit_config/package.xml @@ -6,14 +6,17 @@ An example package with MoveIt2 configurations for UR robots. - Denis Stogl - Felix Exner - Robert Wilbrandt + + Felix Exner + Rune Søe-Knudsen + Universal Robots A/S Apache2.0 Lovro Ivanov Andy Zelenak + Vincenzo Di Pentima + Denis Stogl ament_cmake ament_cmake_python diff --git a/ur_robot_driver/package.xml b/ur_robot_driver/package.xml index bc27de8de..8b0577b00 100644 --- a/ur_robot_driver/package.xml +++ b/ur_robot_driver/package.xml @@ -5,18 +5,20 @@ 2.2.16 The new driver for Universal Robots UR3, UR5 and UR10 robots with CB3 controllers and the e-series. - Denis Stogl - Felix Exner - Robert Wilbrandt + Felix Exner + Rune Søe-Knudsen + Universal Robots A/S BSD-3-Clause https://github.com/UniversalRobots/Universal_Robots_ROS2_Driver/issues https://github.com/UniversalRobots/Universal_Robots_ROS2_Driver + Denis Stogl Marvin Große Besselmann Lovro Ivanov Andy Zelenak + Vincenzo Di Pentima Thomas Timm Andersen Simon Rasmussen Felix Exner From 46f8aea4b3c404a8785db3988d46285c30e95cbf Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Tue, 17 Dec 2024 06:20:56 +0100 Subject: [PATCH 022/133] Add control_toolbox to semi-binary repos file (#1209) --- Universal_Robots_ROS2_Driver.humble.repos | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/Universal_Robots_ROS2_Driver.humble.repos b/Universal_Robots_ROS2_Driver.humble.repos index b63138002..d492a0811 100644 --- a/Universal_Robots_ROS2_Driver.humble.repos +++ b/Universal_Robots_ROS2_Driver.humble.repos @@ -27,6 +27,10 @@ repositories: type: git url: https://github.com/ros-controls/control_msgs.git version: humble + control_toolbox: + type: git + url: https://github.com/ros-controls/control_toolbox.git + version: ros2-master realtime_tools: type: git url: https://github.com/ros-controls/realtime_tools.git From fda52d521e5bd9a60cc4a2eeba74c96f3f014835 Mon Sep 17 00:00:00 2001 From: "mergify[bot]" <37929162+mergify[bot]@users.noreply.github.com> Date: Wed, 18 Dec 2024 10:51:16 +0100 Subject: [PATCH 023/133] Add force mode controller (#1049) (#1193) This enables using the robot's force_mode in a standalone controller. Force mode can be enabled and parametrized using a service call provided by the controller. --------- Co-authored-by: URJala <159417921+URJala@users.noreply.github.com> Co-authored-by: urmarp --- ur_controllers/CMakeLists.txt | 31 ++ ur_controllers/controller_plugins.xml | 5 + ur_controllers/doc/index.rst | 76 +++ .../ur_controllers/force_mode_controller.hpp | 147 +++++ ur_controllers/package.xml | 5 + ur_controllers/src/force_mode_controller.cpp | 380 +++++++++++++ .../src/force_mode_controller_parameters.yaml | 12 + .../test/force_mode_controller_params.yaml | 7 + .../test/test_load_force_mode_controller.cpp | 61 ++ ur_robot_driver/CMakeLists.txt | 9 + ur_robot_driver/config/ur_controllers.yaml | 10 + ur_robot_driver/examples/examples.py | 180 ++++++ ur_robot_driver/examples/force_mode.py | 142 +++++ .../ur_robot_driver/hardware_interface.hpp | 23 +- ur_robot_driver/launch/ur_control.launch.py | 1 + ur_robot_driver/src/hardware_interface.cpp | 270 +++++++-- .../integration_test_controller_switch.py | 112 +++- .../test/integration_test_force_mode.py | 524 ++++++++++++++++++ ur_robot_driver/test/robot_driver.py | 10 +- ur_robot_driver/test/test_common.py | 11 +- 20 files changed, 1938 insertions(+), 78 deletions(-) create mode 100644 ur_controllers/include/ur_controllers/force_mode_controller.hpp create mode 100644 ur_controllers/src/force_mode_controller.cpp create mode 100644 ur_controllers/src/force_mode_controller_parameters.yaml create mode 100644 ur_controllers/test/force_mode_controller_params.yaml create mode 100644 ur_controllers/test/test_load_force_mode_controller.cpp create mode 100644 ur_robot_driver/examples/examples.py create mode 100755 ur_robot_driver/examples/force_mode.py create mode 100644 ur_robot_driver/test/integration_test_force_mode.py diff --git a/ur_controllers/CMakeLists.txt b/ur_controllers/CMakeLists.txt index e94be4f0f..f310a32e8 100644 --- a/ur_controllers/CMakeLists.txt +++ b/ur_controllers/CMakeLists.txt @@ -8,6 +8,7 @@ endif() find_package(ament_cmake REQUIRED) find_package(angles REQUIRED) find_package(controller_interface REQUIRED) +find_package(geometry_msgs REQUIRED) find_package(joint_trajectory_controller REQUIRED) find_package(lifecycle_msgs REQUIRED) find_package(pluginlib REQUIRED) @@ -16,6 +17,8 @@ find_package(rcutils REQUIRED) find_package(realtime_tools REQUIRED) find_package(std_msgs REQUIRED) find_package(std_srvs REQUIRED) +find_package(tf2_geometry_msgs REQUIRED) +find_package(tf2_ros REQUIRED) find_package(ur_dashboard_msgs REQUIRED) find_package(ur_msgs REQUIRED) find_package(generate_parameter_library REQUIRED) @@ -27,6 +30,7 @@ find_package(action_msgs REQUIRED) set(THIS_PACKAGE_INCLUDE_DEPENDS angles controller_interface + geometry_msgs joint_trajectory_controller lifecycle_msgs pluginlib @@ -35,6 +39,8 @@ set(THIS_PACKAGE_INCLUDE_DEPENDS realtime_tools std_msgs std_srvs + tf2_geometry_msgs + tf2_ros ur_dashboard_msgs ur_msgs generate_parameter_library @@ -45,6 +51,10 @@ set(THIS_PACKAGE_INCLUDE_DEPENDS include_directories(include) +generate_parameter_library( + force_mode_controller_parameters + src/force_mode_controller_parameters.yaml +) generate_parameter_library( gpio_controller_parameters @@ -72,6 +82,7 @@ generate_parameter_library( ) add_library(${PROJECT_NAME} SHARED + src/force_mode_controller.cpp src/scaled_joint_trajectory_controller.cpp src/speed_scaling_state_broadcaster.cpp src/gpio_controller.cpp @@ -82,6 +93,7 @@ target_include_directories(${PROJECT_NAME} PRIVATE include ) target_link_libraries(${PROJECT_NAME} + force_mode_controller_parameters gpio_controller_parameters speed_scaling_state_broadcaster_parameters scaled_joint_trajectory_controller_parameters @@ -124,4 +136,23 @@ ament_export_libraries( ${PROJECT_NAME} ) +if(BUILD_TESTING) + find_package(ament_cmake_gmock REQUIRED) + find_package(controller_manager REQUIRED) + find_package(hardware_interface REQUIRED) + find_package(ros2_control_test_assets REQUIRED) + + add_definitions(-DTEST_FILES_DIRECTORY="${CMAKE_CURRENT_SOURCE_DIR}/test") + ament_add_gmock(test_load_force_mode_controller + test/test_load_force_mode_controller.cpp + ) + target_link_libraries(test_load_force_mode_controller + ${PROJECT_NAME} + ) + ament_target_dependencies(test_load_force_mode_controller + controller_manager + ros2_control_test_assets + ) +endif() + ament_package() diff --git a/ur_controllers/controller_plugins.xml b/ur_controllers/controller_plugins.xml index c784b35d0..f3365a26d 100644 --- a/ur_controllers/controller_plugins.xml +++ b/ur_controllers/controller_plugins.xml @@ -14,6 +14,11 @@ This controller publishes the Tool IO. + + + Controller to use UR's force_mode. + + This controller forwards a joint-based trajectory to the robot controller for interpolation. diff --git a/ur_controllers/doc/index.rst b/ur_controllers/doc/index.rst index a488d07e5..38211d083 100644 --- a/ur_controllers/doc/index.rst +++ b/ur_controllers/doc/index.rst @@ -264,3 +264,79 @@ Implementation details / dataflow * 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. + +.. _force_mode_controller: + +ur_controllers/ForceModeController +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +This controller activates the robot's *Force Mode*. This allows direct force control running on the +robot control box. This controller basically interfaces the URScript function ``force_mode(...)``. + +Force mode can be combined with (and only with) the :ref:`passthrough trajectory controller +` in order to execute motions under a given force constraints. + +.. note:: + This is not an admittance controller, as given force constraints in a certain Cartesian + dimension will overwrite the motion commands in that dimension. E.g. when specifying a certain + force in the base frame's ``z`` direction, any motion resulting from the move command in the + base frame's ``z`` axis will not be executed. + +Parameters +"""""""""" + ++----------------------------------+--------+---------------+---------------------------------------------------------------------+ +| Parameter name | Type | Default value | Description | +| | | | | ++----------------------------------+--------+---------------+---------------------------------------------------------------------+ +| ``tf_prefix`` | string | | Urdf prefix of the corresponding arm | ++----------------------------------+--------+---------------+---------------------------------------------------------------------+ +| ``check_io_successful_retries`` | int | 10 | Amount of retries for checking if setting force_mode was successful | ++----------------------------------+--------+---------------+---------------------------------------------------------------------+ + +Service interface / usage +""""""""""""""""""""""""" + +The controller provides two services: One for activating force_mode and one for leaving it. To use +those services, the controller has to be in ``active`` state. + +* ``~/stop_force_mode [std_srvs/srv/Trigger]``: Stop force mode +* ``~/start_force_mode [ur_msgs/srv/SetForceMode]``: Start force mode + +In ``ur_msgs/srv/SetForceMode`` the fields have the following meanings: + +task_frame + All information (selection vector, wrench, limits, etc) will be considered to be relative + to that pose. The pose's frame_id can be anything that is transformable to the robot's + ``base`` frame. +selection_vector_ + 1 means that the robot will be compliant in the corresponding axis of the task frame. +wrench + The forces/torques the robot will apply to its environment. The robot adjusts its position + along/about compliant axis in order to achieve the specified force/torque. Values have no effect for non- + compliant axes. + Actual wrench applied may be lower than requested due to joint safety limits. +type + An integer [1;3] specifying how the robot interprets the force frame + + 1 + The force frame is transformed in a way such that its y-axis is aligned with a vector pointing + from the robot tcp towards the origin of the force frame. + 2 + The force frame is not transformed. + 3 + The force frame is transformed in a way such that its x-axis is the projection of the robot tcp + velocity vector onto the x-y plane of the force frame. +speed_limits + Maximum allowed tcp speed (relative to the task frame). This is **only relevant for axes marked as + compliant** in the selection_vector. +deviation_limits + For **non-compliant axes**, these values are the maximum allowed deviation along/about an axis + between the actual tcp position and the one set by the program. +damping_factor + Force mode damping factor. Sets the damping parameter in force mode. In range [0;1], default value is 0.025 + A value of 1 is full damping, so the robot will decelerate quickly if no force is present. A value of 0 + is no damping, here the robot will maintain the speed. +gain_scaling + Force mode gain scaling factor. Scales the gain in force mode. scaling parameter is in range [0;2], default is 0.5. + A value larger than 1 can make force mode unstable, e.g. in case of collisions or pushing against hard surfaces. diff --git a/ur_controllers/include/ur_controllers/force_mode_controller.hpp b/ur_controllers/include/ur_controllers/force_mode_controller.hpp new file mode 100644 index 000000000..9ae45a04b --- /dev/null +++ b/ur_controllers/include/ur_controllers/force_mode_controller.hpp @@ -0,0 +1,147 @@ +// Copyright 2023, FZI Forschungszentrum Informatik, Created on behalf of 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 Felix Exner exner@fzi.de + * \date 2023-06-29 + */ +//---------------------------------------------------------------------- + +#pragma once +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include + +#include "force_mode_controller_parameters.hpp" + +namespace ur_controllers +{ +enum CommandInterfaces +{ + FORCE_MODE_TASK_FRAME_X = 0u, + FORCE_MODE_TASK_FRAME_Y = 1, + FORCE_MODE_TASK_FRAME_Z = 2, + FORCE_MODE_TASK_FRAME_RX = 3, + FORCE_MODE_TASK_FRAME_RY = 4, + FORCE_MODE_TASK_FRAME_RZ = 5, + FORCE_MODE_SELECTION_VECTOR_X = 6, + FORCE_MODE_SELECTION_VECTOR_Y = 7, + FORCE_MODE_SELECTION_VECTOR_Z = 8, + FORCE_MODE_SELECTION_VECTOR_RX = 9, + FORCE_MODE_SELECTION_VECTOR_RY = 10, + FORCE_MODE_SELECTION_VECTOR_RZ = 11, + FORCE_MODE_WRENCH_X = 12, + FORCE_MODE_WRENCH_Y = 13, + FORCE_MODE_WRENCH_Z = 14, + FORCE_MODE_WRENCH_RX = 15, + FORCE_MODE_WRENCH_RY = 16, + FORCE_MODE_WRENCH_RZ = 17, + FORCE_MODE_TYPE = 18, + FORCE_MODE_LIMITS_X = 19, + FORCE_MODE_LIMITS_Y = 20, + FORCE_MODE_LIMITS_Z = 21, + FORCE_MODE_LIMITS_RX = 22, + FORCE_MODE_LIMITS_RY = 23, + FORCE_MODE_LIMITS_RZ = 24, + FORCE_MODE_ASYNC_SUCCESS = 25, + FORCE_MODE_DISABLE_CMD = 26, + FORCE_MODE_DAMPING = 27, + FORCE_MODE_GAIN_SCALING = 28, +}; +enum StateInterfaces +{ + INITIALIZED_FLAG = 0u, +}; + +struct ForceModeParameters +{ + std::array task_frame; + std::array selection_vec; + std::array limits; + geometry_msgs::msg::Wrench wrench; + double type; + double damping_factor; + double gain_scaling; +}; + +class ForceModeController : public controller_interface::ControllerInterface +{ +public: + controller_interface::InterfaceConfiguration command_interface_configuration() const override; + + controller_interface::InterfaceConfiguration state_interface_configuration() const override; + + controller_interface::return_type update(const rclcpp::Time& time, const rclcpp::Duration& period) override; + + CallbackReturn on_configure(const rclcpp_lifecycle::State& previous_state) override; + + CallbackReturn on_activate(const rclcpp_lifecycle::State& previous_state) override; + + CallbackReturn on_deactivate(const rclcpp_lifecycle::State& previous_state) override; + + CallbackReturn on_init() override; + + CallbackReturn on_cleanup(const rclcpp_lifecycle::State& previous_state) override; + +private: + bool setForceMode(const ur_msgs::srv::SetForceMode::Request::SharedPtr req, + ur_msgs::srv::SetForceMode::Response::SharedPtr resp); + bool disableForceMode(const std_srvs::srv::Trigger::Request::SharedPtr req, + std_srvs::srv::Trigger::Response::SharedPtr resp); + rclcpp::Service::SharedPtr set_force_mode_srv_; + rclcpp::Service::SharedPtr disable_force_mode_srv_; + + std::unique_ptr tf_buffer_; + std::unique_ptr tf_listener_; + + std::shared_ptr param_listener_; + force_mode_controller::Params params_; + + realtime_tools::RealtimeBuffer force_mode_params_buffer_; + std::atomic force_mode_active_; + std::atomic change_requested_; + std::atomic async_state_; + + static constexpr double ASYNC_WAITING = 2.0; + /** + * @brief wait until a command interface isn't in state ASYNC_WAITING anymore or until the parameter maximum_retries + * have been reached + */ + bool waitForAsyncCommand(std::function get_value); +}; +} // namespace ur_controllers diff --git a/ur_controllers/package.xml b/ur_controllers/package.xml index 8e5d999b9..b12cd0845 100644 --- a/ur_controllers/package.xml +++ b/ur_controllers/package.xml @@ -24,6 +24,7 @@ angles controller_interface + geometry_msgs joint_trajectory_controller lifecycle_msgs pluginlib @@ -32,12 +33,16 @@ realtime_tools std_msgs std_srvs + tf2_geometry_msgs + tf2_ros ur_dashboard_msgs ur_msgs control_msgs trajectory_msgs action_msgs + hardware_interface_testing + ament_cmake diff --git a/ur_controllers/src/force_mode_controller.cpp b/ur_controllers/src/force_mode_controller.cpp new file mode 100644 index 000000000..3ecdd6cce --- /dev/null +++ b/ur_controllers/src/force_mode_controller.cpp @@ -0,0 +1,380 @@ +// Copyright 2023, FZI Forschungszentrum Informatik, Created on behalf of 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 Felix Exner exner@fzi.de + * \date 2023-06-29 + */ +//---------------------------------------------------------------------- + +#include +#include +#include +#include + +#include +namespace ur_controllers +{ +controller_interface::CallbackReturn ForceModeController::on_init() +{ + try { + // Create the parameter listener and get the parameters + param_listener_ = std::make_shared(get_node()); + params_ = param_listener_->get_params(); + } catch (const std::exception& e) { + fprintf(stderr, "Exception thrown during init stage with message: %s \n", e.what()); + return CallbackReturn::ERROR; + } + + return controller_interface::CallbackReturn::SUCCESS; +} +controller_interface::InterfaceConfiguration ForceModeController::command_interface_configuration() const +{ + controller_interface::InterfaceConfiguration config; + config.type = controller_interface::interface_configuration_type::INDIVIDUAL; + + const std::string tf_prefix = params_.tf_prefix; + RCLCPP_DEBUG(get_node()->get_logger(), "Configure UR force_mode controller with tf_prefix: %s", tf_prefix.c_str()); + + // Get all the command interfaces needed for force mode from the hardware interface + config.names.emplace_back(tf_prefix + "force_mode/task_frame_x"); + config.names.emplace_back(tf_prefix + "force_mode/task_frame_y"); + config.names.emplace_back(tf_prefix + "force_mode/task_frame_z"); + config.names.emplace_back(tf_prefix + "force_mode/task_frame_rx"); + config.names.emplace_back(tf_prefix + "force_mode/task_frame_ry"); + config.names.emplace_back(tf_prefix + "force_mode/task_frame_rz"); + config.names.emplace_back(tf_prefix + "force_mode/selection_vector_x"); + config.names.emplace_back(tf_prefix + "force_mode/selection_vector_y"); + config.names.emplace_back(tf_prefix + "force_mode/selection_vector_z"); + config.names.emplace_back(tf_prefix + "force_mode/selection_vector_rx"); + config.names.emplace_back(tf_prefix + "force_mode/selection_vector_ry"); + config.names.emplace_back(tf_prefix + "force_mode/selection_vector_rz"); + config.names.emplace_back(tf_prefix + "force_mode/wrench_x"); + config.names.emplace_back(tf_prefix + "force_mode/wrench_y"); + config.names.emplace_back(tf_prefix + "force_mode/wrench_z"); + config.names.emplace_back(tf_prefix + "force_mode/wrench_rx"); + config.names.emplace_back(tf_prefix + "force_mode/wrench_ry"); + config.names.emplace_back(tf_prefix + "force_mode/wrench_rz"); + config.names.emplace_back(tf_prefix + "force_mode/type"); + config.names.emplace_back(tf_prefix + "force_mode/limits_x"); + config.names.emplace_back(tf_prefix + "force_mode/limits_y"); + config.names.emplace_back(tf_prefix + "force_mode/limits_z"); + config.names.emplace_back(tf_prefix + "force_mode/limits_rx"); + config.names.emplace_back(tf_prefix + "force_mode/limits_ry"); + config.names.emplace_back(tf_prefix + "force_mode/limits_rz"); + config.names.emplace_back(tf_prefix + "force_mode/force_mode_async_success"); + config.names.emplace_back(tf_prefix + "force_mode/disable_cmd"); + config.names.emplace_back(tf_prefix + "force_mode/damping"); + config.names.emplace_back(tf_prefix + "force_mode/gain_scaling"); + + return config; +} + +controller_interface::InterfaceConfiguration ur_controllers::ForceModeController::state_interface_configuration() const +{ + controller_interface::InterfaceConfiguration config; + config.type = controller_interface::interface_configuration_type::INDIVIDUAL; + + const std::string tf_prefix = params_.tf_prefix; + // Get the state interface indicating whether the hardware interface has been initialized + config.names.emplace_back(tf_prefix + "system_interface/initialized"); + + return config; +} + +controller_interface::CallbackReturn +ur_controllers::ForceModeController::on_configure(const rclcpp_lifecycle::State& /*previous_state*/) +{ + const auto logger = get_node()->get_logger(); + + if (!param_listener_) { + RCLCPP_ERROR(get_node()->get_logger(), "Error encountered during configuration"); + return controller_interface::CallbackReturn::ERROR; + } + + // update the dynamic map parameters + param_listener_->refresh_dynamic_parameters(); + + // get parameters from the listener in case they were updated + params_ = param_listener_->get_params(); + + tf_buffer_ = std::make_unique(get_node()->get_clock()); + tf_listener_ = std::make_unique(*tf_buffer_); + + // Create the service server that will be used to start force mode + try { + set_force_mode_srv_ = get_node()->create_service( + "~/start_force_mode", + std::bind(&ForceModeController::setForceMode, this, std::placeholders::_1, std::placeholders::_2)); + disable_force_mode_srv_ = get_node()->create_service( + "~/stop_force_mode", + std::bind(&ForceModeController::disableForceMode, this, std::placeholders::_1, std::placeholders::_2)); + } catch (...) { + return LifecycleNodeInterface::CallbackReturn::ERROR; + } + + return LifecycleNodeInterface::CallbackReturn::SUCCESS; +} + +controller_interface::CallbackReturn +ur_controllers::ForceModeController::on_activate(const rclcpp_lifecycle::State& /*previous_state*/) +{ + change_requested_ = false; + force_mode_active_ = false; + async_state_ = std::numeric_limits::quiet_NaN(); + return LifecycleNodeInterface::CallbackReturn::SUCCESS; +} + +controller_interface::CallbackReturn +ur_controllers::ForceModeController::on_deactivate(const rclcpp_lifecycle::State& /*previous_state*/) +{ + // Stop force mode if this controller is deactivated. + command_interfaces_[CommandInterfaces::FORCE_MODE_DISABLE_CMD].set_value(1.0); + return LifecycleNodeInterface::CallbackReturn::SUCCESS; +} + +controller_interface::CallbackReturn +ur_controllers::ForceModeController::on_cleanup(const rclcpp_lifecycle::State& /*previous_state*/) +{ + set_force_mode_srv_.reset(); + disable_force_mode_srv_.reset(); + return CallbackReturn::SUCCESS; +} + +controller_interface::return_type ur_controllers::ForceModeController::update(const rclcpp::Time& /*time*/, + const rclcpp::Duration& /*period*/) +{ + async_state_ = command_interfaces_[CommandInterfaces::FORCE_MODE_ASYNC_SUCCESS].get_value(); + + // Publish state of force_mode? + if (change_requested_) { + if (force_mode_active_) { + const auto force_mode_parameters = force_mode_params_buffer_.readFromRT(); + command_interfaces_[CommandInterfaces::FORCE_MODE_TASK_FRAME_X].set_value(force_mode_parameters->task_frame[0]); + command_interfaces_[CommandInterfaces::FORCE_MODE_TASK_FRAME_Y].set_value(force_mode_parameters->task_frame[1]); + command_interfaces_[CommandInterfaces::FORCE_MODE_TASK_FRAME_Z].set_value(force_mode_parameters->task_frame[2]); + command_interfaces_[CommandInterfaces::FORCE_MODE_TASK_FRAME_RX].set_value(force_mode_parameters->task_frame[3]); + command_interfaces_[CommandInterfaces::FORCE_MODE_TASK_FRAME_RY].set_value(force_mode_parameters->task_frame[4]); + command_interfaces_[CommandInterfaces::FORCE_MODE_TASK_FRAME_RZ].set_value(force_mode_parameters->task_frame[5]); + + command_interfaces_[CommandInterfaces::FORCE_MODE_SELECTION_VECTOR_X].set_value( + force_mode_parameters->selection_vec[0]); + command_interfaces_[CommandInterfaces::FORCE_MODE_SELECTION_VECTOR_Y].set_value( + force_mode_parameters->selection_vec[1]); + command_interfaces_[CommandInterfaces::FORCE_MODE_SELECTION_VECTOR_Z].set_value( + force_mode_parameters->selection_vec[2]); + command_interfaces_[CommandInterfaces::FORCE_MODE_SELECTION_VECTOR_RX].set_value( + force_mode_parameters->selection_vec[3]); + command_interfaces_[CommandInterfaces::FORCE_MODE_SELECTION_VECTOR_RY].set_value( + force_mode_parameters->selection_vec[4]); + command_interfaces_[CommandInterfaces::FORCE_MODE_SELECTION_VECTOR_RZ].set_value( + force_mode_parameters->selection_vec[5]); + + command_interfaces_[CommandInterfaces::FORCE_MODE_WRENCH_X].set_value(force_mode_parameters->wrench.force.x); + command_interfaces_[CommandInterfaces::FORCE_MODE_WRENCH_Y].set_value(force_mode_parameters->wrench.force.y); + command_interfaces_[CommandInterfaces::FORCE_MODE_WRENCH_Z].set_value(force_mode_parameters->wrench.force.z); + command_interfaces_[CommandInterfaces::FORCE_MODE_WRENCH_RX].set_value(force_mode_parameters->wrench.torque.x); + command_interfaces_[CommandInterfaces::FORCE_MODE_WRENCH_RY].set_value(force_mode_parameters->wrench.torque.y); + command_interfaces_[CommandInterfaces::FORCE_MODE_WRENCH_RZ].set_value(force_mode_parameters->wrench.torque.z); + + command_interfaces_[CommandInterfaces::FORCE_MODE_LIMITS_X].set_value(force_mode_parameters->limits[0]); + command_interfaces_[CommandInterfaces::FORCE_MODE_LIMITS_Y].set_value(force_mode_parameters->limits[1]); + command_interfaces_[CommandInterfaces::FORCE_MODE_LIMITS_Z].set_value(force_mode_parameters->limits[2]); + command_interfaces_[CommandInterfaces::FORCE_MODE_LIMITS_RX].set_value(force_mode_parameters->limits[3]); + command_interfaces_[CommandInterfaces::FORCE_MODE_LIMITS_RY].set_value(force_mode_parameters->limits[4]); + command_interfaces_[CommandInterfaces::FORCE_MODE_LIMITS_RZ].set_value(force_mode_parameters->limits[5]); + + command_interfaces_[CommandInterfaces::FORCE_MODE_TYPE].set_value(force_mode_parameters->type); + command_interfaces_[CommandInterfaces::FORCE_MODE_DAMPING].set_value(force_mode_parameters->damping_factor); + command_interfaces_[CommandInterfaces::FORCE_MODE_GAIN_SCALING].set_value(force_mode_parameters->gain_scaling); + + // Signal that we are waiting for confirmation that force mode is activated + command_interfaces_[CommandInterfaces::FORCE_MODE_ASYNC_SUCCESS].set_value(ASYNC_WAITING); + async_state_ = ASYNC_WAITING; + } else { + command_interfaces_[CommandInterfaces::FORCE_MODE_DISABLE_CMD].set_value(1.0); + command_interfaces_[CommandInterfaces::FORCE_MODE_ASYNC_SUCCESS].set_value(ASYNC_WAITING); + async_state_ = ASYNC_WAITING; + } + change_requested_ = false; + } + + return controller_interface::return_type::OK; +} + +bool ForceModeController::setForceMode(const ur_msgs::srv::SetForceMode::Request::SharedPtr req, + ur_msgs::srv::SetForceMode::Response::SharedPtr resp) +{ + // Reject if controller is not active + if (get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE) { + RCLCPP_ERROR(get_node()->get_logger(), "Can't accept new requests. Controller is not running."); + resp->success = false; + return false; + } + + ForceModeParameters force_mode_parameters; + + // transform task frame into base + const std::string tf_prefix = params_.tf_prefix; + if (std::abs(req->task_frame.pose.orientation.x) < 1e-6 && std::abs(req->task_frame.pose.orientation.y) < 1e-6 && + std::abs(req->task_frame.pose.orientation.z) < 1e-6 && std::abs(req->task_frame.pose.orientation.w) < 1e-6) { + RCLCPP_ERROR(get_node()->get_logger(), "Received task frame with all-zeros quaternion. It should have at least one " + "non-zero entry."); + resp->success = false; + return false; + } + + try { + auto task_frame_transformed = tf_buffer_->transform(req->task_frame, tf_prefix + "base"); + + force_mode_parameters.task_frame[0] = task_frame_transformed.pose.position.x; + force_mode_parameters.task_frame[1] = task_frame_transformed.pose.position.y; + force_mode_parameters.task_frame[2] = task_frame_transformed.pose.position.z; + + tf2::Quaternion quat_tf; + tf2::convert(task_frame_transformed.pose.orientation, quat_tf); + tf2::Matrix3x3 rot_mat(quat_tf); + rot_mat.getRPY(force_mode_parameters.task_frame[3], force_mode_parameters.task_frame[4], + force_mode_parameters.task_frame[5]); + } catch (const tf2::TransformException& ex) { + RCLCPP_ERROR(get_node()->get_logger(), "Could not transform %s to robot base: %s", + req->task_frame.header.frame_id.c_str(), ex.what()); + resp->success = false; + return false; + } + + // The selection vector dictates which axes the robot should be compliant along and around + force_mode_parameters.selection_vec[0] = req->selection_vector_x; + force_mode_parameters.selection_vec[1] = req->selection_vector_y; + force_mode_parameters.selection_vec[2] = req->selection_vector_z; + force_mode_parameters.selection_vec[3] = req->selection_vector_rx; + force_mode_parameters.selection_vec[4] = req->selection_vector_ry; + force_mode_parameters.selection_vec[5] = req->selection_vector_rz; + + // The wrench parameters dictate the amount of force/torque the robot will apply to its environment. The robot will + // move along/around compliant axes to match the specified force/torque. Has no effect for non-compliant axes. + force_mode_parameters.wrench = req->wrench; + + /* The limits specifies the maximum allowed speed along/around compliant axes. For non-compliant axes this value is + * the maximum allowed deviation between actual tcp position and the one that has been programmed. */ + force_mode_parameters.limits[0] = req->selection_vector_x ? req->speed_limits.linear.x : req->deviation_limits[0]; + force_mode_parameters.limits[1] = req->selection_vector_y ? req->speed_limits.linear.y : req->deviation_limits[1]; + force_mode_parameters.limits[2] = req->selection_vector_z ? req->speed_limits.linear.z : req->deviation_limits[2]; + force_mode_parameters.limits[3] = req->selection_vector_rx ? req->speed_limits.angular.x : req->deviation_limits[3]; + force_mode_parameters.limits[4] = req->selection_vector_ry ? req->speed_limits.angular.y : req->deviation_limits[4]; + force_mode_parameters.limits[5] = req->selection_vector_rz ? req->speed_limits.angular.z : req->deviation_limits[5]; + + if (req->type < 1 || req->type > 3) { + RCLCPP_ERROR(get_node()->get_logger(), "The force mode type has to be 1, 2, or 3. Received %u", req->type); + resp->success = false; + return false; + } + + /* The type decides how the robot interprets the force frame (the one defined in task_frame). See ur_script manual + * for explanation, under force_mode. */ + force_mode_parameters.type = static_cast(req->type); + + /* The damping factor decides how fast the robot decelarates if no force is present. 0 means no deceleration, 1 + * means quick deceleration*/ + if (req->damping_factor < 0.0 || req->damping_factor > 1.0) { + RCLCPP_ERROR(get_node()->get_logger(), "The damping factor has to be between 0 and 1. Received %f", + req->damping_factor); + resp->success = false; + return false; + } + force_mode_parameters.damping_factor = req->damping_factor; + + /*The gain scaling factor scales the force mode gain. A value larger than 1 may make force mode unstable. */ + if (req->gain_scaling < 0.0 || req->gain_scaling > 2.0) { + RCLCPP_ERROR(get_node()->get_logger(), "The gain scaling has to be between 0 and 2. Received %f", + req->gain_scaling); + resp->success = false; + return false; + } + if (req->gain_scaling > 1.0) { + RCLCPP_WARN(get_node()->get_logger(), + "A gain_scaling >1.0 can make force mode unstable, e.g. in case of collisions or pushing against " + "hard surfaces. Received %f", + req->gain_scaling); + } + force_mode_parameters.gain_scaling = req->gain_scaling; + + force_mode_params_buffer_.writeFromNonRT(force_mode_parameters); + force_mode_active_ = true; + change_requested_ = true; + + RCLCPP_DEBUG(get_node()->get_logger(), "Waiting for force mode to be set."); + const auto maximum_retries = params_.check_io_successful_retries; + int retries = 0; + while (async_state_ == ASYNC_WAITING || change_requested_) { + std::this_thread::sleep_for(std::chrono::milliseconds(10)); + retries++; + + if (retries > maximum_retries) { + resp->success = false; + } + } + + resp->success = async_state_ == 1.0; + + if (resp->success) { + RCLCPP_INFO(get_node()->get_logger(), "Force mode has been set successfully."); + } else { + RCLCPP_ERROR(get_node()->get_logger(), "Could not set the force mode."); + return false; + } + + return true; +} + +bool ForceModeController::disableForceMode(const std_srvs::srv::Trigger::Request::SharedPtr /*req*/, + std_srvs::srv::Trigger::Response::SharedPtr resp) +{ + force_mode_active_ = false; + change_requested_ = true; + RCLCPP_DEBUG(get_node()->get_logger(), "Waiting for force mode to be disabled."); + while (async_state_ == ASYNC_WAITING || change_requested_) { + // Asynchronous wait until the hardware interface has set the force mode + std::this_thread::sleep_for(std::chrono::milliseconds(10)); + } + resp->success = async_state_ == 1.0; + if (resp->success) { + RCLCPP_INFO(get_node()->get_logger(), "Force mode has been disabled successfully."); + } else { + RCLCPP_ERROR(get_node()->get_logger(), "Could not disable force mode."); + return false; + } + return true; +} +} // namespace ur_controllers + +#include "pluginlib/class_list_macros.hpp" + +PLUGINLIB_EXPORT_CLASS(ur_controllers::ForceModeController, controller_interface::ControllerInterface) diff --git a/ur_controllers/src/force_mode_controller_parameters.yaml b/ur_controllers/src/force_mode_controller_parameters.yaml new file mode 100644 index 000000000..4eb69efbe --- /dev/null +++ b/ur_controllers/src/force_mode_controller_parameters.yaml @@ -0,0 +1,12 @@ +--- +force_mode_controller: + tf_prefix: { + type: string, + default_value: "", + description: "Urdf prefix of the corresponding arm" + } + check_io_successful_retries: { + type: int, + default_value: 10, + description: "Amount of retries for checking if setting force_mode was successful" + } diff --git a/ur_controllers/test/force_mode_controller_params.yaml b/ur_controllers/test/force_mode_controller_params.yaml new file mode 100644 index 000000000..648b9195c --- /dev/null +++ b/ur_controllers/test/force_mode_controller_params.yaml @@ -0,0 +1,7 @@ +--- +force_mode_controller: + ros__parameters: + tf_prefix: "" + damping: 0.025 + gain_scaling: 0.5 + check_io_successful_retries: 10 diff --git a/ur_controllers/test/test_load_force_mode_controller.cpp b/ur_controllers/test/test_load_force_mode_controller.cpp new file mode 100644 index 000000000..b7b016b61 --- /dev/null +++ b/ur_controllers/test/test_load_force_mode_controller.cpp @@ -0,0 +1,61 @@ +// 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. + +#include +#include "controller_manager/controller_manager.hpp" +#include "rclcpp/executor.hpp" +#include "rclcpp/executors/single_threaded_executor.hpp" +#include "rclcpp/utilities.hpp" +#include "ros2_control_test_assets/descriptions.hpp" + +TEST(TestLoadForceModeController, load_controller) +{ + std::shared_ptr executor = std::make_shared(); + + controller_manager::ControllerManager cm( + std::make_unique(ros2_control_test_assets::minimal_robot_urdf), executor, + "test_controller_manager"); + + const std::string test_file_path = std::string{ TEST_FILES_DIRECTORY } + "/force_mode_controller_params.yaml"; + cm.set_parameter({ "test_force_mode_controller.params_file", test_file_path }); + + cm.set_parameter({ "test_force_mode_controller.type", "ur_controllers/ForceModeController" }); + + ASSERT_NE(cm.load_controller("test_force_mode_controller"), nullptr); +} + +int main(int argc, char* argv[]) +{ + ::testing::InitGoogleMock(&argc, argv); + rclcpp::init(argc, argv); + + int result = RUN_ALL_TESTS(); + rclcpp::shutdown(); + + return result; +} diff --git a/ur_robot_driver/CMakeLists.txt b/ur_robot_driver/CMakeLists.txt index 26f533b3f..68c082aa1 100644 --- a/ur_robot_driver/CMakeLists.txt +++ b/ur_robot_driver/CMakeLists.txt @@ -33,6 +33,7 @@ find_package(std_srvs REQUIRED) find_package(tf2_geometry_msgs REQUIRED) find_package(ur_client_library REQUIRED) find_package(ur_dashboard_msgs REQUIRED) +find_package(ur_msgs REQUIRED) include_directories(include) @@ -49,6 +50,7 @@ set(THIS_PACKAGE_INCLUDE_DEPENDS tf2_geometry_msgs ur_client_library ur_dashboard_msgs + ur_msgs ) add_library(ur_robot_driver_plugin @@ -157,6 +159,9 @@ ament_python_install_package(${PROJECT_NAME}) install(PROGRAMS scripts/tool_communication.py scripts/example_move.py + scripts/start_ursim.sh + examples/examples.py + examples/force_mode.py DESTINATION lib/${PROJECT_NAME} ) @@ -197,6 +202,10 @@ if(BUILD_TESTING) TIMEOUT 800 ) + add_launch_test(test/integration_test_force_mode.py + TIMEOUT + 800 + ) add_launch_test(test/urscript_interface.py TIMEOUT 500 diff --git a/ur_robot_driver/config/ur_controllers.yaml b/ur_robot_driver/config/ur_controllers.yaml index 8c7e30b96..b93bfb146 100644 --- a/ur_robot_driver/config/ur_controllers.yaml +++ b/ur_robot_driver/config/ur_controllers.yaml @@ -24,6 +24,12 @@ controller_manager: forward_position_controller: type: position_controllers/JointGroupPositionController + force_mode_controller: + type: ur_controllers/ForceModeController + + passthrough_trajectory_controller: + type: ur_controllers/PassthroughTrajectoryController + tcp_pose_broadcaster: type: pose_broadcaster/PoseBroadcaster @@ -152,6 +158,10 @@ forward_position_controller: - $(var tf_prefix)wrist_2_joint - $(var tf_prefix)wrist_3_joint +force_mode_controller: + ros__parameters: + tf_prefix: "$(var tf_prefix)" + tcp_pose_broadcaster: ros__parameters: frame_id: $(var tf_prefix)base diff --git a/ur_robot_driver/examples/examples.py b/ur_robot_driver/examples/examples.py new file mode 100644 index 000000000..17b03a916 --- /dev/null +++ b/ur_robot_driver/examples/examples.py @@ -0,0 +1,180 @@ +#!/usr/bin/env python3 +# 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. + +import rclpy +from builtin_interfaces.msg import Duration +from control_msgs.action import FollowJointTrajectory + +from rclpy.action import ActionClient +from rclpy.node import Node +from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint +from ur_msgs.srv import SetIO +from controller_manager_msgs.srv import SwitchController +from std_srvs.srv import Trigger + +TIMEOUT_WAIT_SERVICE = 10 +TIMEOUT_WAIT_SERVICE_INITIAL = 60 +TIMEOUT_WAIT_ACTION = 10 + +ROBOT_JOINTS = [ + "shoulder_pan_joint", + "shoulder_lift_joint", + "elbow_joint", + "wrist_1_joint", + "wrist_2_joint", + "wrist_3_joint", +] + + +# Helper functions +def waitForService(node, srv_name, srv_type, timeout=TIMEOUT_WAIT_SERVICE): + client = node.create_client(srv_type, srv_name) + if client.wait_for_service(timeout) is False: + raise Exception(f"Could not reach service '{srv_name}' within timeout of {timeout}") + + node.get_logger().info(f"Successfully connected to service '{srv_name}'") + return client + + +def waitForAction(node, action_name, action_type, timeout=TIMEOUT_WAIT_ACTION): + client = ActionClient(node, action_type, action_name) + if client.wait_for_server(timeout) is False: + raise Exception( + f"Could not reach action server '{action_name}' within timeout of {timeout}" + ) + + node.get_logger().info(f"Successfully connected to action '{action_name}'") + return client + + +class Robot: + def __init__(self, node): + self.node = node + self.service_interfaces = { + "/io_and_status_controller/set_io": SetIO, + "/dashboard_client/play": Trigger, + "/controller_manager/switch_controller": SwitchController, + } + self.init_robot() + + def init_robot(self): + self.service_clients = { + srv_name: waitForService(self.node, srv_name, srv_type) + for (srv_name, srv_type) in self.service_interfaces.items() + } + + self.jtc_action_client = waitForAction( + self.node, + "/scaled_joint_trajectory_controller/follow_joint_trajectory", + FollowJointTrajectory, + ) + self.passthrough_trajectory_action_client = waitForAction( + self.node, + "/passthrough_trajectory_controller/follow_joint_trajectory", + FollowJointTrajectory, + ) + + def set_io(self, pin, value): + """Test to set an IO.""" + set_io_req = SetIO.Request() + set_io_req.fun = 1 + set_io_req.pin = pin + set_io_req.state = value + + self.call_service("/io_and_status_controller/set_io", set_io_req) + + def send_trajectory(self, waypts, time_vec, action_client): + """Send robot trajectory.""" + if len(waypts) != len(time_vec): + raise Exception("waypoints vector and time vec should be same length") + + # Construct test trajectory + joint_trajectory = JointTrajectory() + joint_trajectory.joint_names = ROBOT_JOINTS + for i in range(len(waypts)): + point = JointTrajectoryPoint() + point.positions = waypts[i] + point.time_from_start = time_vec[i] + joint_trajectory.points.append(point) + + # Sending trajectory goal + goal_response = self.call_action( + action_client, FollowJointTrajectory.Goal(trajectory=joint_trajectory) + ) + if not goal_response.accepted: + raise Exception("trajectory was not accepted") + + # Verify execution + result = self.get_result(action_client, goal_response) + return result.error_code == FollowJointTrajectory.Result.SUCCESSFUL + + def call_service(self, srv_name, request): + future = self.service_clients[srv_name].call_async(request) + rclpy.spin_until_future_complete(self.node, future) + if future.result() is not None: + return future.result() + else: + raise Exception(f"Exception while calling service: {future.exception()}") + + def call_action(self, ac_client, g): + future = ac_client.send_goal_async(g) + rclpy.spin_until_future_complete(self.node, future) + + if future.result() is not None: + return future.result() + else: + raise Exception(f"Exception while calling action: {future.exception()}") + + def get_result(self, ac_client, goal_response): + future_res = ac_client._get_result_async(goal_response) + rclpy.spin_until_future_complete(self.node, future_res) + if future_res.result() is not None: + return future_res.result().result + else: + raise Exception(f"Exception while calling action: {future_res.exception()}") + + +if __name__ == "__main__": + rclpy.init() + node = Node("robot_driver_test") + robot = Robot(node) + + # The following list are arbitrary joint positions, change according to your own needs + waypts = [ + [-1.6006, -1.7272, -2.2030, -0.8079, 1.5951, -0.0311], + [-1.2, -1.4, -1.9, -1.2, 1.5951, -0.0311], + [-1.6006, -1.7272, -2.2030, -0.8079, 1.5951, -0.0311], + ] + time_vec = [Duration(sec=4, nanosec=0), Duration(sec=8, nanosec=0), Duration(sec=12, nanosec=0)] + + # Execute trajectory on robot, make sure that the robot is booted and the control script is running + robot.send_trajectory(waypts, time_vec) + + # Set digital output 1 to true + robot.set_io(1, 1.0) diff --git a/ur_robot_driver/examples/force_mode.py b/ur_robot_driver/examples/force_mode.py new file mode 100755 index 000000000..86ea05b70 --- /dev/null +++ b/ur_robot_driver/examples/force_mode.py @@ -0,0 +1,142 @@ +#!/usr/bin/env python3 +# 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. + +import time + +import rclpy +from rclpy.node import Node +from controller_manager_msgs.srv import SwitchController +from builtin_interfaces.msg import Duration +from geometry_msgs.msg import Twist +from std_msgs.msg import Header +from std_srvs.srv import Trigger + +from geometry_msgs.msg import ( + Point, + Quaternion, + Pose, + PoseStamped, + Wrench, + Vector3, +) + +from ur_msgs.srv import SetForceMode + +from examples import Robot + +if __name__ == "__main__": + rclpy.init() + node = Node("robot_driver_test") + robot = Robot(node) + + # Add force mode service to service interfaces and re-init robot + robot.service_interfaces.update({"/force_mode_controller/start_force_mode": SetForceMode}) + robot.service_interfaces.update({"/force_mode_controller/stop_force_mode": Trigger}) + robot.init_robot() + time.sleep(0.5) + # Press play on the robot + robot.call_service("/dashboard_client/play", Trigger.Request()) + + time.sleep(0.5) + # Start controllers + robot.call_service( + "/controller_manager/switch_controller", + SwitchController.Request( + deactivate_controllers=["scaled_joint_trajectory_controller"], + activate_controllers=["passthrough_trajectory_controller", "force_mode_controller"], + strictness=SwitchController.Request.BEST_EFFORT, + ), + ) + + # Move robot in to position + robot.send_trajectory( + waypts=[[-1.5707, -1.5707, -1.5707, -1.5707, 1.5707, 0]], + time_vec=[Duration(sec=5, nanosec=0)], + action_client=robot.passthrough_trajectory_action_client, + ) + + # Finished moving + # Create task frame for force mode + point = Point(x=0.0, y=0.0, z=0.0) + orientation = Quaternion(x=0.0, y=0.0, z=0.0, w=1.0) + task_frame_pose = Pose() + task_frame_pose.position = point + task_frame_pose.orientation = orientation + header = Header(seq=1, frame_id="world") + header.stamp.sec = int(time.time()) + 1 + header.stamp.nanosec = 0 + frame_stamp = PoseStamped() + frame_stamp.header = header + frame_stamp.pose = task_frame_pose + + # Create compliance vector (which axes should be force controlled) + compliance = [False, False, True, False, False, False] + + # Create Wrench message for force mode + wrench_vec = Wrench(force=Vector3(x=0.0, y=0.0, z=-20.0), torque=Vector3(x=0.0, y=0.0, z=0.0)) + # Specify interpretation of task frame (no transform) + type_spec = SetForceMode.Request.NO_TRANSFORM + + # Specify max speeds and deviations of force mode + speed_limits = Twist() + speed_limits.linear = Vector3(x=0.0, y=0.0, z=1.0) + speed_limits.angular = Vector3(x=0.0, y=0.0, z=1.0) + deviation_limits = [1.0, 1.0, 1.0, 1.0, 1.0, 1.0] + + # specify damping and gain scaling + damping_factor = 0.1 + gain_scale = 0.8 + + req = SetForceMode.Request() + req.task_frame = frame_stamp + req.selection_vector_x = compliance[0] + req.selection_vector_y = compliance[1] + req.selection_vector_z = compliance[2] + req.selection_vector_rx = compliance[3] + req.selection_vector_ry = compliance[4] + req.selection_vector_rz = compliance[5] + req.wrench = wrench_vec + req.type = type_spec + req.speed_limits = speed_limits + req.deviation_limits = deviation_limits + req.damping_factor = damping_factor + req.gain_scaling = gain_scale + + # Send request to controller + node.get_logger().info(f"Starting force mode with {req}") + robot.call_service("/force_mode_controller/start_force_mode", req) + robot.send_trajectory( + waypts=[[1.5707, -1.5707, -1.5707, -1.5707, 1.5707, 0]], + time_vec=[Duration(sec=5, nanosec=0)], + action_client=robot.passthrough_trajectory_action_client, + ) + + time.sleep(3) + node.get_logger().info("Deactivating force mode controller.") + robot.call_service("/force_mode_controller/stop_force_mode", Trigger.Request()) diff --git a/ur_robot_driver/include/ur_robot_driver/hardware_interface.hpp b/ur_robot_driver/include/ur_robot_driver/hardware_interface.hpp index 428456113..1f665a1b2 100644 --- a/ur_robot_driver/include/ur_robot_driver/hardware_interface.hpp +++ b/ur_robot_driver/include/ur_robot_driver/hardware_interface.hpp @@ -78,7 +78,8 @@ enum StoppingInterface NONE, STOP_POSITION, STOP_VELOCITY, - STOP_PASSTHROUGH + STOP_PASSTHROUGH, + STOP_FORCE_MODE }; // We define our own quaternion to use it as a buffer, since we need to pass pointers to the state @@ -158,6 +159,8 @@ class URPositionHardwareInterface : public hardware_interface::SystemInterface void updateNonDoubleValues(); void extractToolPose(); void transformForceTorque(); + void start_force_mode(); + void stop_force_mode(); void check_passthrough_trajectory_controller(); void trajectory_done_callback(urcl::control::TrajectoryResult result); bool has_accelerations(std::vector> accelerations); @@ -238,6 +241,18 @@ class URPositionHardwareInterface : public hardware_interface::SystemInterface double payload_mass_; double payload_async_success_; + // force mode parameters + urcl::vector6d_t force_mode_task_frame_; + urcl::vector6d_t force_mode_selection_vector_; + urcl::vector6uint32_t force_mode_selection_vector_copy_; + urcl::vector6d_t force_mode_wrench_; + urcl::vector6d_t force_mode_limits_; + double force_mode_type_; + double force_mode_async_success_; + double force_mode_disable_cmd_; + double force_mode_damping_; + double force_mode_gain_scaling_; + // copy of non double values std::array actual_dig_out_bits_copy_; std::array actual_dig_in_bits_copy_; @@ -265,10 +280,11 @@ class URPositionHardwareInterface : public hardware_interface::SystemInterface double pausing_ramp_up_increment_; // resources switching aux vars - std::vector stop_modes_; - std::vector start_modes_; + std::vector> stop_modes_; + std::vector> start_modes_; bool position_controller_running_; bool velocity_controller_running_; + bool force_mode_controller_running_ = false; std::unique_ptr ur_driver_; std::shared_ptr async_thread_; @@ -277,6 +293,7 @@ class URPositionHardwareInterface : public hardware_interface::SystemInterface urcl::RobotReceiveTimeout receive_timeout_ = urcl::RobotReceiveTimeout::millisec(20); + const std::string FORCE_MODE_GPIO = "force_mode"; const std::string PASSTHROUGH_GPIO = "trajectory_passthrough"; }; } // namespace ur_robot_driver diff --git a/ur_robot_driver/launch/ur_control.launch.py b/ur_robot_driver/launch/ur_control.launch.py index f98d7a880..02fd59a0c 100644 --- a/ur_robot_driver/launch/ur_control.launch.py +++ b/ur_robot_driver/launch/ur_control.launch.py @@ -347,6 +347,7 @@ def controller_spawner(controllers, active=True): "joint_trajectory_controller", "forward_velocity_controller", "forward_position_controller", + "force_mode_controller", "passthrough_trajectory_controller", ] if activate_joint_controller.perform(context) == "true": diff --git a/ur_robot_driver/src/hardware_interface.cpp b/ur_robot_driver/src/hardware_interface.cpp index 30285f9a2..9cb3350ec 100644 --- a/ur_robot_driver/src/hardware_interface.cpp +++ b/ur_robot_driver/src/hardware_interface.cpp @@ -81,9 +81,6 @@ URPositionHardwareInterface::on_init(const hardware_interface::HardwareInfo& sys urcl_position_commands_ = { { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 } }; urcl_position_commands_old_ = { { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 } }; urcl_velocity_commands_ = { { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 } }; - stop_modes_ = { StoppingInterface::NONE, StoppingInterface::NONE, StoppingInterface::NONE, - StoppingInterface::NONE, StoppingInterface::NONE, StoppingInterface::NONE }; - start_modes_ = {}; position_controller_running_ = false; velocity_controller_running_ = false; passthrough_trajectory_controller_running_ = false; @@ -317,6 +314,36 @@ std::vector URPositionHardwareInterface::e command_interfaces.emplace_back( hardware_interface::CommandInterface(tf_prefix + "payload", "payload_async_success", &payload_async_success_)); + command_interfaces.emplace_back(tf_prefix + FORCE_MODE_GPIO, "task_frame_x", &force_mode_task_frame_[0]); + command_interfaces.emplace_back(tf_prefix + FORCE_MODE_GPIO, "task_frame_y", &force_mode_task_frame_[1]); + command_interfaces.emplace_back(tf_prefix + FORCE_MODE_GPIO, "task_frame_z", &force_mode_task_frame_[2]); + command_interfaces.emplace_back(tf_prefix + FORCE_MODE_GPIO, "task_frame_rx", &force_mode_task_frame_[3]); + command_interfaces.emplace_back(tf_prefix + FORCE_MODE_GPIO, "task_frame_ry", &force_mode_task_frame_[4]); + command_interfaces.emplace_back(tf_prefix + FORCE_MODE_GPIO, "task_frame_rz", &force_mode_task_frame_[5]); + command_interfaces.emplace_back(tf_prefix + FORCE_MODE_GPIO, "selection_vector_x", &force_mode_selection_vector_[0]); + command_interfaces.emplace_back(tf_prefix + FORCE_MODE_GPIO, "selection_vector_y", &force_mode_selection_vector_[1]); + command_interfaces.emplace_back(tf_prefix + FORCE_MODE_GPIO, "selection_vector_z", &force_mode_selection_vector_[2]); + command_interfaces.emplace_back(tf_prefix + FORCE_MODE_GPIO, "selection_vector_rx", &force_mode_selection_vector_[3]); + command_interfaces.emplace_back(tf_prefix + FORCE_MODE_GPIO, "selection_vector_ry", &force_mode_selection_vector_[4]); + command_interfaces.emplace_back(tf_prefix + FORCE_MODE_GPIO, "selection_vector_rz", &force_mode_selection_vector_[5]); + command_interfaces.emplace_back(tf_prefix + FORCE_MODE_GPIO, "wrench_x", &force_mode_wrench_[0]); + command_interfaces.emplace_back(tf_prefix + FORCE_MODE_GPIO, "wrench_y", &force_mode_wrench_[1]); + command_interfaces.emplace_back(tf_prefix + FORCE_MODE_GPIO, "wrench_z", &force_mode_wrench_[2]); + command_interfaces.emplace_back(tf_prefix + FORCE_MODE_GPIO, "wrench_rx", &force_mode_wrench_[3]); + command_interfaces.emplace_back(tf_prefix + FORCE_MODE_GPIO, "wrench_ry", &force_mode_wrench_[4]); + command_interfaces.emplace_back(tf_prefix + FORCE_MODE_GPIO, "wrench_rz", &force_mode_wrench_[5]); + command_interfaces.emplace_back(tf_prefix + FORCE_MODE_GPIO, "type", &force_mode_type_); + command_interfaces.emplace_back(tf_prefix + FORCE_MODE_GPIO, "limits_x", &force_mode_limits_[0]); + command_interfaces.emplace_back(tf_prefix + FORCE_MODE_GPIO, "limits_y", &force_mode_limits_[1]); + command_interfaces.emplace_back(tf_prefix + FORCE_MODE_GPIO, "limits_z", &force_mode_limits_[2]); + command_interfaces.emplace_back(tf_prefix + FORCE_MODE_GPIO, "limits_rx", &force_mode_limits_[3]); + command_interfaces.emplace_back(tf_prefix + FORCE_MODE_GPIO, "limits_ry", &force_mode_limits_[4]); + command_interfaces.emplace_back(tf_prefix + FORCE_MODE_GPIO, "limits_rz", &force_mode_limits_[5]); + command_interfaces.emplace_back(tf_prefix + FORCE_MODE_GPIO, "force_mode_async_success", &force_mode_async_success_); + command_interfaces.emplace_back(tf_prefix + FORCE_MODE_GPIO, "disable_cmd", &force_mode_disable_cmd_); + command_interfaces.emplace_back(tf_prefix + FORCE_MODE_GPIO, "damping", &force_mode_damping_); + command_interfaces.emplace_back(tf_prefix + FORCE_MODE_GPIO, "gain_scaling", &force_mode_gain_scaling_); + for (size_t i = 0; i < 18; ++i) { command_interfaces.emplace_back(hardware_interface::CommandInterface( tf_prefix + "gpio", "standard_digital_output_cmd_" + std::to_string(i), &standard_dig_out_bits_cmd_[i])); @@ -541,6 +568,14 @@ hardware_interface::CallbackReturn URPositionHardwareInterface::on_activate(const rclcpp_lifecycle::State& previous_state) { RCLCPP_INFO(rclcpp::get_logger("URPositionHardwareInterface"), "Activating HW interface"); + + for (size_t i = 0; i < 6; i++) { + force_mode_task_frame_[i] = NO_NEW_CMD_; + force_mode_selection_vector_[i] = static_cast(NO_NEW_CMD_); + force_mode_wrench_[i] = NO_NEW_CMD_; + force_mode_limits_[i] = NO_NEW_CMD_; + } + force_mode_type_ = static_cast(NO_NEW_CMD_); return hardware_interface::CallbackReturn::SUCCESS; } @@ -680,6 +715,7 @@ hardware_interface::return_type URPositionHardwareInterface::read(const rclcpp:: resend_robot_program_cmd_ = NO_NEW_CMD_; zero_ftsensor_cmd_ = NO_NEW_CMD_; hand_back_control_cmd_ = NO_NEW_CMD_; + force_mode_disable_cmd_ = NO_NEW_CMD_; initialized_ = true; } @@ -715,6 +751,14 @@ hardware_interface::return_type URPositionHardwareInterface::write(const rclcpp: ur_driver_->writeKeepalive(); } + if (!std::isnan(force_mode_task_frame_[0]) && !std::isnan(force_mode_selection_vector_[0]) && + !std::isnan(force_mode_wrench_[0]) && !std::isnan(force_mode_type_) && !std::isnan(force_mode_limits_[0]) && + !std::isnan(force_mode_damping_) && !std::isnan(force_mode_gain_scaling_) && ur_driver_ != nullptr) { + start_force_mode(); + } else if (!std::isnan(force_mode_disable_cmd_) && ur_driver_ != nullptr && force_mode_async_success_ == 2.0) { + stop_force_mode(); + } + packet_read_ = false; } @@ -880,26 +924,29 @@ hardware_interface::return_type URPositionHardwareInterface::prepare_command_mod { hardware_interface::return_type ret_val = hardware_interface::return_type::OK; - start_modes_ = std::vector(info_.joints.size(), "UNDEFINED"); - stop_modes_.clear(); - std::vector control_modes(info_.joints.size()); + start_modes_ = std::vector>(info_.joints.size()); + stop_modes_ = std::vector>(info_.joints.size()); + std::vector> control_modes(info_.joints.size()); const std::string tf_prefix = info_.hardware_parameters.at("tf_prefix"); // Assess current state for (auto i = 0u; i < info_.joints.size(); i++) { if (position_controller_running_) { - control_modes[i] = hardware_interface::HW_IF_POSITION; + control_modes[i] = { hardware_interface::HW_IF_POSITION }; } if (velocity_controller_running_) { - control_modes[i] = hardware_interface::HW_IF_VELOCITY; + control_modes[i] = { hardware_interface::HW_IF_VELOCITY }; + } + if (force_mode_controller_running_) { + control_modes[i].push_back(FORCE_MODE_GPIO); } if (passthrough_trajectory_controller_running_) { - control_modes[i] = PASSTHROUGH_GPIO; + control_modes[i].push_back(PASSTHROUGH_GPIO); } } if (!std::all_of(start_modes_.begin() + 1, start_modes_.end(), - [&](const std::string& other) { return other == start_modes_[0]; })) { + [&](const std::vector& other) { return other == start_modes_[0]; })) { RCLCPP_ERROR(rclcpp::get_logger("URPositionHardwareInterface"), "Start modes of all joints have to be the same."); return hardware_interface::return_type::ERROR; } @@ -910,20 +957,43 @@ hardware_interface::return_type URPositionHardwareInterface::prepare_command_mod for (const auto& key : start_interfaces) { for (auto i = 0u; i < info_.joints.size(); i++) { if (key == info_.joints[i].name + "/" + hardware_interface::HW_IF_POSITION) { - if (start_modes_[i] != "UNDEFINED") { + if (!start_modes_[i].empty()) { + RCLCPP_ERROR(rclcpp::get_logger("URPositionHardwareInterface"), "Attempting to start position control while " + "there is another control mode already " + "requested."); return hardware_interface::return_type::ERROR; } - start_modes_[i] = hardware_interface::HW_IF_POSITION; + start_modes_[i] = { hardware_interface::HW_IF_POSITION }; } else if (key == info_.joints[i].name + "/" + hardware_interface::HW_IF_VELOCITY) { - if (start_modes_[i] != "UNDEFINED") { + if (!start_modes_[i].empty()) { + RCLCPP_ERROR(rclcpp::get_logger("URPositionHardwareInterface"), "Attempting to start velocity control while " + "there is another control mode already " + "requested."); return hardware_interface::return_type::ERROR; } - start_modes_[i] = hardware_interface::HW_IF_VELOCITY; + start_modes_[i] = { hardware_interface::HW_IF_VELOCITY }; + } else if (key == tf_prefix + FORCE_MODE_GPIO + "/type") { + if (std::any_of(start_modes_[i].begin(), start_modes_[i].end(), [&](const std::string& item) { + return item == hardware_interface::HW_IF_POSITION || item == hardware_interface::HW_IF_VELOCITY; + })) { + RCLCPP_ERROR(rclcpp::get_logger("URPositionHardwareInterface"), "Attempting to start force_mode control " + "while there is either position or " + "velocity mode already requested by another " + "controller."); + return hardware_interface::return_type::ERROR; + } + start_modes_[i].push_back(FORCE_MODE_GPIO); } else if (key == tf_prefix + PASSTHROUGH_GPIO + "/setpoint_positions_" + std::to_string(i)) { - if (start_modes_[i] != "UNDEFINED") { + if (std::any_of(start_modes_[i].begin(), start_modes_[i].end(), [&](const std::string& item) { + return item == hardware_interface::HW_IF_POSITION || item == hardware_interface::HW_IF_VELOCITY; + })) { + RCLCPP_ERROR(rclcpp::get_logger("URPositionHardwareInterface"), "Attempting to start trajectory passthrough " + "control while there is either " + "position or velocity mode already requested " + "by another controller."); return hardware_interface::return_type::ERROR; } - start_modes_[i] = PASSTHROUGH_GPIO; + start_modes_[i].push_back(PASSTHROUGH_GPIO); } } } @@ -933,56 +1003,99 @@ hardware_interface::return_type URPositionHardwareInterface::prepare_command_mod for (const auto& key : stop_interfaces) { for (auto i = 0u; i < info_.joints.size(); i++) { if (key == info_.joints[i].name + "/" + hardware_interface::HW_IF_POSITION) { - stop_modes_.push_back(StoppingInterface::STOP_POSITION); - if (control_modes[i] == hardware_interface::HW_IF_POSITION) { - control_modes[i] = "UNDEFINED"; - } + stop_modes_[i].push_back(StoppingInterface::STOP_POSITION); + control_modes[i].erase( + std::remove_if(control_modes[i].begin(), control_modes[i].end(), + [](const std::string& item) { return item == hardware_interface::HW_IF_POSITION; }), + control_modes[i].end()); } if (key == info_.joints[i].name + "/" + hardware_interface::HW_IF_VELOCITY) { - stop_modes_.push_back(StoppingInterface::STOP_VELOCITY); - if (control_modes[i] == hardware_interface::HW_IF_VELOCITY) { - control_modes[i] = "UNDEFINED"; - } + stop_modes_[i].push_back(StoppingInterface::STOP_VELOCITY); + control_modes[i].erase( + std::remove_if(control_modes[i].begin(), control_modes[i].end(), + [](const std::string& item) { return item == hardware_interface::HW_IF_VELOCITY; }), + control_modes[i].end()); + } + if (key == tf_prefix + FORCE_MODE_GPIO + "/disable_cmd") { + stop_modes_[i].push_back(StoppingInterface::STOP_FORCE_MODE); + control_modes[i].erase(std::remove_if(control_modes[i].begin(), control_modes[i].end(), + [&](const std::string& item) { return item == FORCE_MODE_GPIO; }), + control_modes[i].end()); } if (key == tf_prefix + PASSTHROUGH_GPIO + "/setpoint_positions_" + std::to_string(i)) { - stop_modes_.push_back(StoppingInterface::STOP_PASSTHROUGH); - if (control_modes[i] == PASSTHROUGH_GPIO) { - control_modes[i] = "UNDEFINED"; - } + stop_modes_[i].push_back(StoppingInterface::STOP_PASSTHROUGH); + control_modes[i].erase(std::remove_if(control_modes[i].begin(), control_modes[i].end(), + [&](const std::string& item) { return item == PASSTHROUGH_GPIO; }), + control_modes[i].end()); } } } // Do not start conflicting controllers - if (std::any_of(start_modes_.begin(), start_modes_.end(), + if (std::any_of(start_modes_[0].begin(), start_modes_[0].end(), [this](auto& item) { return (item == PASSTHROUGH_GPIO); }) && - (std::any_of(start_modes_.begin(), start_modes_.end(), + (std::any_of(start_modes_[0].begin(), start_modes_[0].end(), + [](auto& item) { + return (item == hardware_interface::HW_IF_VELOCITY || item == hardware_interface::HW_IF_POSITION); + }) || + std::any_of(control_modes[0].begin(), control_modes[0].end(), [this](auto& item) { + return (item == hardware_interface::HW_IF_VELOCITY || item == hardware_interface::HW_IF_POSITION); + }))) { + RCLCPP_ERROR(rclcpp::get_logger("URPositionHardwareInterface"), "Attempting to start passthrough_trajectory " + "control while there is either position or " + "velocity mode is running."); + ret_val = hardware_interface::return_type::ERROR; + } + + if (std::any_of(start_modes_[0].begin(), start_modes_[0].end(), + [this](auto& item) { return (item == FORCE_MODE_GPIO); }) && + (std::any_of(start_modes_[0].begin(), start_modes_[0].end(), [](auto& item) { return (item == hardware_interface::HW_IF_VELOCITY || item == hardware_interface::HW_IF_POSITION); }) || - std::any_of(control_modes.begin(), control_modes.end(), [this](auto& item) { + std::any_of(control_modes[0].begin(), control_modes[0].end(), [this](auto& item) { return (item == hardware_interface::HW_IF_VELOCITY || item == hardware_interface::HW_IF_POSITION || - item == PASSTHROUGH_GPIO); + item == FORCE_MODE_GPIO); }))) { + RCLCPP_ERROR(rclcpp::get_logger("URPositionHardwareInterface"), "Attempting to start force mode control while " + "there is either position or " + "velocity mode running."); ret_val = hardware_interface::return_type::ERROR; } - if (std::any_of(start_modes_.begin(), start_modes_.end(), + + // Position mode requested to start + if (std::any_of(start_modes_[0].begin(), start_modes_[0].end(), [](auto& item) { return (item == hardware_interface::HW_IF_POSITION); }) && - (std::any_of( - start_modes_.begin(), start_modes_.end(), - [this](auto& item) { return (item == hardware_interface::HW_IF_VELOCITY || item == PASSTHROUGH_GPIO); }) || - std::any_of(control_modes.begin(), control_modes.end(), [this](auto& item) { + (std::any_of(start_modes_[0].begin(), start_modes_[0].end(), + [this](auto& item) { + return (item == hardware_interface::HW_IF_VELOCITY || item == PASSTHROUGH_GPIO || + item == FORCE_MODE_GPIO); + }) || + std::any_of(control_modes[0].begin(), control_modes[0].end(), [this](auto& item) { return (item == hardware_interface::HW_IF_VELOCITY || item == hardware_interface::HW_IF_POSITION || - item == PASSTHROUGH_GPIO); + item == PASSTHROUGH_GPIO || item == FORCE_MODE_GPIO); }))) { + RCLCPP_ERROR(rclcpp::get_logger("URPositionHardwareInterface"), "Attempting to start position control while there " + "is either trajectory passthrough or " + "velocity mode or force_mode running."); ret_val = hardware_interface::return_type::ERROR; } - if (std::any_of(start_modes_.begin(), start_modes_.end(), + + // Velocity mode requested to start + if (std::any_of(start_modes_[0].begin(), start_modes_[0].end(), [](auto& item) { return (item == hardware_interface::HW_IF_VELOCITY); }) && - std::any_of(start_modes_.begin(), start_modes_.end(), [this](auto& item) { - return (item == hardware_interface::HW_IF_VELOCITY || item == hardware_interface::HW_IF_POSITION || - item == PASSTHROUGH_GPIO); - })) { + (std::any_of(start_modes_[0].begin(), start_modes_[0].end(), + [this](auto& item) { + return (item == hardware_interface::HW_IF_POSITION || item == PASSTHROUGH_GPIO || + item == FORCE_MODE_GPIO); + }) || + std::any_of(control_modes[0].begin(), control_modes[0].end(), [this](auto& item) { + return (item == hardware_interface::HW_IF_VELOCITY || item == hardware_interface::HW_IF_POSITION || + item == PASSTHROUGH_GPIO || item == FORCE_MODE_GPIO); + }))) { + RCLCPP_ERROR(rclcpp::get_logger("URPositionHardwareInterface"), "Attempting to start velosity control while there " + "is either trajectory passthrough or " + "position mode or force_mode running."); ret_val = hardware_interface::return_type::ERROR; } @@ -995,16 +1108,20 @@ hardware_interface::return_type URPositionHardwareInterface::perform_command_mod { hardware_interface::return_type ret_val = hardware_interface::return_type::OK; - if (stop_modes_.size() != 0 && - std::find(stop_modes_.begin(), stop_modes_.end(), StoppingInterface::STOP_POSITION) != stop_modes_.end()) { + if (stop_modes_[0].size() != 0 && std::find(stop_modes_[0].begin(), stop_modes_[0].end(), + StoppingInterface::STOP_POSITION) != stop_modes_[0].end()) { position_controller_running_ = false; urcl_position_commands_ = urcl_position_commands_old_ = urcl_joint_positions_; - } else if (stop_modes_.size() != 0 && - std::find(stop_modes_.begin(), stop_modes_.end(), StoppingInterface::STOP_VELOCITY) != stop_modes_.end()) { + } else if (stop_modes_[0].size() != 0 && std::find(stop_modes_[0].begin(), stop_modes_[0].end(), + StoppingInterface::STOP_VELOCITY) != stop_modes_[0].end()) { velocity_controller_running_ = false; urcl_velocity_commands_ = { { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 } }; - } else if (stop_modes_.size() != 0 && std::find(stop_modes_.begin(), stop_modes_.end(), - StoppingInterface::STOP_PASSTHROUGH) != stop_modes_.end()) { + } else if (stop_modes_[0].size() != 0 && std::find(stop_modes_[0].begin(), stop_modes_[0].end(), + StoppingInterface::STOP_FORCE_MODE) != stop_modes_[0].end()) { + force_mode_controller_running_ = false; + stop_force_mode(); + } else if (stop_modes_[0].size() != 0 && std::find(stop_modes_[0].begin(), stop_modes_[0].end(), + StoppingInterface::STOP_PASSTHROUGH) != stop_modes_[0].end()) { passthrough_trajectory_controller_running_ = false; passthrough_trajectory_abort_ = 1.0; trajectory_joint_positions_.clear(); @@ -1012,21 +1129,24 @@ hardware_interface::return_type URPositionHardwareInterface::perform_command_mod trajectory_joint_velocities_.clear(); } - if (start_modes_.size() != 0 && - std::find(start_modes_.begin(), start_modes_.end(), hardware_interface::HW_IF_POSITION) != start_modes_.end()) { + if (start_modes_.size() != 0 && std::find(start_modes_[0].begin(), start_modes_[0].end(), + hardware_interface::HW_IF_POSITION) != start_modes_[0].end()) { velocity_controller_running_ = false; passthrough_trajectory_controller_running_ = false; urcl_position_commands_ = urcl_position_commands_old_ = urcl_joint_positions_; position_controller_running_ = true; - } else if (start_modes_.size() != 0 && std::find(start_modes_.begin(), start_modes_.end(), - hardware_interface::HW_IF_VELOCITY) != start_modes_.end()) { + } else if (start_modes_[0].size() != 0 && std::find(start_modes_[0].begin(), start_modes_[0].end(), + hardware_interface::HW_IF_VELOCITY) != start_modes_[0].end()) { position_controller_running_ = false; passthrough_trajectory_controller_running_ = false; urcl_velocity_commands_ = { { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 } }; velocity_controller_running_ = true; - } else if (start_modes_.size() != 0 && - std::find(start_modes_.begin(), start_modes_.end(), PASSTHROUGH_GPIO) != start_modes_.end()) { + } else if (start_modes_[0].size() != 0 && + std::find(start_modes_[0].begin(), start_modes_[0].end(), FORCE_MODE_GPIO) != start_modes_[0].end()) { + force_mode_controller_running_ = true; + } else if (start_modes_[0].size() != 0 && + std::find(start_modes_[0].begin(), start_modes_[0].end(), PASSTHROUGH_GPIO) != start_modes_[0].end()) { velocity_controller_running_ = false; position_controller_running_ = false; passthrough_trajectory_controller_running_ = true; @@ -1039,6 +1159,46 @@ hardware_interface::return_type URPositionHardwareInterface::perform_command_mod return ret_val; } +void URPositionHardwareInterface::start_force_mode() +{ + for (size_t i = 0; i < force_mode_selection_vector_.size(); i++) { + force_mode_selection_vector_copy_[i] = force_mode_selection_vector_[i]; + } + /* Check version of robot to ensure that the correct startForceMode is called. */ + if (ur_driver_->getVersion().major < 5) { + force_mode_async_success_ = + ur_driver_->startForceMode(force_mode_task_frame_, force_mode_selection_vector_copy_, force_mode_wrench_, + force_mode_type_, force_mode_limits_, force_mode_damping_); + if (force_mode_gain_scaling_ != 0.5) { + RCLCPP_WARN(rclcpp::get_logger("URPositionHardwareInterface"), "Force mode gain scaling cannot be used on " + "CB3 " + "robots. Starting force mode, but " + "disregarding " + "gain scaling."); + } + } else { + force_mode_async_success_ = + ur_driver_->startForceMode(force_mode_task_frame_, force_mode_selection_vector_copy_, force_mode_wrench_, + force_mode_type_, force_mode_limits_, force_mode_damping_, force_mode_gain_scaling_); + } + + for (size_t i = 0; i < 6; i++) { + force_mode_task_frame_[i] = NO_NEW_CMD_; + force_mode_selection_vector_[i] = static_cast(NO_NEW_CMD_); + force_mode_wrench_[i] = NO_NEW_CMD_; + force_mode_limits_[i] = NO_NEW_CMD_; + } + force_mode_type_ = static_cast(NO_NEW_CMD_); + force_mode_damping_ = NO_NEW_CMD_; + force_mode_gain_scaling_ = NO_NEW_CMD_; +} + +void URPositionHardwareInterface::stop_force_mode() +{ + force_mode_async_success_ = ur_driver_->endForceMode(); + force_mode_disable_cmd_ = NO_NEW_CMD_; +} + void URPositionHardwareInterface::check_passthrough_trajectory_controller() { static double last_time = 0.0; diff --git a/ur_robot_driver/test/integration_test_controller_switch.py b/ur_robot_driver/test/integration_test_controller_switch.py index 20ea69190..bdc9a0eca 100644 --- a/ur_robot_driver/test/integration_test_controller_switch.py +++ b/ur_robot_driver/test/integration_test_controller_switch.py @@ -52,7 +52,8 @@ @pytest.mark.launch_test @launch_testing.parametrize( "tf_prefix", - [(""), ("my_ur_")], + [("")], + # [(""), ("my_ur_")], ) def generate_test_description(tf_prefix): return generate_driver_test_description(tf_prefix=tf_prefix) @@ -94,6 +95,7 @@ def test_activating_multiple_controllers_same_interface_fails(self): "forward_position_controller", "forward_velocity_controller", "passthrough_trajectory_controller", + "force_mode_controller", ], ).ok ) @@ -117,15 +119,6 @@ def test_activating_multiple_controllers_same_interface_fails(self): ], ).ok ) - self.assertFalse( - self._controller_manager_interface.switch_controller( - strictness=SwitchController.Request.STRICT, - activate_controllers=[ - "scaled_joint_trajectory_controller", - "forward_position_controller", - ], - ).ok - ) def test_activating_multiple_controllers_different_interface_fails(self): # Deactivate all writing controllers @@ -137,6 +130,7 @@ def test_activating_multiple_controllers_different_interface_fails(self): "joint_trajectory_controller", "forward_position_controller", "forward_velocity_controller", + "force_mode_controller", "passthrough_trajectory_controller", ], ).ok @@ -177,6 +171,15 @@ def test_activating_multiple_controllers_different_interface_fails(self): ], ).ok ) + self.assertFalse( + self._controller_manager_interface.switch_controller( + strictness=SwitchController.Request.STRICT, + activate_controllers=[ + "scaled_joint_trajectory_controller", + "force_mode_controller", + ], + ).ok + ) def test_activating_controller_with_running_position_controller_fails(self): # Having a position-based controller active, no other controller should be able to @@ -191,6 +194,7 @@ def test_activating_controller_with_running_position_controller_fails(self): "joint_trajectory_controller", "forward_position_controller", "forward_velocity_controller", + "force_mode_controller", "passthrough_trajectory_controller", ], ).ok @@ -219,6 +223,14 @@ def test_activating_controller_with_running_position_controller_fails(self): ], ).ok ) + self.assertFalse( + self._controller_manager_interface.switch_controller( + strictness=SwitchController.Request.STRICT, + activate_controllers=[ + "force_mode_controller", + ], + ).ok + ) # Stop controller again self.assertTrue( self._controller_manager_interface.switch_controller( @@ -241,6 +253,7 @@ def test_activating_controller_with_running_passthrough_trajectory_controller_fa "joint_trajectory_controller", "forward_position_controller", "forward_velocity_controller", + "force_mode_controller", # tested in separate test ], ).ok ) @@ -285,3 +298,82 @@ def test_activating_controller_with_running_passthrough_trajectory_controller_fa ], ).ok ) + + def test_force_mode_and_trajectory_passthrough_controller_are_compatible(self): + # Deactivate all writing controllers + self.assertTrue( + self._controller_manager_interface.switch_controller( + strictness=SwitchController.Request.BEST_EFFORT, + deactivate_controllers=[ + "scaled_joint_trajectory_controller", + "joint_trajectory_controller", + "forward_position_controller", + "forward_velocity_controller", + "passthrough_trajectory_controller", + "force_mode_controller", + ], + ).ok + ) + + time.sleep(3) + + # Start both together + self.assertTrue( + self._controller_manager_interface.switch_controller( + strictness=SwitchController.Request.STRICT, + activate_controllers=[ + "passthrough_trajectory_controller", + "force_mode_controller", + ], + ).ok + ) + + # With passthrough traj controller running, start force mode controller + self.assertTrue( + self._controller_manager_interface.switch_controller( + strictness=SwitchController.Request.BEST_EFFORT, + activate_controllers=[ + "passthrough_trajectory_controller", + ], + deactivate_controllers=[ + "scaled_joint_trajectory_controller", + "joint_trajectory_controller", + "forward_position_controller", + "forward_velocity_controller", + "force_mode_controller", + ], + ).ok + ) + self.assertTrue( + self._controller_manager_interface.switch_controller( + strictness=SwitchController.Request.STRICT, + activate_controllers=[ + "force_mode_controller", + ], + ).ok + ) + + # With start force mode controller running, passthrough traj controller + self.assertTrue( + self._controller_manager_interface.switch_controller( + strictness=SwitchController.Request.BEST_EFFORT, + activate_controllers=[ + "force_mode_controller", + ], + deactivate_controllers=[ + "scaled_joint_trajectory_controller", + "joint_trajectory_controller", + "forward_position_controller", + "forward_velocity_controller", + "passthrough_trajectory_controller", + ], + ).ok + ) + self.assertTrue( + self._controller_manager_interface.switch_controller( + strictness=SwitchController.Request.STRICT, + activate_controllers=[ + "passthrough_trajectory_controller", + ], + ).ok + ) diff --git a/ur_robot_driver/test/integration_test_force_mode.py b/ur_robot_driver/test/integration_test_force_mode.py new file mode 100644 index 000000000..5c793081b --- /dev/null +++ b/ur_robot_driver/test/integration_test_force_mode.py @@ -0,0 +1,524 @@ +#!/usr/bin/env python +# Copyright 2019, 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 os +import sys +import time +import unittest + +import pytest + +import launch_testing +import rclpy +from rclpy.node import Node + +from tf2_ros import TransformException +from tf2_ros.buffer import Buffer +from tf2_ros.transform_listener import TransformListener + +import std_msgs +from controller_manager_msgs.srv import SwitchController +from geometry_msgs.msg import ( + Pose, + PoseStamped, + Quaternion, + Point, + Twist, + Wrench, + Vector3, +) + +sys.path.append(os.path.dirname(__file__)) +from test_common import ( # noqa: E402 + ControllerManagerInterface, + DashboardInterface, + ForceModeInterface, + IoStatusInterface, + ConfigurationInterface, + generate_driver_test_description, +) + +TIMEOUT_EXECUTE_TRAJECTORY = 30 + + +@pytest.mark.launch_test +@launch_testing.parametrize( + "tf_prefix", + [(""), ("my_ur_")], +) +def generate_test_description(tf_prefix): + return generate_driver_test_description(tf_prefix=tf_prefix) + + +class RobotDriverTest(unittest.TestCase): + @classmethod + def setUpClass(cls): + # Initialize the ROS context + rclpy.init() + cls.node = Node("robot_driver_test") + time.sleep(1) + cls.init_robot(cls) + + @classmethod + def tearDownClass(cls): + # Shutdown the ROS context + cls.node.destroy_node() + rclpy.shutdown() + + def init_robot(self): + self._dashboard_interface = DashboardInterface(self.node) + self._controller_manager_interface = ControllerManagerInterface(self.node) + self._io_status_controller_interface = IoStatusInterface(self.node) + self._configuration_controller_interface = ConfigurationInterface(self.node) + + def setUp(self): + self._dashboard_interface.start_robot() + time.sleep(1) + self.assertTrue(self._io_status_controller_interface.resend_robot_program().success) + self.tf_buffer = Buffer() + self.tf_listener = TransformListener(self.tf_buffer, self.node) + + def lookup_tcp_in_base(self, tf_prefix, timepoint): + trans = None + while not trans: + rclpy.spin_once(self.node) + try: + trans = self.tf_buffer.lookup_transform( + tf_prefix + "base", tf_prefix + "tool0", timepoint + ) + except TransformException: + pass + return trans + + def test_force_mode_controller(self, tf_prefix): + self.assertTrue( + self._controller_manager_interface.switch_controller( + strictness=SwitchController.Request.BEST_EFFORT, + activate_controllers=[ + "force_mode_controller", + ], + deactivate_controllers=[ + "scaled_joint_trajectory_controller", + "joint_trajectory_controller", + ], + ).ok + ) + self._force_mode_controller_interface = ForceModeInterface(self.node) + + # Create task frame for force mode + point = Point(x=0.8, y=0.8, z=0.8) + orientation = Quaternion(x=0.7071, y=0.0, z=0.0, w=0.7071) + task_frame_pose = Pose() + task_frame_pose.position = point + task_frame_pose.orientation = orientation + header = std_msgs.msg.Header(frame_id=tf_prefix + "base") + header.stamp.sec = int(time.time()) + 1 + header.stamp.nanosec = 0 + frame_stamp = PoseStamped() + frame_stamp.header = header + frame_stamp.pose = task_frame_pose + + # Create compliance vector (which axes should be force controlled) + compliance = [False, False, True, False, False, False] + + # Create Wrench message for force mode + wrench = Wrench() + wrench.force = Vector3(x=0.0, y=0.0, z=5.0) + wrench.torque = Vector3(x=0.0, y=0.0, z=0.0) + + # Specify interpretation of task frame (no transform) + type_spec = 2 + + # Specify max speeds and deviations of force mode + speed_limits = Twist() + speed_limits.linear = Vector3(x=0.0, y=0.0, z=1.0) + speed_limits.angular = Vector3(x=0.0, y=0.0, z=1.0) + deviation_limits = [1.0, 1.0, 1.0, 1.0, 1.0, 1.0] + + # specify damping and gain scaling + damping_factor = 0.1 + gain_scale = 0.8 + + trans_before = self.lookup_tcp_in_base(tf_prefix, rclpy.time.Time()) + + # Send request to controller + res = self._force_mode_controller_interface.start_force_mode( + task_frame=frame_stamp, + selection_vector_x=compliance[0], + selection_vector_y=compliance[1], + selection_vector_z=compliance[2], + selection_vector_rx=compliance[3], + selection_vector_ry=compliance[4], + selection_vector_rz=compliance[5], + wrench=wrench, + type=type_spec, + speed_limits=speed_limits, + deviation_limits=deviation_limits, + damping_factor=damping_factor, + gain_scaling=gain_scale, + ) + self.assertTrue(res.success) + + time.sleep(5.0) + + trans_after = self.lookup_tcp_in_base(tf_prefix, self.node.get_clock().now()) + + # task frame and wrench determines the expected motion + # In the example we used + # - a task frame rotated pi/2 deg around the base frame's x axis + # - a wrench with a positive z component for the force + # => we should expect a motion in negative y of the base frame + self.assertTrue(trans_after.transform.translation.y < trans_before.transform.translation.y) + self.assertAlmostEqual( + trans_after.transform.translation.x, + trans_before.transform.translation.x, + delta=0.001, + ) + self.assertAlmostEqual( + trans_after.transform.translation.z, + trans_before.transform.translation.z, + delta=0.001, + ) + self.assertAlmostEqual( + trans_after.transform.rotation.x, + trans_before.transform.rotation.x, + delta=0.01, + ) + self.assertAlmostEqual( + trans_after.transform.rotation.y, + trans_before.transform.rotation.y, + delta=0.01, + ) + self.assertAlmostEqual( + trans_after.transform.rotation.z, + trans_before.transform.rotation.z, + delta=0.01, + ) + self.assertAlmostEqual( + trans_after.transform.rotation.w, + trans_before.transform.rotation.w, + delta=0.01, + ) + + res = self._force_mode_controller_interface.stop_force_mode() + self.assertTrue(res.success) + + # Deactivate controller + self.assertTrue( + self._controller_manager_interface.switch_controller( + strictness=SwitchController.Request.STRICT, + deactivate_controllers=["force_mode_controller"], + ).ok + ) + + def test_illegal_force_mode_types(self, tf_prefix): + self.assertTrue( + self._controller_manager_interface.switch_controller( + strictness=SwitchController.Request.BEST_EFFORT, + activate_controllers=[ + "force_mode_controller", + ], + deactivate_controllers=[ + "scaled_joint_trajectory_controller", + "joint_trajectory_controller", + ], + ).ok + ) + self._force_mode_controller_interface = ForceModeInterface(self.node) + + # Create task frame for force mode + point = Point(x=0.0, y=0.0, z=0.0) + orientation = Quaternion(x=0.0, y=0.0, z=0.0, w=1.0) + task_frame_pose = Pose() + task_frame_pose.position = point + task_frame_pose.orientation = orientation + header = std_msgs.msg.Header(frame_id=tf_prefix + "base") + header.stamp.sec = int(time.time()) + 1 + header.stamp.nanosec = 0 + frame_stamp = PoseStamped() + frame_stamp.header = header + frame_stamp.pose = task_frame_pose + + res = self._force_mode_controller_interface.start_force_mode(task_frame=frame_stamp, type=0) + self.assertFalse(res.success) + res = self._force_mode_controller_interface.start_force_mode(task_frame=frame_stamp, type=4) + self.assertFalse(res.success) + res = self._force_mode_controller_interface.start_force_mode(task_frame=frame_stamp, type=1) + self.assertTrue(res.success) + res = self._force_mode_controller_interface.stop_force_mode() + res = self._force_mode_controller_interface.start_force_mode(task_frame=frame_stamp, type=2) + self.assertTrue(res.success) + res = self._force_mode_controller_interface.stop_force_mode() + res = self._force_mode_controller_interface.start_force_mode(task_frame=frame_stamp, type=3) + self.assertTrue(res.success) + res = self._force_mode_controller_interface.stop_force_mode() + + def test_illegal_task_frame(self, tf_prefix): + self.assertTrue( + self._controller_manager_interface.switch_controller( + strictness=SwitchController.Request.BEST_EFFORT, + activate_controllers=[ + "force_mode_controller", + ], + deactivate_controllers=[ + "scaled_joint_trajectory_controller", + "joint_trajectory_controller", + ], + ).ok + ) + self._force_mode_controller_interface = ForceModeInterface(self.node) + + # Create task frame for force mode + point = Point(x=0.0, y=0.0, z=0.0) + orientation = Quaternion(x=0.0, y=0.0, z=0.0, w=1.0) + task_frame_pose = Pose() + task_frame_pose.position = point + task_frame_pose.orientation = orientation + header = std_msgs.msg.Header(frame_id=tf_prefix + "base") + header.stamp.sec = int(time.time()) + 1 + header.stamp.nanosec = 0 + frame_stamp = PoseStamped() + frame_stamp.header = header + frame_stamp.pose = task_frame_pose + + # Illegal frame name produces error + header.frame_id = "nonexisting6t54" + res = self._force_mode_controller_interface.start_force_mode( + task_frame=frame_stamp, + ) + self.assertFalse(res.success) + header.frame_id = "base" + + # Illegal quaternion produces error + task_frame_pose.orientation = Quaternion(x=0.0, y=0.0, z=0.0, w=0.0) + res = self._force_mode_controller_interface.start_force_mode( + task_frame=frame_stamp, + ) + self.assertFalse(res.success) + + def test_start_force_mode_on_inactive_controller_fails(self, tf_prefix): + self.assertTrue( + self._controller_manager_interface.switch_controller( + strictness=SwitchController.Request.BEST_EFFORT, + activate_controllers=[], + deactivate_controllers=[ + "force_mode_controller", + "scaled_joint_trajectory_controller", + "joint_trajectory_controller", + ], + ).ok + ) + self._force_mode_controller_interface = ForceModeInterface(self.node) + + # Create task frame for force mode + point = Point(x=0.0, y=0.0, z=0.0) + orientation = Quaternion(x=0.0, y=0.0, z=0.0, w=1.0) + task_frame_pose = Pose() + task_frame_pose.position = point + task_frame_pose.orientation = orientation + header = std_msgs.msg.Header(frame_id=tf_prefix + "base") + header.stamp.sec = int(time.time()) + 1 + header.stamp.nanosec = 0 + frame_stamp = PoseStamped() + frame_stamp.header = header + frame_stamp.pose = task_frame_pose + + res = self._force_mode_controller_interface.start_force_mode( + task_frame=frame_stamp, + ) + self.assertFalse(res.success) + + def test_deactivating_controller_stops_force_mode(self, tf_prefix): + self.assertTrue( + self._controller_manager_interface.switch_controller( + strictness=SwitchController.Request.BEST_EFFORT, + activate_controllers=[ + "force_mode_controller", + ], + deactivate_controllers=[ + "scaled_joint_trajectory_controller", + "joint_trajectory_controller", + ], + ).ok + ) + self._force_mode_controller_interface = ForceModeInterface(self.node) + + # Create task frame for force mode + point = Point(x=0.0, y=0.0, z=0.0) + orientation = Quaternion(x=0.0, y=0.0, z=0.0, w=1.0) + task_frame_pose = Pose() + task_frame_pose.position = point + task_frame_pose.orientation = orientation + header = std_msgs.msg.Header(frame_id=tf_prefix + "base") + header.stamp.sec = int(time.time()) + 1 + header.stamp.nanosec = 0 + frame_stamp = PoseStamped() + frame_stamp.header = header + frame_stamp.pose = task_frame_pose + + # Create compliance vector (which axes should be force controlled) + compliance = [False, False, True, False, False, False] + + # Create Wrench message for force mode + wrench = Wrench() + wrench.force = Vector3(x=0.0, y=0.0, z=5.0) + wrench.torque = Vector3(x=0.0, y=0.0, z=0.0) + + # Specify interpretation of task frame (no transform) + type_spec = 2 + + # Specify max speeds and deviations of force mode + speed_limits = Twist() + speed_limits.linear = Vector3(x=0.0, y=0.0, z=1.0) + speed_limits.angular = Vector3(x=0.0, y=0.0, z=1.0) + deviation_limits = [1.0, 1.0, 1.0, 1.0, 1.0, 1.0] + + # specify damping and gain scaling + damping_factor = 0.1 + gain_scale = 0.8 + + res = self._force_mode_controller_interface.start_force_mode( + task_frame=frame_stamp, + selection_vector_x=compliance[0], + selection_vector_y=compliance[1], + selection_vector_z=compliance[2], + selection_vector_rx=compliance[3], + selection_vector_ry=compliance[4], + selection_vector_rz=compliance[5], + wrench=wrench, + type=type_spec, + speed_limits=speed_limits, + deviation_limits=deviation_limits, + damping_factor=damping_factor, + gain_scaling=gain_scale, + ) + self.assertTrue(res.success) + + time.sleep(0.5) + + self.assertTrue( + self._controller_manager_interface.switch_controller( + strictness=SwitchController.Request.BEST_EFFORT, + activate_controllers=[], + deactivate_controllers=[ + "force_mode_controller", + "scaled_joint_trajectory_controller", + "joint_trajectory_controller", + ], + ).ok + ) + self._force_mode_controller_interface = ForceModeInterface(self.node) + + time.sleep(0.5) + trans_before_wait = self.lookup_tcp_in_base(tf_prefix, self.node.get_clock().now()) + + # Make sure the robot didn't move anymore + time.sleep(0.5) + trans_after_wait = self.lookup_tcp_in_base(tf_prefix, self.node.get_clock().now()) + + self.assertAlmostEqual( + trans_before_wait.transform.translation.z, trans_after_wait.transform.translation.z + ) + + def test_params_out_of_range_fails(self, tf_prefix): + self.assertTrue( + self._controller_manager_interface.switch_controller( + strictness=SwitchController.Request.BEST_EFFORT, + activate_controllers=[ + "force_mode_controller", + ], + deactivate_controllers=[ + "scaled_joint_trajectory_controller", + "joint_trajectory_controller", + ], + ).ok + ) + self._force_mode_controller_interface = ForceModeInterface(self.node) + + # Create task frame for force mode + point = Point(x=0.0, y=0.0, z=0.0) + orientation = Quaternion(x=0.0, y=0.0, z=0.0, w=1.0) + task_frame_pose = Pose() + task_frame_pose.position = point + task_frame_pose.orientation = orientation + header = std_msgs.msg.Header(frame_id=tf_prefix + "base") + header.stamp.sec = int(time.time()) + 1 + header.stamp.nanosec = 0 + frame_stamp = PoseStamped() + frame_stamp.header = header + frame_stamp.pose = task_frame_pose + + res = self._force_mode_controller_interface.start_force_mode( + task_frame=frame_stamp, gain_scaling=-0.1 + ) + self.assertFalse(res.success) + + res = self._force_mode_controller_interface.start_force_mode( + task_frame=frame_stamp, gain_scaling=0.0 + ) + self.assertTrue(res.success) + res = self._force_mode_controller_interface.stop_force_mode() + self.assertTrue(res.success) + + res = self._force_mode_controller_interface.start_force_mode( + task_frame=frame_stamp, gain_scaling=2.0 + ) + self.assertTrue(res.success) + res = self._force_mode_controller_interface.stop_force_mode() + self.assertTrue(res.success) + + res = self._force_mode_controller_interface.start_force_mode( + task_frame=frame_stamp, gain_scaling=2.1 + ) + self.assertFalse(res.success) + + # damping factor + res = self._force_mode_controller_interface.start_force_mode( + task_frame=frame_stamp, damping_factor=-0.1 + ) + self.assertFalse(res.success) + + res = self._force_mode_controller_interface.start_force_mode( + task_frame=frame_stamp, damping_factor=0.0 + ) + self.assertTrue(res.success) + res = self._force_mode_controller_interface.stop_force_mode() + self.assertTrue(res.success) + + res = self._force_mode_controller_interface.start_force_mode( + task_frame=frame_stamp, damping_factor=1.0 + ) + self.assertTrue(res.success) + res = self._force_mode_controller_interface.stop_force_mode() + self.assertTrue(res.success) + + res = self._force_mode_controller_interface.start_force_mode( + task_frame=frame_stamp, damping_factor=1.1 + ) + self.assertFalse(res.success) diff --git a/ur_robot_driver/test/robot_driver.py b/ur_robot_driver/test/robot_driver.py index 6350ea78d..b0b2a4c84 100644 --- a/ur_robot_driver/test/robot_driver.py +++ b/ur_robot_driver/test/robot_driver.py @@ -52,19 +52,11 @@ IoStatusInterface, ConfigurationInterface, generate_driver_test_description, + ROBOT_JOINTS, ) TIMEOUT_EXECUTE_TRAJECTORY = 30 -ROBOT_JOINTS = [ - "shoulder_pan_joint", - "shoulder_lift_joint", - "elbow_joint", - "wrist_1_joint", - "wrist_2_joint", - "wrist_3_joint", -] - @pytest.mark.launch_test @launch_testing.parametrize( diff --git a/ur_robot_driver/test/test_common.py b/ur_robot_driver/test/test_common.py index a8a1a8ffb..927542676 100644 --- a/ur_robot_driver/test/test_common.py +++ b/ur_robot_driver/test/test_common.py @@ -57,7 +57,7 @@ IsProgramRunning, Load, ) -from ur_msgs.srv import SetIO, GetRobotSoftwareVersion +from ur_msgs.srv import SetIO, GetRobotSoftwareVersion, SetForceMode TIMEOUT_WAIT_SERVICE = 10 TIMEOUT_WAIT_SERVICE_INITIAL = 120 # If we download the docker image simultaneously to the tests, it can take quite some time until the dashboard server is reachable and usable. @@ -274,6 +274,15 @@ class ConfigurationInterface( pass +class ForceModeInterface( + _ServiceInterface, + namespace="/force_mode_controller", + initial_services={}, + services={"start_force_mode": SetForceMode, "stop_force_mode": Trigger}, +): + pass + + def _declare_launch_arguments(): declared_arguments = [] From 92b1bea47a939a266378f3095c1a87a6176f8537 Mon Sep 17 00:00:00 2001 From: "mergify[bot]" <37929162+mergify[bot]@users.noreply.github.com> Date: Wed, 18 Dec 2024 11:53:41 +0100 Subject: [PATCH 024/133] Freedrive Controller (#1114) (#1211) This controller allows activating freedrive mode by continuously publishing an a topic --- Co-authored-by: Vincenzo Di Pentima --- ur_controllers/CMakeLists.txt | 17 + ur_controllers/controller_plugins.xml | 5 + ur_controllers/doc/index.rst | 38 ++ .../freedrive_mode_controller.hpp | 130 +++++++ .../src/freedrive_mode_controller.cpp | 337 ++++++++++++++++++ .../freedrive_mode_controller_parameters.yaml | 17 + .../freedrive_mode_controller_params.yaml | 5 + .../test_load_freedrive_mode_controller.cpp | 61 ++++ ur_robot_driver/config/ur_controllers.yaml | 7 + ur_robot_driver/doc/usage/controllers.rst | 127 +++++++ .../ur_robot_driver/hardware_interface.hpp | 14 +- ur_robot_driver/launch/ur10.launch.py | 1 + ur_robot_driver/launch/ur10e.launch.py | 1 + ur_robot_driver/launch/ur16e.launch.py | 1 + ur_robot_driver/launch/ur20.launch.py | 1 + ur_robot_driver/launch/ur3.launch.py | 1 + ur_robot_driver/launch/ur30.launch.py | 1 + ur_robot_driver/launch/ur3e.launch.py | 1 + ur_robot_driver/launch/ur5.launch.py | 1 + ur_robot_driver/launch/ur5e.launch.py | 1 + ur_robot_driver/launch/ur_control.launch.py | 2 + ur_robot_driver/src/hardware_interface.cpp | 130 +++++-- .../integration_test_controller_switch.py | 28 ++ ur_robot_driver/urdf/ur.ros2_control.xacro | 303 ++++++++++++++++ 24 files changed, 1206 insertions(+), 24 deletions(-) create mode 100644 ur_controllers/include/ur_controllers/freedrive_mode_controller.hpp create mode 100644 ur_controllers/src/freedrive_mode_controller.cpp create mode 100644 ur_controllers/src/freedrive_mode_controller_parameters.yaml create mode 100644 ur_controllers/test/freedrive_mode_controller_params.yaml create mode 100644 ur_controllers/test/test_load_freedrive_mode_controller.cpp create mode 100644 ur_robot_driver/doc/usage/controllers.rst create mode 100644 ur_robot_driver/urdf/ur.ros2_control.xacro diff --git a/ur_controllers/CMakeLists.txt b/ur_controllers/CMakeLists.txt index f310a32e8..79a89acbd 100644 --- a/ur_controllers/CMakeLists.txt +++ b/ur_controllers/CMakeLists.txt @@ -71,6 +71,11 @@ generate_parameter_library( src/scaled_joint_trajectory_controller_parameters.yaml ) +generate_parameter_library( + freedrive_mode_controller_parameters + src/freedrive_mode_controller_parameters.yaml +) + generate_parameter_library( passthrough_trajectory_controller_parameters src/passthrough_trajectory_controller_parameters.yaml @@ -85,6 +90,7 @@ add_library(${PROJECT_NAME} SHARED src/force_mode_controller.cpp src/scaled_joint_trajectory_controller.cpp src/speed_scaling_state_broadcaster.cpp + src/freedrive_mode_controller.cpp src/gpio_controller.cpp src/passthrough_trajectory_controller.cpp src/ur_configuration_controller.cpp) @@ -97,6 +103,7 @@ target_link_libraries(${PROJECT_NAME} gpio_controller_parameters speed_scaling_state_broadcaster_parameters scaled_joint_trajectory_controller_parameters + freedrive_mode_controller_parameters passthrough_trajectory_controller_parameters ur_configuration_controller_parameters ) @@ -153,6 +160,16 @@ if(BUILD_TESTING) controller_manager ros2_control_test_assets ) + ament_add_gmock(test_load_freedrive_mode_controller + test/test_load_freedrive_mode_controller.cpp + ) + target_link_libraries(test_load_freedrive_mode_controller + ${PROJECT_NAME} + ) + ament_target_dependencies(test_load_freedrive_mode_controller + controller_manager + ros2_control_test_assets + ) endif() ament_package() diff --git a/ur_controllers/controller_plugins.xml b/ur_controllers/controller_plugins.xml index f3365a26d..ec15809a1 100644 --- a/ur_controllers/controller_plugins.xml +++ b/ur_controllers/controller_plugins.xml @@ -19,6 +19,11 @@ Controller to use UR's force_mode. + + + This controller handles the activation of the freedrive mode. + + This controller forwards a joint-based trajectory to the robot controller for interpolation. diff --git a/ur_controllers/doc/index.rst b/ur_controllers/doc/index.rst index 38211d083..f03632d53 100644 --- a/ur_controllers/doc/index.rst +++ b/ur_controllers/doc/index.rst @@ -340,3 +340,41 @@ damping_factor gain_scaling Force mode gain scaling factor. Scales the gain in force mode. scaling parameter is in range [0;2], default is 0.5. A value larger than 1 can make force mode unstable, e.g. in case of collisions or pushing against hard surfaces. + +.. _freedrive_mode_controller: + +ur_controllers/FreedriveModeController +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +This controller activates the robot's *Freedrive Mode*, allowing to manually move the robot' joints. +This controller can't be combined with any other motion controller. + +Parameters +"""""""""" + ++----------------------+--------+---------------+---------------------------------------------------------------------------------------+ +| Parameter name | Type | Default value | Description | +| | | | | ++----------------------+--------+---------------+---------------------------------------------------------------------------------------+ +| ``tf_prefix`` | string | | Urdf prefix of the corresponding arm | ++----------------------+--------+---------------+---------------------------------------------------------------------------------------+ +| ``inactive_timeout`` | int | 1 | Time interval (in seconds) of message inactivity after which freedrive is deactivated | ++----------------------+--------+---------------+---------------------------------------------------------------------------------------+ + +Usage +""""" + +The controller provides the ``~/enable_freedrive_mode`` topic of type ``[std_msgs/msg/Bool]`` for handling activation and deactivation: + +* to start and keep freedrive active, you'll have to frequently publish a ``True`` msg on the indicated topic. + If no further messages are received by the controller within the ``inactive_timeout`` seconds, + freedrive mode will be deactivated. Hence, it is recommended to publish a ``True`` message at least every + ``inactive_timeout/2`` seconds. + + .. code-block:: + + ros2 topic pub --rate 2 /freedrive_mode_controller/enable_freedrive_mode std_msgs/msg/Bool "{data: true}" + +* to deactivate freedrive mode is enough to publish a ``False`` msg on the indicated topic or + to deactivate the controller or to stop publishing ``True`` on the enable topic and wait for the + controller timeout. diff --git a/ur_controllers/include/ur_controllers/freedrive_mode_controller.hpp b/ur_controllers/include/ur_controllers/freedrive_mode_controller.hpp new file mode 100644 index 000000000..04e90805e --- /dev/null +++ b/ur_controllers/include/ur_controllers/freedrive_mode_controller.hpp @@ -0,0 +1,130 @@ +// Copyright 2024, FZI Forschungszentrum Informatik, Created on behalf of 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 Vincenzo Di Pentima dipentima@fzi.de + * \date 2024-09-26 + */ +//---------------------------------------------------------------------- +#ifndef UR_CONTROLLERS__FREEDRIVE_MODE_CONTROLLER_HPP_ +#define UR_CONTROLLERS__FREEDRIVE_MODE_CONTROLLER_HPP_ + +#pragma once + +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include "std_msgs/msg/bool.hpp" + +#include "freedrive_mode_controller_parameters.hpp" + +namespace ur_controllers +{ +enum CommandInterfaces +{ + FREEDRIVE_MODE_ASYNC_SUCCESS = 0u, + FREEDRIVE_MODE_ENABLE = 1, + FREEDRIVE_MODE_ABORT = 2, +}; + +using namespace std::chrono_literals; // NOLINT + +class FreedriveModeController : public controller_interface::ControllerInterface +{ +public: + controller_interface::InterfaceConfiguration command_interface_configuration() const override; + + controller_interface::InterfaceConfiguration state_interface_configuration() const override; + + // Change the input for the update function + controller_interface::return_type update(const rclcpp::Time& time, const rclcpp::Duration& period) override; + + CallbackReturn on_configure(const rclcpp_lifecycle::State& previous_state) override; + + CallbackReturn on_activate(const rclcpp_lifecycle::State& previous_state) override; + + CallbackReturn on_cleanup(const rclcpp_lifecycle::State& previous_state) override; + + CallbackReturn on_deactivate(const rclcpp_lifecycle::State& previous_state) override; + + CallbackReturn on_init() override; + +private: + // Command interfaces: optional is used only to avoid adding reference initialization + std::optional> async_success_command_interface_; + std::optional> enable_command_interface_; + std::optional> abort_command_interface_; + + std::shared_ptr> enable_freedrive_mode_sub_; + + rclcpp::TimerBase::SharedPtr freedrive_sub_timer_; ///< Timer to check for timeout on input + mutable std::chrono::seconds timeout_interval_; + void freedrive_cmd_callback(const std_msgs::msg::Bool::SharedPtr msg); + + std::shared_ptr freedrive_param_listener_; + freedrive_mode_controller::Params freedrive_params_; + + std::atomic freedrive_active_; + std::atomic change_requested_; + std::atomic async_state_; + std::atomic first_log_; + std::atomic timer_started_; + + void start_timer(); + void timeout_callback(); + + std::thread logging_thread_; + std::atomic logging_thread_running_; + std::atomic logging_requested_; + std::condition_variable logging_condition_; + std::mutex log_mutex_; + void log_task(); + void start_logging_thread(); + void stop_logging_thread(); + + static constexpr double ASYNC_WAITING = 2.0; + /** + * @brief wait until a command interface isn't in state ASYNC_WAITING anymore or until the parameter maximum_retries + * have been reached + */ + bool waitForAsyncCommand(std::function get_value); +}; +} // namespace ur_controllers +#endif // UR_CONTROLLERS__PASSTHROUGH_TRAJECTORY_CONTROLLER_HPP_ diff --git a/ur_controllers/src/freedrive_mode_controller.cpp b/ur_controllers/src/freedrive_mode_controller.cpp new file mode 100644 index 000000000..e1ae016e5 --- /dev/null +++ b/ur_controllers/src/freedrive_mode_controller.cpp @@ -0,0 +1,337 @@ + +// Copyright 2024, FZI Forschungszentrum Informatik, Created on behalf of 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 Vincenzo Di Pentima dipentima@fzi.de + * \date 2024-09-16 + */ +//---------------------------------------------------------------------- +#include +#include +#include +#include +#include + +namespace ur_controllers +{ +controller_interface::CallbackReturn FreedriveModeController::on_init() +{ + try { + // Create the parameter listener and get the parameters + freedrive_param_listener_ = std::make_shared(get_node()); + freedrive_params_ = freedrive_param_listener_->get_params(); + } catch (const std::exception& e) { + fprintf(stderr, "Exception thrown during init stage with message: %s \n", e.what()); + return CallbackReturn::ERROR; + } + + return controller_interface::CallbackReturn::SUCCESS; +} + +controller_interface::InterfaceConfiguration FreedriveModeController::command_interface_configuration() const +{ + controller_interface::InterfaceConfiguration config; + config.type = controller_interface::interface_configuration_type::INDIVIDUAL; + + const std::string tf_prefix = freedrive_params_.tf_prefix; + timeout_interval_ = std::chrono::seconds(freedrive_params_.inactive_timeout); + + // Get the command interfaces needed for freedrive mode from the hardware interface + config.names.emplace_back(tf_prefix + "freedrive_mode/async_success"); + config.names.emplace_back(tf_prefix + "freedrive_mode/enable"); + config.names.emplace_back(tf_prefix + "freedrive_mode/abort"); + + return config; +} + +controller_interface::InterfaceConfiguration +ur_controllers::FreedriveModeController::state_interface_configuration() const +{ + controller_interface::InterfaceConfiguration config; + config.type = controller_interface::interface_configuration_type::NONE; + + return config; +} + +controller_interface::CallbackReturn +ur_controllers::FreedriveModeController::on_configure(const rclcpp_lifecycle::State& previous_state) +{ + // Subscriber definition + enable_freedrive_mode_sub_ = get_node()->create_subscription( + "~/enable_freedrive_mode", 10, + std::bind(&FreedriveModeController::freedrive_cmd_callback, this, std::placeholders::_1)); + + timer_started_ = false; + + const auto logger = get_node()->get_logger(); + + if (!freedrive_param_listener_) { + RCLCPP_ERROR(get_node()->get_logger(), "Error encountered during configuration"); + return controller_interface::CallbackReturn::ERROR; + } + + // Update the dynamic map parameters + freedrive_param_listener_->refresh_dynamic_parameters(); + + // Get parameters from the listener in case they were updated + freedrive_params_ = freedrive_param_listener_->get_params(); + + start_logging_thread(); + + return ControllerInterface::on_configure(previous_state); +} + +controller_interface::CallbackReturn +ur_controllers::FreedriveModeController::on_activate(const rclcpp_lifecycle::State& state) +{ + change_requested_ = false; + freedrive_active_ = false; + async_state_ = std::numeric_limits::quiet_NaN(); + + first_log_ = false; + logging_thread_running_ = true; + logging_requested_ = false; + + { + const std::string interface_name = freedrive_params_.tf_prefix + "freedrive_mode/" + "async_success"; + auto it = std::find_if(command_interfaces_.begin(), command_interfaces_.end(), + [&](auto& interface) { return (interface.get_name() == interface_name); }); + if (it != command_interfaces_.end()) { + async_success_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 interface_name = freedrive_params_.tf_prefix + "freedrive_mode/" + "enable"; + auto it = std::find_if(command_interfaces_.begin(), command_interfaces_.end(), + [&](auto& interface) { return (interface.get_name() == interface_name); }); + if (it != command_interfaces_.end()) { + enable_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 interface_name = freedrive_params_.tf_prefix + "freedrive_mode/" + "abort"; + auto it = std::find_if(command_interfaces_.begin(), command_interfaces_.end(), + [&](auto& interface) { return (interface.get_name() == interface_name); }); + if (it != command_interfaces_.end()) { + abort_command_interface_ = *it; + abort_command_interface_->get().set_value(0.0); + } else { + RCLCPP_ERROR(get_node()->get_logger(), "Did not find '%s' in command interfaces.", interface_name.c_str()); + return controller_interface::CallbackReturn::ERROR; + } + } + + return ControllerInterface::on_activate(state); +} + +controller_interface::CallbackReturn +ur_controllers::FreedriveModeController::on_cleanup(const rclcpp_lifecycle::State& /*previous_state*/) +{ + abort_command_interface_->get().set_value(1.0); + + stop_logging_thread(); + + return CallbackReturn::SUCCESS; +} + +controller_interface::CallbackReturn +ur_controllers::FreedriveModeController::on_deactivate(const rclcpp_lifecycle::State&) +{ + freedrive_active_ = false; + + freedrive_sub_timer_.reset(); + timer_started_ = false; + + return CallbackReturn::SUCCESS; +} + +controller_interface::return_type ur_controllers::FreedriveModeController::update(const rclcpp::Time& /*time*/, + const rclcpp::Duration& /*period*/) +{ + async_state_ = async_success_command_interface_->get().get_value(); + + if (change_requested_) { + if (freedrive_active_) { + // Check if the freedrive mode has been aborted from the hardware interface. E.g. the robot was stopped on the + // teach pendant. + if (!std::isnan(abort_command_interface_->get().get_value()) && + abort_command_interface_->get().get_value() == 1.0) { + RCLCPP_INFO(get_node()->get_logger(), "Freedrive mode aborted by hardware, aborting request."); + freedrive_active_ = false; + return controller_interface::return_type::OK; + } else { + RCLCPP_INFO(get_node()->get_logger(), "Received command to start Freedrive Mode."); + + // Set command interface to enable + enable_command_interface_->get().set_value(1.0); + + async_success_command_interface_->get().set_value(ASYNC_WAITING); + async_state_ = ASYNC_WAITING; + } + + } else { + RCLCPP_INFO(get_node()->get_logger(), "Received command to stop Freedrive Mode."); + + abort_command_interface_->get().set_value(1.0); + + async_success_command_interface_->get().set_value(ASYNC_WAITING); + async_state_ = ASYNC_WAITING; + } + first_log_ = true; + change_requested_ = false; + } + + if ((async_state_ == 1.0) && (first_log_)) { + first_log_ = false; + logging_requested_ = true; + + // Notify logging thread + logging_condition_.notify_one(); + } + return controller_interface::return_type::OK; +} + +void FreedriveModeController::freedrive_cmd_callback(const std_msgs::msg::Bool::SharedPtr msg) +{ + // Process the freedrive_mode command. + if (get_node()->get_current_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) { + if (msg->data) { + if ((!freedrive_active_) && (!change_requested_)) { + freedrive_active_ = true; + change_requested_ = true; + start_timer(); + } + } else { + if ((freedrive_active_) && (!change_requested_)) { + freedrive_active_ = false; + change_requested_ = true; + } + } + } + + if (freedrive_sub_timer_) { + freedrive_sub_timer_->reset(); + } +} + +void FreedriveModeController::start_timer() +{ + if (!timer_started_) { + // Start the timer only after the first message is received + freedrive_sub_timer_ = + get_node()->create_wall_timer(timeout_interval_, std::bind(&FreedriveModeController::timeout_callback, this)); + timer_started_ = true; + + RCLCPP_INFO(get_node()->get_logger(), "Timer started after receiving first command."); + } +} + +void FreedriveModeController::timeout_callback() +{ + if (timer_started_ && freedrive_active_) { + RCLCPP_INFO(get_node()->get_logger(), "Freedrive mode will be deactivated since no new message received."); + + freedrive_active_ = false; + change_requested_ = true; + } + + timer_started_ = false; +} + +void FreedriveModeController::start_logging_thread() +{ + if (!logging_thread_running_) { + logging_thread_running_ = true; + logging_thread_ = std::thread(&FreedriveModeController::log_task, this); + } +} + +void FreedriveModeController::stop_logging_thread() +{ + logging_thread_running_ = false; + if (logging_thread_.joinable()) { + logging_thread_.join(); + } +} + +void FreedriveModeController::log_task() +{ + while (logging_thread_running_) { + std::unique_lock lock(log_mutex_); + + auto condition = [this] { return !logging_thread_running_ || logging_requested_; }; + + // Wait for the condition + logging_condition_.wait(lock, condition); + + if (!logging_thread_running_) + break; + + if (freedrive_active_) { + RCLCPP_INFO(get_node()->get_logger(), "Freedrive mode has been enabled successfully."); + } else { + RCLCPP_INFO(get_node()->get_logger(), "Freedrive mode has been disabled successfully."); + } + + // Reset to log only once + logging_requested_ = false; + } +} + +bool FreedriveModeController::waitForAsyncCommand(std::function get_value) +{ + const auto maximum_retries = freedrive_params_.check_io_successful_retries; + int retries = 0; + while (get_value() == ASYNC_WAITING) { + std::this_thread::sleep_for(std::chrono::milliseconds(50)); + retries++; + + if (retries > maximum_retries) + return false; + } + return true; +} +} // namespace ur_controllers + +#include "pluginlib/class_list_macros.hpp" + +PLUGINLIB_EXPORT_CLASS(ur_controllers::FreedriveModeController, controller_interface::ControllerInterface) diff --git a/ur_controllers/src/freedrive_mode_controller_parameters.yaml b/ur_controllers/src/freedrive_mode_controller_parameters.yaml new file mode 100644 index 000000000..e50b42260 --- /dev/null +++ b/ur_controllers/src/freedrive_mode_controller_parameters.yaml @@ -0,0 +1,17 @@ +--- +freedrive_mode_controller: + tf_prefix: { + type: string, + default_value: "", + description: "Urdf prefix of the corresponding arm" + } + inactive_timeout: { + type: int, + default_value: 1, + description: "Time interval (in seconds) of message inactivity after which freedrive is deactivated" + } + check_io_successful_retries: { + type: int, + default_value: 10, + description: "Amount of retries for checking if setting force_mode was successful" + } diff --git a/ur_controllers/test/freedrive_mode_controller_params.yaml b/ur_controllers/test/freedrive_mode_controller_params.yaml new file mode 100644 index 000000000..417d385c0 --- /dev/null +++ b/ur_controllers/test/freedrive_mode_controller_params.yaml @@ -0,0 +1,5 @@ +--- +freedrive_mode_controller: + ros__parameters: + tf_prefix: "" + inactive_timeout: 10 diff --git a/ur_controllers/test/test_load_freedrive_mode_controller.cpp b/ur_controllers/test/test_load_freedrive_mode_controller.cpp new file mode 100644 index 000000000..0fd65c41c --- /dev/null +++ b/ur_controllers/test/test_load_freedrive_mode_controller.cpp @@ -0,0 +1,61 @@ +// 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. + +#include +#include "controller_manager/controller_manager.hpp" +#include "rclcpp/executor.hpp" +#include "rclcpp/executors/single_threaded_executor.hpp" +#include "rclcpp/utilities.hpp" +#include "ros2_control_test_assets/descriptions.hpp" + +TEST(TestLoadFreedriveModeController, load_controller) +{ + std::shared_ptr executor = std::make_shared(); + + controller_manager::ControllerManager cm( + std::make_unique(ros2_control_test_assets::minimal_robot_urdf), executor, + "test_controller_manager"); + + const std::string test_file_path = std::string{ TEST_FILES_DIRECTORY } + "/freedrive_mode_controller_params.yaml"; + cm.set_parameter({ "test_freedrive_mode_controller.params_file", test_file_path }); + + cm.set_parameter({ "test_freedrive_mode_controller.type", "ur_controllers/FreedriveModeController" }); + + ASSERT_NE(cm.load_controller("test_freedrive_mode_controller"), nullptr); +} + +int main(int argc, char* argv[]) +{ + ::testing::InitGoogleMock(&argc, argv); + rclcpp::init(argc, argv); + + int result = RUN_ALL_TESTS(); + rclcpp::shutdown(); + + return result; +} diff --git a/ur_robot_driver/config/ur_controllers.yaml b/ur_robot_driver/config/ur_controllers.yaml index b93bfb146..5e996bd24 100644 --- a/ur_robot_driver/config/ur_controllers.yaml +++ b/ur_robot_driver/config/ur_controllers.yaml @@ -27,6 +27,9 @@ controller_manager: force_mode_controller: type: ur_controllers/ForceModeController + freedrive_mode_controller: + type: ur_controllers/FreedriveModeController + passthrough_trajectory_controller: type: ur_controllers/PassthroughTrajectoryController @@ -162,6 +165,10 @@ force_mode_controller: ros__parameters: tf_prefix: "$(var tf_prefix)" +freedrive_mode_controller: + ros__parameters: + tf_prefix: "$(var tf_prefix)" + tcp_pose_broadcaster: ros__parameters: frame_id: $(var tf_prefix)base diff --git a/ur_robot_driver/doc/usage/controllers.rst b/ur_robot_driver/doc/usage/controllers.rst new file mode 100644 index 000000000..67c3a4926 --- /dev/null +++ b/ur_robot_driver/doc/usage/controllers.rst @@ -0,0 +1,127 @@ +Controllers +=========== + +This help page describes the different controllers available for the ``ur_robot_driver``. This +should help users finding the right controller for their specific use case. + +Where are controllers defined? +------------------------------ + +Controllers are defined in the ``config/ur_controllers.yaml`` file. + +How do controllers get loaded and started? +------------------------------------------ + +As this driver uses `ros2_control `_ all controllers are managed by the +controller_manager. During startup, a default set of running controllers is loaded and started, +another set is loaded in stopped mode. Stopped controllers won't be usable right away, but they +need to be started individually. + +Controllers that are actually writing to some command interfaces (e.g. joint positions) will claim +those interfaces. Only one controller claiming a certain interface can be active at one point. + +Controllers can be switched either through the controller_manager's service calls, through the +`rqt_controller_manager `_ gui or through the ``ros2 control`` verb from the package ``ros-${ROS_DISTRO}-ros2controlcli`` package. + +For example, to switch from the default ``scaled_joint_trajectory_controller`` to the +``forward_position_controller`` you can call + +.. code-block:: console + + $ ros2 control switch_controllers --deactivate scaled_joint_trajectory_controller \ + --activate forward_position_controller + [INFO 2024-09-23 20:32:04.373] [_ros2cli_1207798]: waiting for service /controller_manager/switch_controller to become available... + Successfully switched controllers + +Read-only broadcasters +---------------------- + +These broadcasters are read-only. They read states from the robot and publish them on a ROS topic. +As they are read-only, they don't claim any resources and can be combined freely. By default, they +are all started and running. Those controllers do not require the robot to have the +external_control script running. + +joint_state_broadcaster +^^^^^^^^^^^^^^^^^^^^^^^ + +Type: `joint_state_broadcaster/JointStateBroadcaster `_ + +Publishes all joints' positions, velocities, and motor currents as ``sensor_msgs/JointState`` on the ``joint_states`` topic. + +.. note:: + + The effort field contains the currents reported by the joints and not the actual efforts in a + physical sense. + +speed_scaling_state_broadcaster +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +Type: :ref:`ur_controllers/SpeedScalingStateBroadcaster ` + +This broadcaster publishes the current actual execution speed as reported by the robot. Values are +floating points between 0 and 1. + +force_torque_sensor_broadcaster +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +Type: `force_torque_sensor_broadcaster/ForceTorqueSensorBroadcaster `_ + +Publishes the robot's wrench as reported from the controller. + +Commanding controllers +---------------------- + +The commanding controllers control the robot's motions. Those controllers can't be combined +arbitrarily, as they will claim hardware resources. Only one controller can claim one hardware +resource at a time. + +scaled_joint_trajectory_controller (Default motion controller) +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +Type: :ref:`ur_controllers/ScaledJointTrajectoryController ` + +Scaled version of the +`joint_trajectory_controller +`_. +It uses the robot's speed scaling information and thereby the safety compliance features, like pause on safeguard stop. In addition, it also makes it possible to adjust execution speed using the speed slider on the teach pendant or set the program in pause and restart it again. +See it's linked documentation for details. + +io_and_status_controller +^^^^^^^^^^^^^^^^^^^^^^^^ + +Type: :ref:`ur_controllers/GPIOController ` + +Allows setting I/O ports, controlling some UR-specific functionality and publishes status information about the robot. + +forward_velocity_controller +^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +Type: `velocity_controllers/JointGroupVelocityController `_ + +Allows setting target joint positions directly. The robot tries to reach the target position as +fast as possible. The user is therefore responsible for sending commands that are achievable. This +controller is particularly useful when doing servoing such as ``moveit_servo``. + +forward_position_controller +^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +Type: `position_controllers/JointGroupPositionController `_ + +Allows setting target joint velocities directly. The user is responsible for sending commands that +are achievable. This controller is particularly useful when doing servoing such as +``moveit_servo``. + +force_mode_controller +^^^^^^^^^^^^^^^^^^^^^ + +Type: :ref:`ur_controllers/ForceModeController ` + +Allows utilizing the robot's builtin *Force Mode*. + +freedrive_mode_controller +^^^^^^^^^^^^^^^^^^^^^^^^^ + +Type: :ref:`ur_controllers/FreedriveModeController ` + +Allows utilizing the robot's *Freedrive mode*, making it possible to manually move the robot's +joints. diff --git a/ur_robot_driver/include/ur_robot_driver/hardware_interface.hpp b/ur_robot_driver/include/ur_robot_driver/hardware_interface.hpp index 1f665a1b2..4d525e92a 100644 --- a/ur_robot_driver/include/ur_robot_driver/hardware_interface.hpp +++ b/ur_robot_driver/include/ur_robot_driver/hardware_interface.hpp @@ -79,7 +79,8 @@ enum StoppingInterface STOP_POSITION, STOP_VELOCITY, STOP_PASSTHROUGH, - STOP_FORCE_MODE + STOP_FORCE_MODE, + STOP_FREEDRIVE, }; // We define our own quaternion to use it as a buffer, since we need to pass pointers to the state @@ -228,6 +229,13 @@ class URPositionHardwareInterface : public hardware_interface::SystemInterface double get_robot_software_version_bugfix_; double get_robot_software_version_build_; + // Freedrive mode controller interface values + bool freedrive_activated_; + bool freedrive_mode_controller_running_; + double freedrive_mode_async_success_; + double freedrive_mode_enable_; + double freedrive_mode_abort_; + // Passthrough trajectory controller interface values double passthrough_trajectory_transfer_state_; double passthrough_trajectory_abort_; @@ -236,6 +244,7 @@ class URPositionHardwareInterface : public hardware_interface::SystemInterface urcl::vector6d_t passthrough_trajectory_velocities_; urcl::vector6d_t passthrough_trajectory_accelerations_; double passthrough_trajectory_time_from_start_; + // payload stuff urcl::vector3d_t payload_center_of_gravity_; double payload_mass_; @@ -293,8 +302,9 @@ class URPositionHardwareInterface : public hardware_interface::SystemInterface urcl::RobotReceiveTimeout receive_timeout_ = urcl::RobotReceiveTimeout::millisec(20); - const std::string FORCE_MODE_GPIO = "force_mode"; const std::string PASSTHROUGH_GPIO = "trajectory_passthrough"; + const std::string FORCE_MODE_GPIO = "force_mode"; + const std::string FREEDRIVE_MODE_GPIO = "freedrive_mode"; }; } // namespace ur_robot_driver diff --git a/ur_robot_driver/launch/ur10.launch.py b/ur_robot_driver/launch/ur10.launch.py index 31c4fc82c..db4c7df91 100644 --- a/ur_robot_driver/launch/ur10.launch.py +++ b/ur_robot_driver/launch/ur10.launch.py @@ -69,6 +69,7 @@ def generate_launch_description(): "joint_trajectory_controller", "forward_velocity_controller", "forward_position_controller", + "freedrive_mode_controller", "passthrough_trajectory_controller", ], ) diff --git a/ur_robot_driver/launch/ur10e.launch.py b/ur_robot_driver/launch/ur10e.launch.py index 7009abfd2..c2c4dd92c 100644 --- a/ur_robot_driver/launch/ur10e.launch.py +++ b/ur_robot_driver/launch/ur10e.launch.py @@ -69,6 +69,7 @@ def generate_launch_description(): "joint_trajectory_controller", "forward_velocity_controller", "forward_position_controller", + "freedrive_mode_controller", "passthrough_trajectory_controller", ], ) diff --git a/ur_robot_driver/launch/ur16e.launch.py b/ur_robot_driver/launch/ur16e.launch.py index 02a56fd59..1e90e0388 100644 --- a/ur_robot_driver/launch/ur16e.launch.py +++ b/ur_robot_driver/launch/ur16e.launch.py @@ -69,6 +69,7 @@ def generate_launch_description(): "joint_trajectory_controller", "forward_velocity_controller", "forward_position_controller", + "freedrive_mode_controller", "passthrough_trajectory_controller", ], ) diff --git a/ur_robot_driver/launch/ur20.launch.py b/ur_robot_driver/launch/ur20.launch.py index 3f9ad3a91..aaf26af18 100644 --- a/ur_robot_driver/launch/ur20.launch.py +++ b/ur_robot_driver/launch/ur20.launch.py @@ -69,6 +69,7 @@ def generate_launch_description(): "joint_trajectory_controller", "forward_velocity_controller", "forward_position_controller", + "freedrive_mode_controller", "passthrough_trajectory_controller", ], ) diff --git a/ur_robot_driver/launch/ur3.launch.py b/ur_robot_driver/launch/ur3.launch.py index feaa54c56..ce2b1ae33 100644 --- a/ur_robot_driver/launch/ur3.launch.py +++ b/ur_robot_driver/launch/ur3.launch.py @@ -69,6 +69,7 @@ def generate_launch_description(): "joint_trajectory_controller", "forward_velocity_controller", "forward_position_controller", + "freedrive_mode_controller", "passthrough_trajectory_controller", ], ) diff --git a/ur_robot_driver/launch/ur30.launch.py b/ur_robot_driver/launch/ur30.launch.py index 3822f5e2c..e5d38e584 100644 --- a/ur_robot_driver/launch/ur30.launch.py +++ b/ur_robot_driver/launch/ur30.launch.py @@ -69,6 +69,7 @@ def generate_launch_description(): "joint_trajectory_controller", "forward_velocity_controller", "forward_position_controller", + "freedrive_mode_controller", "passthrough_trajectory_controller", ], ) diff --git a/ur_robot_driver/launch/ur3e.launch.py b/ur_robot_driver/launch/ur3e.launch.py index cbb3b2a5e..8c23de139 100644 --- a/ur_robot_driver/launch/ur3e.launch.py +++ b/ur_robot_driver/launch/ur3e.launch.py @@ -69,6 +69,7 @@ def generate_launch_description(): "joint_trajectory_controller", "forward_velocity_controller", "forward_position_controller", + "freedrive_mode_controller", "passthrough_trajectory_controller", ], ) diff --git a/ur_robot_driver/launch/ur5.launch.py b/ur_robot_driver/launch/ur5.launch.py index 6281bbf90..bc157faae 100644 --- a/ur_robot_driver/launch/ur5.launch.py +++ b/ur_robot_driver/launch/ur5.launch.py @@ -69,6 +69,7 @@ def generate_launch_description(): "joint_trajectory_controller", "forward_velocity_controller", "forward_position_controller", + "freedrive_mode_controller", "passthrough_trajectory_controller", ], ) diff --git a/ur_robot_driver/launch/ur5e.launch.py b/ur_robot_driver/launch/ur5e.launch.py index 372bf2ef3..0fbdeb62c 100644 --- a/ur_robot_driver/launch/ur5e.launch.py +++ b/ur_robot_driver/launch/ur5e.launch.py @@ -69,6 +69,7 @@ def generate_launch_description(): "joint_trajectory_controller", "forward_velocity_controller", "forward_position_controller", + "freedrive_mode_controller", "passthrough_trajectory_controller", ], ) diff --git a/ur_robot_driver/launch/ur_control.launch.py b/ur_robot_driver/launch/ur_control.launch.py index 02fd59a0c..1d1ab86bc 100644 --- a/ur_robot_driver/launch/ur_control.launch.py +++ b/ur_robot_driver/launch/ur_control.launch.py @@ -349,6 +349,7 @@ def controller_spawner(controllers, active=True): "forward_position_controller", "force_mode_controller", "passthrough_trajectory_controller", + "freedrive_mode_controller", ] if activate_joint_controller.perform(context) == "true": controllers_active.append(initial_joint_controller.perform(context)) @@ -500,6 +501,7 @@ def generate_launch_description(): "joint_trajectory_controller", "forward_velocity_controller", "forward_position_controller", + "freedrive_mode_controller", "passthrough_trajectory_controller", ], description="Initially loaded robot controller.", diff --git a/ur_robot_driver/src/hardware_interface.cpp b/ur_robot_driver/src/hardware_interface.cpp index 9cb3350ec..157310e96 100644 --- a/ur_robot_driver/src/hardware_interface.cpp +++ b/ur_robot_driver/src/hardware_interface.cpp @@ -83,6 +83,7 @@ URPositionHardwareInterface::on_init(const hardware_interface::HardwareInfo& sys urcl_velocity_commands_ = { { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 } }; position_controller_running_ = false; velocity_controller_running_ = false; + freedrive_mode_controller_running_ = false; passthrough_trajectory_controller_running_ = false; runtime_state_ = static_cast(rtde::RUNTIME_STATE::STOPPED); pausing_state_ = PausingState::RUNNING; @@ -92,6 +93,7 @@ URPositionHardwareInterface::on_init(const hardware_interface::HardwareInfo& sys initialized_ = false; async_thread_shutdown_ = false; system_interface_initialized_ = 0.0; + freedrive_mode_abort_ = 0.0; passthrough_trajectory_transfer_state_ = 0.0; passthrough_trajectory_abort_ = 0.0; trajectory_joint_positions_.clear(); @@ -365,6 +367,15 @@ std::vector URPositionHardwareInterface::e command_interfaces.emplace_back(hardware_interface::CommandInterface( tf_prefix + "zero_ftsensor", "zero_ftsensor_async_success", &zero_ftsensor_async_success_)); + command_interfaces.emplace_back(hardware_interface::CommandInterface(tf_prefix + FREEDRIVE_MODE_GPIO, "async_success", + &freedrive_mode_async_success_)); + + command_interfaces.emplace_back( + hardware_interface::CommandInterface(tf_prefix + FREEDRIVE_MODE_GPIO, "enable", &freedrive_mode_enable_)); + + command_interfaces.emplace_back( + hardware_interface::CommandInterface(tf_prefix + FREEDRIVE_MODE_GPIO, "abort", &freedrive_mode_abort_)); + command_interfaces.emplace_back(hardware_interface::CommandInterface(tf_prefix + PASSTHROUGH_GPIO, "transfer_state", &passthrough_trajectory_transfer_state_)); @@ -716,6 +727,8 @@ hardware_interface::return_type URPositionHardwareInterface::read(const rclcpp:: zero_ftsensor_cmd_ = NO_NEW_CMD_; hand_back_control_cmd_ = NO_NEW_CMD_; force_mode_disable_cmd_ = NO_NEW_CMD_; + freedrive_mode_abort_ = NO_NEW_CMD_; + freedrive_mode_enable_ = NO_NEW_CMD_; initialized_ = true; } @@ -744,6 +757,9 @@ hardware_interface::return_type URPositionHardwareInterface::write(const rclcpp: } else if (velocity_controller_running_) { ur_driver_->writeJointCommand(urcl_velocity_commands_, urcl::comm::ControlMode::MODE_SPEEDJ, receive_timeout_); + } else if (freedrive_mode_controller_running_ && freedrive_activated_) { + ur_driver_->writeFreedriveControlMessage(urcl::control::FreedriveControlMessage::FREEDRIVE_NOOP); + } else if (passthrough_trajectory_controller_running_) { ur_driver_->writeTrajectoryControlMessage(urcl::control::TrajectoryControlMessage::TRAJECTORY_NOOP); check_passthrough_trajectory_controller(); @@ -859,6 +875,23 @@ void URPositionHardwareInterface::checkAsyncIO() zero_ftsensor_async_success_ = ur_driver_->zeroFTSensor(); zero_ftsensor_cmd_ = NO_NEW_CMD_; } + + if (!std::isnan(freedrive_mode_enable_) && ur_driver_ != nullptr) { + RCLCPP_INFO(rclcpp::get_logger("URPosistionHardwareInterface"), "Starting freedrive mode."); + freedrive_mode_async_success_ = + ur_driver_->writeFreedriveControlMessage(urcl::control::FreedriveControlMessage::FREEDRIVE_START); + freedrive_mode_enable_ = NO_NEW_CMD_; + freedrive_activated_ = true; + } + + if (!std::isnan(freedrive_mode_abort_) && freedrive_mode_abort_ == 1.0 && freedrive_activated_ && + ur_driver_ != nullptr) { + RCLCPP_INFO(rclcpp::get_logger("URPosistionHardwareInterface"), "Stopping freedrive mode."); + freedrive_mode_async_success_ = + ur_driver_->writeFreedriveControlMessage(urcl::control::FreedriveControlMessage::FREEDRIVE_STOP); + freedrive_activated_ = false; + freedrive_mode_abort_ = NO_NEW_CMD_; + } } void URPositionHardwareInterface::updateNonDoubleValues() @@ -943,6 +976,9 @@ hardware_interface::return_type URPositionHardwareInterface::prepare_command_mod if (passthrough_trajectory_controller_running_) { control_modes[i].push_back(PASSTHROUGH_GPIO); } + if (freedrive_mode_controller_running_) { + control_modes[i].push_back(FREEDRIVE_MODE_GPIO); + } } if (!std::all_of(start_modes_.begin() + 1, start_modes_.end(), @@ -994,6 +1030,14 @@ hardware_interface::return_type URPositionHardwareInterface::prepare_command_mod return hardware_interface::return_type::ERROR; } start_modes_[i].push_back(PASSTHROUGH_GPIO); + } else if (key == tf_prefix + FREEDRIVE_MODE_GPIO + "/async_success") { + if (std::any_of(start_modes_[i].begin(), start_modes_[i].end(), [&](const std::string& item) { + return item == hardware_interface::HW_IF_POSITION || item == hardware_interface::HW_IF_VELOCITY || + item == PASSTHROUGH_GPIO || item == FORCE_MODE_GPIO; + })) { + return hardware_interface::return_type::ERROR; + } + start_modes_[i].push_back(FREEDRIVE_MODE_GPIO); } } } @@ -1028,38 +1072,67 @@ hardware_interface::return_type URPositionHardwareInterface::prepare_command_mod [&](const std::string& item) { return item == PASSTHROUGH_GPIO; }), control_modes[i].end()); } + if (key == tf_prefix + FREEDRIVE_MODE_GPIO + "/async_success") { + stop_modes_[i].push_back(StoppingInterface::STOP_FREEDRIVE); + control_modes[i].erase(std::remove_if(control_modes[i].begin(), control_modes[i].end(), + [&](const std::string& item) { return item == FREEDRIVE_MODE_GPIO; }), + control_modes[i].end()); + } } } // Do not start conflicting controllers + // Passthrough controller requested to start if (std::any_of(start_modes_[0].begin(), start_modes_[0].end(), [this](auto& item) { return (item == PASSTHROUGH_GPIO); }) && (std::any_of(start_modes_[0].begin(), start_modes_[0].end(), - [](auto& item) { - return (item == hardware_interface::HW_IF_VELOCITY || item == hardware_interface::HW_IF_POSITION); + [this](auto& item) { + return (item == hardware_interface::HW_IF_VELOCITY || item == hardware_interface::HW_IF_POSITION || + item == FREEDRIVE_MODE_GPIO); }) || std::any_of(control_modes[0].begin(), control_modes[0].end(), [this](auto& item) { - return (item == hardware_interface::HW_IF_VELOCITY || item == hardware_interface::HW_IF_POSITION); + return (item == hardware_interface::HW_IF_VELOCITY || item == hardware_interface::HW_IF_POSITION || + item == FREEDRIVE_MODE_GPIO); }))) { - RCLCPP_ERROR(rclcpp::get_logger("URPositionHardwareInterface"), "Attempting to start passthrough_trajectory " - "control while there is either position or " - "velocity mode is running."); + RCLCPP_ERROR(rclcpp::get_logger("URPosistionHardwareInterface"), "Attempting to start passthrough_trajectory " + "control while there is either position or " + "velocity or freedrive mode running."); ret_val = hardware_interface::return_type::ERROR; } + // Force mode requested to start if (std::any_of(start_modes_[0].begin(), start_modes_[0].end(), [this](auto& item) { return (item == FORCE_MODE_GPIO); }) && (std::any_of(start_modes_[0].begin(), start_modes_[0].end(), - [](auto& item) { - return (item == hardware_interface::HW_IF_VELOCITY || item == hardware_interface::HW_IF_POSITION); + [this](auto& item) { + return (item == hardware_interface::HW_IF_VELOCITY || item == hardware_interface::HW_IF_POSITION || + item == FREEDRIVE_MODE_GPIO); }) || std::any_of(control_modes[0].begin(), control_modes[0].end(), [this](auto& item) { return (item == hardware_interface::HW_IF_VELOCITY || item == hardware_interface::HW_IF_POSITION || - item == FORCE_MODE_GPIO); + item == FORCE_MODE_GPIO || item == FREEDRIVE_MODE_GPIO); }))) { - RCLCPP_ERROR(rclcpp::get_logger("URPositionHardwareInterface"), "Attempting to start force mode control while " - "there is either position or " - "velocity mode running."); + RCLCPP_ERROR(rclcpp::get_logger("URPosistionHardwareInterface"), "Attempting to start force mode control while " + "there is either position or " + "velocity mode running."); + ret_val = hardware_interface::return_type::ERROR; + } + + // Freedrive mode requested to start + if (std::any_of(start_modes_[0].begin(), start_modes_[0].end(), + [this](auto& item) { return (item == FREEDRIVE_MODE_GPIO); }) && + (std::any_of(start_modes_[0].begin(), start_modes_[0].end(), + [this](auto& item) { + return (item == hardware_interface::HW_IF_VELOCITY || item == hardware_interface::HW_IF_POSITION || + item == PASSTHROUGH_GPIO || item == FORCE_MODE_GPIO); + }) || + std::any_of(control_modes[0].begin(), control_modes[0].end(), [this](auto& item) { + return (item == hardware_interface::HW_IF_VELOCITY || item == hardware_interface::HW_IF_POSITION || + item == PASSTHROUGH_GPIO || item == FORCE_MODE_GPIO); + }))) { + RCLCPP_ERROR(rclcpp::get_logger("URPosistionHardwareInterface"), "Attempting to start force mode control while " + "there is either position or " + "velocity mode running."); ret_val = hardware_interface::return_type::ERROR; } @@ -1069,15 +1142,16 @@ hardware_interface::return_type URPositionHardwareInterface::prepare_command_mod (std::any_of(start_modes_[0].begin(), start_modes_[0].end(), [this](auto& item) { return (item == hardware_interface::HW_IF_VELOCITY || item == PASSTHROUGH_GPIO || - item == FORCE_MODE_GPIO); + item == FORCE_MODE_GPIO || item == FREEDRIVE_MODE_GPIO); }) || std::any_of(control_modes[0].begin(), control_modes[0].end(), [this](auto& item) { return (item == hardware_interface::HW_IF_VELOCITY || item == hardware_interface::HW_IF_POSITION || - item == PASSTHROUGH_GPIO || item == FORCE_MODE_GPIO); + item == PASSTHROUGH_GPIO || item == FORCE_MODE_GPIO || item == FREEDRIVE_MODE_GPIO); }))) { - RCLCPP_ERROR(rclcpp::get_logger("URPositionHardwareInterface"), "Attempting to start position control while there " - "is either trajectory passthrough or " - "velocity mode or force_mode running."); + RCLCPP_ERROR(rclcpp::get_logger("URPosistionHardwareInterface"), "Attempting to start position control while there " + "is either trajectory passthrough or " + "velocity mode or force_mode or freedrive mode " + "running."); ret_val = hardware_interface::return_type::ERROR; } @@ -1087,15 +1161,16 @@ hardware_interface::return_type URPositionHardwareInterface::prepare_command_mod (std::any_of(start_modes_[0].begin(), start_modes_[0].end(), [this](auto& item) { return (item == hardware_interface::HW_IF_POSITION || item == PASSTHROUGH_GPIO || - item == FORCE_MODE_GPIO); + item == FORCE_MODE_GPIO || item == FREEDRIVE_MODE_GPIO); }) || std::any_of(control_modes[0].begin(), control_modes[0].end(), [this](auto& item) { return (item == hardware_interface::HW_IF_VELOCITY || item == hardware_interface::HW_IF_POSITION || - item == PASSTHROUGH_GPIO || item == FORCE_MODE_GPIO); + item == PASSTHROUGH_GPIO || item == FORCE_MODE_GPIO || item == FREEDRIVE_MODE_GPIO); }))) { - RCLCPP_ERROR(rclcpp::get_logger("URPositionHardwareInterface"), "Attempting to start velosity control while there " - "is either trajectory passthrough or " - "position mode or force_mode running."); + RCLCPP_ERROR(rclcpp::get_logger("URPosistionHardwareInterface"), "Attempting to start velocity control while there " + "is either trajectory passthrough or " + "position mode or force_mode or freedrive mode " + "running."); ret_val = hardware_interface::return_type::ERROR; } @@ -1127,6 +1202,11 @@ hardware_interface::return_type URPositionHardwareInterface::perform_command_mod trajectory_joint_positions_.clear(); trajectory_joint_accelerations_.clear(); trajectory_joint_velocities_.clear(); + } else if (stop_modes_.size() != 0 && std::find(stop_modes_[0].begin(), stop_modes_[0].end(), + StoppingInterface::STOP_FREEDRIVE) != stop_modes_[0].end()) { + freedrive_mode_controller_running_ = false; + freedrive_activated_ = false; + freedrive_mode_abort_ = 1.0; } if (start_modes_.size() != 0 && std::find(start_modes_[0].begin(), start_modes_[0].end(), @@ -1151,6 +1231,12 @@ hardware_interface::return_type URPositionHardwareInterface::perform_command_mod position_controller_running_ = false; passthrough_trajectory_controller_running_ = true; passthrough_trajectory_abort_ = 0.0; + } else if (start_modes_[0].size() != 0 && + std::find(start_modes_[0].begin(), start_modes_[0].end(), FREEDRIVE_MODE_GPIO) != start_modes_[0].end()) { + velocity_controller_running_ = false; + position_controller_running_ = false; + freedrive_mode_controller_running_ = true; + freedrive_activated_ = false; } start_modes_.clear(); diff --git a/ur_robot_driver/test/integration_test_controller_switch.py b/ur_robot_driver/test/integration_test_controller_switch.py index bdc9a0eca..a9356ffc7 100644 --- a/ur_robot_driver/test/integration_test_controller_switch.py +++ b/ur_robot_driver/test/integration_test_controller_switch.py @@ -96,6 +96,7 @@ def test_activating_multiple_controllers_same_interface_fails(self): "forward_velocity_controller", "passthrough_trajectory_controller", "force_mode_controller", + "freedrive_mode_controller", ], ).ok ) @@ -132,6 +133,7 @@ def test_activating_multiple_controllers_different_interface_fails(self): "forward_velocity_controller", "force_mode_controller", "passthrough_trajectory_controller", + "freedrive_mode_controller", ], ).ok ) @@ -180,6 +182,15 @@ def test_activating_multiple_controllers_different_interface_fails(self): ], ).ok ) + self.assertFalse( + self._controller_manager_interface.switch_controller( + strictness=SwitchController.Request.STRICT, + activate_controllers=[ + "scaled_joint_trajectory_controller", + "freedrive_mode_controller", + ], + ).ok + ) def test_activating_controller_with_running_position_controller_fails(self): # Having a position-based controller active, no other controller should be able to @@ -195,6 +206,7 @@ def test_activating_controller_with_running_position_controller_fails(self): "forward_position_controller", "forward_velocity_controller", "force_mode_controller", + "freedrive_mode_controller", "passthrough_trajectory_controller", ], ).ok @@ -215,6 +227,14 @@ def test_activating_controller_with_running_position_controller_fails(self): ], ).ok ) + self.assertFalse( + self._controller_manager_interface.switch_controller( + strictness=SwitchController.Request.STRICT, + activate_controllers=[ + "freedrive_mode_controller", + ], + ).ok + ) self.assertFalse( self._controller_manager_interface.switch_controller( strictness=SwitchController.Request.STRICT, @@ -289,6 +309,14 @@ def test_activating_controller_with_running_passthrough_trajectory_controller_fa ], ).ok ) + self.assertFalse( + self._controller_manager_interface.switch_controller( + strictness=SwitchController.Request.STRICT, + activate_controllers=[ + "freedrive_mode_controller", + ], + ).ok + ) # Stop the controller again self.assertTrue( self._controller_manager_interface.switch_controller( diff --git a/ur_robot_driver/urdf/ur.ros2_control.xacro b/ur_robot_driver/urdf/ur.ros2_control.xacro new file mode 100644 index 000000000..91e73a888 --- /dev/null +++ b/ur_robot_driver/urdf/ur.ros2_control.xacro @@ -0,0 +1,303 @@ + + + + + + + + + + + + + + + + + + + + mock_components/GenericSystem + ${mock_sensor_commands} + 0.0 + true + + + ur_robot_driver/URPositionHardwareInterface + ${robot_ip} + ${script_filename} + ${output_recipe_filename} + ${input_recipe_filename} + ${headless_mode} + ${reverse_port} + ${script_sender_port} + ${reverse_ip} + ${script_command_port} + ${trajectory_port} + ${tf_prefix} + ${non_blocking_read} + 2000 + 0.03 + ${use_tool_communication} + ${kinematics_hash} + ${tool_voltage} + ${tool_parity} + ${tool_baud_rate} + ${tool_stop_bits} + ${tool_rx_idle_chars} + ${tool_tx_idle_chars} + ${tool_device_name} + ${tool_tcp_port} + ${robot_receive_timeout} + + + + + + + + + + + 1.0 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + From aae112959d28998fb700b1e4e7874814f2f99f56 Mon Sep 17 00:00:00 2001 From: Felix Exner Date: Wed, 18 Dec 2024 11:02:24 +0000 Subject: [PATCH 025/133] Prepare changelogs for upcoming release --- ur/CHANGELOG.rst | 5 +++++ ur_bringup/CHANGELOG.rst | 6 ++++++ ur_calibration/CHANGELOG.rst | 6 ++++++ ur_controllers/CHANGELOG.rst | 9 +++++++++ ur_dashboard_msgs/CHANGELOG.rst | 5 +++++ ur_moveit_config/CHANGELOG.rst | 5 +++++ ur_robot_driver/CHANGELOG.rst | 9 +++++++++ 7 files changed, 45 insertions(+) diff --git a/ur/CHANGELOG.rst b/ur/CHANGELOG.rst index 6297de836..ece06b0f6 100644 --- a/ur/CHANGELOG.rst +++ b/ur/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package ur ^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Update package maintainers (backport of `#1203 `_) +* Contributors: mergify[bot] + 2.2.16 (2024-10-28) ------------------- diff --git a/ur_bringup/CHANGELOG.rst b/ur_bringup/CHANGELOG.rst index 9008fa5ef..8b22975c3 100644 --- a/ur_bringup/CHANGELOG.rst +++ b/ur_bringup/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package ur_bringup ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Update package maintainers (backport of `#1203 `_) +* Forward trajectory controller (backport of `#944 `_) +* Contributors: mergify[bot] + 2.2.16 (2024-10-28) ------------------- diff --git a/ur_calibration/CHANGELOG.rst b/ur_calibration/CHANGELOG.rst index 85047f1e9..80bd3895d 100644 --- a/ur_calibration/CHANGELOG.rst +++ b/ur_calibration/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package ur_calibration ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Update package maintainers (backport of `#1203 `_) +* Initialize segments in constructor of DHRobot in calibration.hpp (backport of `#1197 `_) +* Contributors: mergify[bot] + 2.2.16 (2024-10-28) ------------------- diff --git a/ur_controllers/CHANGELOG.rst b/ur_controllers/CHANGELOG.rst index 03c1c984f..cb4166020 100644 --- a/ur_controllers/CHANGELOG.rst +++ b/ur_controllers/CHANGELOG.rst @@ -2,6 +2,15 @@ Changelog for package ur_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Freedrive Controller (`#1114 `_) (`#1211 `_) +* Add force mode controller (`#1049 `_) (`#1193 `_) +* Update package maintainers (backport of `#1203 `_) +* Forward trajectory controller (backport of `#944 `_) +* [SJTC] Make scaling interface optional (`#1145 `_) (`#1172 `_) +* Contributors: mergify[bot] + 2.2.16 (2024-10-28) ------------------- * Allow setting the analog output domain when setting an analog output (backport of `#1123 `_) diff --git a/ur_dashboard_msgs/CHANGELOG.rst b/ur_dashboard_msgs/CHANGELOG.rst index 6e47280fe..8db749ab0 100644 --- a/ur_dashboard_msgs/CHANGELOG.rst +++ b/ur_dashboard_msgs/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package ur_dashboard_msgs ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Update package maintainers (backport of `#1203 `_) +* Contributors: mergify[bot] + 2.2.16 (2024-10-28) ------------------- diff --git a/ur_moveit_config/CHANGELOG.rst b/ur_moveit_config/CHANGELOG.rst index e5add7ed8..0bcc5333b 100644 --- a/ur_moveit_config/CHANGELOG.rst +++ b/ur_moveit_config/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package ur_moveit_config ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Update package maintainers (backport of `#1203 `_) +* Contributors: mergify[bot] + 2.2.16 (2024-10-28) ------------------- * Properly handle use_sim_time (`#1146 `_) (`#1159 `_) diff --git a/ur_robot_driver/CHANGELOG.rst b/ur_robot_driver/CHANGELOG.rst index 1ec669fd4..8355708c0 100644 --- a/ur_robot_driver/CHANGELOG.rst +++ b/ur_robot_driver/CHANGELOG.rst @@ -1,3 +1,12 @@ +Forthcoming +----------- +* Freedrive Controller (`#1114 `_) (`#1211 `_) +* Add force mode controller (`#1049 `_) (`#1193 `_) +* Update package maintainers (backport of `#1203 `_) +* Forward trajectory controller (backport of `#944 `_) +* Use pose_broadcaster to publish the TCP pose (backport of `#1108 `_) +* Contributors: mergify[bot] + 2.2.16 (2024-10-28) ------------------- * Allow setting the analog output domain when setting an analog output (backport of `#1123 `_) From b7fe050659fc69e26661bc7758ccdb0ee3116b31 Mon Sep 17 00:00:00 2001 From: Felix Exner Date: Wed, 18 Dec 2024 11:02:46 +0000 Subject: [PATCH 026/133] 2.5.0 --- ur/CHANGELOG.rst | 4 ++-- ur/package.xml | 2 +- ur_bringup/CHANGELOG.rst | 4 ++-- ur_bringup/package.xml | 2 +- ur_calibration/CHANGELOG.rst | 4 ++-- ur_calibration/package.xml | 2 +- ur_controllers/CHANGELOG.rst | 4 ++-- ur_controllers/package.xml | 2 +- ur_dashboard_msgs/CHANGELOG.rst | 4 ++-- ur_dashboard_msgs/package.xml | 2 +- ur_moveit_config/CHANGELOG.rst | 4 ++-- ur_moveit_config/package.xml | 2 +- ur_robot_driver/CHANGELOG.rst | 4 ++-- ur_robot_driver/package.xml | 2 +- 14 files changed, 21 insertions(+), 21 deletions(-) diff --git a/ur/CHANGELOG.rst b/ur/CHANGELOG.rst index ece06b0f6..71f7b2eb9 100644 --- a/ur/CHANGELOG.rst +++ b/ur/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package ur ^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.5.0 (2024-12-18) +------------------ * Update package maintainers (backport of `#1203 `_) * Contributors: mergify[bot] diff --git a/ur/package.xml b/ur/package.xml index b37aef93b..5c1d94ef1 100644 --- a/ur/package.xml +++ b/ur/package.xml @@ -2,7 +2,7 @@ ur - 2.2.16 + 2.5.0 Metapackage for universal robots Felix Exner diff --git a/ur_bringup/CHANGELOG.rst b/ur_bringup/CHANGELOG.rst index 8b22975c3..c6f940531 100644 --- a/ur_bringup/CHANGELOG.rst +++ b/ur_bringup/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package ur_bringup ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.5.0 (2024-12-18) +------------------ * Update package maintainers (backport of `#1203 `_) * Forward trajectory controller (backport of `#944 `_) * Contributors: mergify[bot] diff --git a/ur_bringup/package.xml b/ur_bringup/package.xml index 4f23de068..a62cbb65f 100644 --- a/ur_bringup/package.xml +++ b/ur_bringup/package.xml @@ -2,7 +2,7 @@ ur_bringup - 2.2.16 + 2.5.0 Launch file and run-time configurations, e.g. controllers. Felix Exner diff --git a/ur_calibration/CHANGELOG.rst b/ur_calibration/CHANGELOG.rst index 80bd3895d..00a70c780 100644 --- a/ur_calibration/CHANGELOG.rst +++ b/ur_calibration/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package ur_calibration ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.5.0 (2024-12-18) +------------------ * Update package maintainers (backport of `#1203 `_) * Initialize segments in constructor of DHRobot in calibration.hpp (backport of `#1197 `_) * Contributors: mergify[bot] diff --git a/ur_calibration/package.xml b/ur_calibration/package.xml index 425b503d3..d55c42c9e 100644 --- a/ur_calibration/package.xml +++ b/ur_calibration/package.xml @@ -1,7 +1,7 @@ ur_calibration - 2.2.16 + 2.5.0 Package for extracting the factory calibration from a UR robot and change it such that it can be used by ur_description to gain a correct URDF Felix Exner diff --git a/ur_controllers/CHANGELOG.rst b/ur_controllers/CHANGELOG.rst index cb4166020..b96a25072 100644 --- a/ur_controllers/CHANGELOG.rst +++ b/ur_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package ur_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.5.0 (2024-12-18) +------------------ * Freedrive Controller (`#1114 `_) (`#1211 `_) * Add force mode controller (`#1049 `_) (`#1193 `_) * Update package maintainers (backport of `#1203 `_) diff --git a/ur_controllers/package.xml b/ur_controllers/package.xml index b12cd0845..c9b9d0487 100644 --- a/ur_controllers/package.xml +++ b/ur_controllers/package.xml @@ -2,7 +2,7 @@ ur_controllers - 2.2.16 + 2.5.0 Provides controllers that use the speed scaling interface of Universal Robots. Felix Exner diff --git a/ur_dashboard_msgs/CHANGELOG.rst b/ur_dashboard_msgs/CHANGELOG.rst index 8db749ab0..32c2f9eeb 100644 --- a/ur_dashboard_msgs/CHANGELOG.rst +++ b/ur_dashboard_msgs/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package ur_dashboard_msgs ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.5.0 (2024-12-18) +------------------ * Update package maintainers (backport of `#1203 `_) * Contributors: mergify[bot] diff --git a/ur_dashboard_msgs/package.xml b/ur_dashboard_msgs/package.xml index 286ed8486..448a4ba10 100644 --- a/ur_dashboard_msgs/package.xml +++ b/ur_dashboard_msgs/package.xml @@ -2,7 +2,7 @@ ur_dashboard_msgs - 2.2.16 + 2.5.0 Messages around the UR Dashboard server. Felix Exner diff --git a/ur_moveit_config/CHANGELOG.rst b/ur_moveit_config/CHANGELOG.rst index 0bcc5333b..2128429f7 100644 --- a/ur_moveit_config/CHANGELOG.rst +++ b/ur_moveit_config/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package ur_moveit_config ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.5.0 (2024-12-18) +------------------ * Update package maintainers (backport of `#1203 `_) * Contributors: mergify[bot] diff --git a/ur_moveit_config/package.xml b/ur_moveit_config/package.xml index 15f9ef071..726e20744 100644 --- a/ur_moveit_config/package.xml +++ b/ur_moveit_config/package.xml @@ -2,7 +2,7 @@ ur_moveit_config - 2.2.16 + 2.5.0 An example package with MoveIt2 configurations for UR robots. diff --git a/ur_robot_driver/CHANGELOG.rst b/ur_robot_driver/CHANGELOG.rst index 8355708c0..5b2f77cb5 100644 --- a/ur_robot_driver/CHANGELOG.rst +++ b/ur_robot_driver/CHANGELOG.rst @@ -1,5 +1,5 @@ -Forthcoming ------------ +2.5.0 (2024-12-18) +------------------ * Freedrive Controller (`#1114 `_) (`#1211 `_) * Add force mode controller (`#1049 `_) (`#1193 `_) * Update package maintainers (backport of `#1203 `_) diff --git a/ur_robot_driver/package.xml b/ur_robot_driver/package.xml index 8b0577b00..da4aab2ed 100644 --- a/ur_robot_driver/package.xml +++ b/ur_robot_driver/package.xml @@ -2,7 +2,7 @@ ur_robot_driver - 2.2.16 + 2.5.0 The new driver for Universal Robots UR3, UR5 and UR10 robots with CB3 controllers and the e-series. Felix Exner From d66c4b4633bdaaea2b4692be0ea4f049e22aa3a4 Mon Sep 17 00:00:00 2001 From: "mergify[bot]" <37929162+mergify[bot]@users.noreply.github.com> Date: Sat, 21 Dec 2024 22:15:10 +0100 Subject: [PATCH 027/133] Add missing test dependencies for ur_controllers (backport of #1215) --- ur_controllers/package.xml | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/ur_controllers/package.xml b/ur_controllers/package.xml index c9b9d0487..c2573435e 100644 --- a/ur_controllers/package.xml +++ b/ur_controllers/package.xml @@ -25,6 +25,7 @@ angles controller_interface geometry_msgs + hardware_interface joint_trajectory_controller lifecycle_msgs pluginlib @@ -41,8 +42,9 @@ trajectory_msgs action_msgs + controller_manager hardware_interface_testing - + ros2_control_test_assets ament_cmake From ed365c46a01e27c8a4ef20ece0da9cb5d39dfcfd Mon Sep 17 00:00:00 2001 From: Felix Exner Date: Sat, 21 Dec 2024 22:16:03 +0100 Subject: [PATCH 028/133] Update changelogs for upcoming release --- ur/CHANGELOG.rst | 3 +++ ur_bringup/CHANGELOG.rst | 3 +++ ur_calibration/CHANGELOG.rst | 3 +++ ur_controllers/CHANGELOG.rst | 5 +++++ ur_dashboard_msgs/CHANGELOG.rst | 3 +++ ur_moveit_config/CHANGELOG.rst | 3 +++ ur_robot_driver/CHANGELOG.rst | 3 +++ 7 files changed, 23 insertions(+) diff --git a/ur/CHANGELOG.rst b/ur/CHANGELOG.rst index 71f7b2eb9..772919b51 100644 --- a/ur/CHANGELOG.rst +++ b/ur/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package ur ^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 2.5.0 (2024-12-18) ------------------ * Update package maintainers (backport of `#1203 `_) diff --git a/ur_bringup/CHANGELOG.rst b/ur_bringup/CHANGELOG.rst index c6f940531..d31fbe26e 100644 --- a/ur_bringup/CHANGELOG.rst +++ b/ur_bringup/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package ur_bringup ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 2.5.0 (2024-12-18) ------------------ * Update package maintainers (backport of `#1203 `_) diff --git a/ur_calibration/CHANGELOG.rst b/ur_calibration/CHANGELOG.rst index 00a70c780..996ced8b1 100644 --- a/ur_calibration/CHANGELOG.rst +++ b/ur_calibration/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package ur_calibration ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 2.5.0 (2024-12-18) ------------------ * Update package maintainers (backport of `#1203 `_) diff --git a/ur_controllers/CHANGELOG.rst b/ur_controllers/CHANGELOG.rst index b96a25072..d5dfb5128 100644 --- a/ur_controllers/CHANGELOG.rst +++ b/ur_controllers/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package ur_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Add missing test dependencies for ur_controllers (backport of `#1215 `_) +* Contributors: mergify[bot] + 2.5.0 (2024-12-18) ------------------ * Freedrive Controller (`#1114 `_) (`#1211 `_) diff --git a/ur_dashboard_msgs/CHANGELOG.rst b/ur_dashboard_msgs/CHANGELOG.rst index 32c2f9eeb..bf076a629 100644 --- a/ur_dashboard_msgs/CHANGELOG.rst +++ b/ur_dashboard_msgs/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package ur_dashboard_msgs ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 2.5.0 (2024-12-18) ------------------ * Update package maintainers (backport of `#1203 `_) diff --git a/ur_moveit_config/CHANGELOG.rst b/ur_moveit_config/CHANGELOG.rst index 2128429f7..85276745c 100644 --- a/ur_moveit_config/CHANGELOG.rst +++ b/ur_moveit_config/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package ur_moveit_config ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 2.5.0 (2024-12-18) ------------------ * Update package maintainers (backport of `#1203 `_) diff --git a/ur_robot_driver/CHANGELOG.rst b/ur_robot_driver/CHANGELOG.rst index 5b2f77cb5..1a5e2b558 100644 --- a/ur_robot_driver/CHANGELOG.rst +++ b/ur_robot_driver/CHANGELOG.rst @@ -1,3 +1,6 @@ +Forthcoming +----------- + 2.5.0 (2024-12-18) ------------------ * Freedrive Controller (`#1114 `_) (`#1211 `_) From 05386d938596f8427631f5d066f38e307921c56a Mon Sep 17 00:00:00 2001 From: Felix Exner Date: Sat, 21 Dec 2024 22:16:22 +0100 Subject: [PATCH 029/133] 2.5.1 --- ur/CHANGELOG.rst | 4 ++-- ur/package.xml | 2 +- ur_bringup/CHANGELOG.rst | 4 ++-- ur_bringup/package.xml | 2 +- ur_calibration/CHANGELOG.rst | 4 ++-- ur_calibration/package.xml | 2 +- ur_controllers/CHANGELOG.rst | 4 ++-- ur_controllers/package.xml | 2 +- ur_dashboard_msgs/CHANGELOG.rst | 4 ++-- ur_dashboard_msgs/package.xml | 2 +- ur_moveit_config/CHANGELOG.rst | 4 ++-- ur_moveit_config/package.xml | 2 +- ur_robot_driver/CHANGELOG.rst | 4 ++-- ur_robot_driver/package.xml | 2 +- 14 files changed, 21 insertions(+), 21 deletions(-) diff --git a/ur/CHANGELOG.rst b/ur/CHANGELOG.rst index 772919b51..d7e2eb858 100644 --- a/ur/CHANGELOG.rst +++ b/ur/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package ur ^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.5.1 (2024-12-21) +------------------ 2.5.0 (2024-12-18) ------------------ diff --git a/ur/package.xml b/ur/package.xml index 5c1d94ef1..4c104bed2 100644 --- a/ur/package.xml +++ b/ur/package.xml @@ -2,7 +2,7 @@ ur - 2.5.0 + 2.5.1 Metapackage for universal robots Felix Exner diff --git a/ur_bringup/CHANGELOG.rst b/ur_bringup/CHANGELOG.rst index d31fbe26e..3c02b7ab0 100644 --- a/ur_bringup/CHANGELOG.rst +++ b/ur_bringup/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package ur_bringup ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.5.1 (2024-12-21) +------------------ 2.5.0 (2024-12-18) ------------------ diff --git a/ur_bringup/package.xml b/ur_bringup/package.xml index a62cbb65f..ede082a14 100644 --- a/ur_bringup/package.xml +++ b/ur_bringup/package.xml @@ -2,7 +2,7 @@ ur_bringup - 2.5.0 + 2.5.1 Launch file and run-time configurations, e.g. controllers. Felix Exner diff --git a/ur_calibration/CHANGELOG.rst b/ur_calibration/CHANGELOG.rst index 996ced8b1..31db24a59 100644 --- a/ur_calibration/CHANGELOG.rst +++ b/ur_calibration/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package ur_calibration ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.5.1 (2024-12-21) +------------------ 2.5.0 (2024-12-18) ------------------ diff --git a/ur_calibration/package.xml b/ur_calibration/package.xml index d55c42c9e..5546a7a88 100644 --- a/ur_calibration/package.xml +++ b/ur_calibration/package.xml @@ -1,7 +1,7 @@ ur_calibration - 2.5.0 + 2.5.1 Package for extracting the factory calibration from a UR robot and change it such that it can be used by ur_description to gain a correct URDF Felix Exner diff --git a/ur_controllers/CHANGELOG.rst b/ur_controllers/CHANGELOG.rst index d5dfb5128..08ef6265d 100644 --- a/ur_controllers/CHANGELOG.rst +++ b/ur_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package ur_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.5.1 (2024-12-21) +------------------ * Add missing test dependencies for ur_controllers (backport of `#1215 `_) * Contributors: mergify[bot] diff --git a/ur_controllers/package.xml b/ur_controllers/package.xml index c2573435e..b72920a04 100644 --- a/ur_controllers/package.xml +++ b/ur_controllers/package.xml @@ -2,7 +2,7 @@ ur_controllers - 2.5.0 + 2.5.1 Provides controllers that use the speed scaling interface of Universal Robots. Felix Exner diff --git a/ur_dashboard_msgs/CHANGELOG.rst b/ur_dashboard_msgs/CHANGELOG.rst index bf076a629..c15e7c1fa 100644 --- a/ur_dashboard_msgs/CHANGELOG.rst +++ b/ur_dashboard_msgs/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package ur_dashboard_msgs ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.5.1 (2024-12-21) +------------------ 2.5.0 (2024-12-18) ------------------ diff --git a/ur_dashboard_msgs/package.xml b/ur_dashboard_msgs/package.xml index 448a4ba10..12f39a421 100644 --- a/ur_dashboard_msgs/package.xml +++ b/ur_dashboard_msgs/package.xml @@ -2,7 +2,7 @@ ur_dashboard_msgs - 2.5.0 + 2.5.1 Messages around the UR Dashboard server. Felix Exner diff --git a/ur_moveit_config/CHANGELOG.rst b/ur_moveit_config/CHANGELOG.rst index 85276745c..cde6861b6 100644 --- a/ur_moveit_config/CHANGELOG.rst +++ b/ur_moveit_config/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package ur_moveit_config ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.5.1 (2024-12-21) +------------------ 2.5.0 (2024-12-18) ------------------ diff --git a/ur_moveit_config/package.xml b/ur_moveit_config/package.xml index 726e20744..11f07baea 100644 --- a/ur_moveit_config/package.xml +++ b/ur_moveit_config/package.xml @@ -2,7 +2,7 @@ ur_moveit_config - 2.5.0 + 2.5.1 An example package with MoveIt2 configurations for UR robots. diff --git a/ur_robot_driver/CHANGELOG.rst b/ur_robot_driver/CHANGELOG.rst index 1a5e2b558..48fa56406 100644 --- a/ur_robot_driver/CHANGELOG.rst +++ b/ur_robot_driver/CHANGELOG.rst @@ -1,5 +1,5 @@ -Forthcoming ------------ +2.5.1 (2024-12-21) +------------------ 2.5.0 (2024-12-18) ------------------ diff --git a/ur_robot_driver/package.xml b/ur_robot_driver/package.xml index da4aab2ed..142ec4166 100644 --- a/ur_robot_driver/package.xml +++ b/ur_robot_driver/package.xml @@ -2,7 +2,7 @@ ur_robot_driver - 2.5.0 + 2.5.1 The new driver for Universal Robots UR3, UR5 and UR10 robots with CB3 controllers and the e-series. Felix Exner From 1cd4e05c4339157ab72510a4c6a4029c65a19eab Mon Sep 17 00:00:00 2001 From: "mergify[bot]" <37929162+mergify[bot]@users.noreply.github.com> Date: Sat, 28 Dec 2024 13:28:49 +0100 Subject: [PATCH 030/133] Remove unused include (backport of #1220) Co-authored-by: Bence Magyar --- ur_robot_driver/include/ur_robot_driver/hardware_interface.hpp | 1 - 1 file changed, 1 deletion(-) diff --git a/ur_robot_driver/include/ur_robot_driver/hardware_interface.hpp b/ur_robot_driver/include/ur_robot_driver/hardware_interface.hpp index 4d525e92a..14bdd29ea 100644 --- a/ur_robot_driver/include/ur_robot_driver/hardware_interface.hpp +++ b/ur_robot_driver/include/ur_robot_driver/hardware_interface.hpp @@ -49,7 +49,6 @@ #include "hardware_interface/hardware_info.hpp" #include "hardware_interface/system_interface.hpp" #include "hardware_interface/types/hardware_interface_return_values.hpp" -#include "hardware_interface/visibility_control.h" // UR stuff #include "ur_client_library/ur/ur_driver.h" From 9b6103a770a71942844a42ca375478b8931936c5 Mon Sep 17 00:00:00 2001 From: "mergify[bot]" <37929162+mergify[bot]@users.noreply.github.com> Date: Fri, 3 Jan 2025 08:26:21 +0100 Subject: [PATCH 031/133] Auto-update pre-commit hooks (backport of #1218) --- .pre-commit-config.yaml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml index 67ecafab9..b937ac1d8 100644 --- a/.pre-commit-config.yaml +++ b/.pre-commit-config.yaml @@ -33,7 +33,7 @@ repos: # Python hooks - repo: https://github.com/asottile/pyupgrade - rev: v3.19.0 + rev: v3.19.1 hooks: - id: pyupgrade args: [--py36-plus] From 588aa3093c4506b008b50cb44d1b5c1f78dd6c8a Mon Sep 17 00:00:00 2001 From: "mergify[bot]" <37929162+mergify[bot]@users.noreply.github.com> Date: Tue, 14 Jan 2025 14:02:10 +0100 Subject: [PATCH 032/133] Disable pose broadcaster on mock hardware (backport of #1229) (#1230) As mock hardware doesn't send the same pose information as the real robot does, it does make sense to not start it in mock hardware. Co-authored-by: URJala <159417921+URJala@users.noreply.github.com> Co-authored-by: Felix Exner --- ur_robot_driver/launch/ur_control.launch.py | 8 ++++++-- 1 file changed, 6 insertions(+), 2 deletions(-) diff --git a/ur_robot_driver/launch/ur_control.launch.py b/ur_robot_driver/launch/ur_control.launch.py index 1d1ab86bc..72de393ab 100644 --- a/ur_robot_driver/launch/ur_control.launch.py +++ b/ur_robot_driver/launch/ur_control.launch.py @@ -355,8 +355,12 @@ def controller_spawner(controllers, active=True): controllers_active.append(initial_joint_controller.perform(context)) controllers_inactive.remove(initial_joint_controller.perform(context)) - controller_spawners = [controller_spawner(controllers_active)] + [ - controller_spawner(controllers_inactive, active=False) + if use_fake_hardware.perform(context) == "true": + controllers_active.remove("tcp_pose_broadcaster") + + controller_spawners = [ + controller_spawner(controllers_active), + controller_spawner(controllers_inactive, active=False), ] nodes_to_start = [ From 3accd7f9e8aad621d5a659227535759104ce658e Mon Sep 17 00:00:00 2001 From: Felix Exner Date: Wed, 15 Jan 2025 08:16:21 +0100 Subject: [PATCH 033/133] Update pre-commit the same as on the main branch (#1232) * Update pre-commit the same as on the main branch * Fix typo in changelog --- .pre-commit-config.yaml | 10 +++++----- ur_bringup/CHANGELOG.rst | 2 +- ur_controllers/CHANGELOG.rst | 2 +- ur_robot_driver/CHANGELOG.rst | 2 +- 4 files changed, 8 insertions(+), 8 deletions(-) diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml index b937ac1d8..aa81841f3 100644 --- a/.pre-commit-config.yaml +++ b/.pre-commit-config.yaml @@ -45,13 +45,13 @@ repos: args: ["--line-length=100"] - repo: https://github.com/pycqa/pydocstyle - rev: 6.1.1 + rev: 6.3.0 hooks: - id: pydocstyle args: ["--ignore=D100,D101,D102,D103,D104,D105,D106,D107,D203,D212,D401,D404"] - repo: https://github.com/pycqa/flake8 - rev: 3.9.0 + rev: 7.1.1 hooks: - id: flake8 args: ["--ignore=E501,W503"] @@ -126,7 +126,7 @@ repos: args: ['--max-line-length=100', '--ignore=D001'] - repo: https://github.com/pre-commit/pygrep-hooks - rev: v1.8.0 + rev: v1.10.0 hooks: - id: rst-backticks - id: rst-directive-colons @@ -135,8 +135,8 @@ repos: # Spellcheck in comments and docs # skipping of *.svg files is not working... - repo: https://github.com/codespell-project/codespell - rev: v2.0.0 + rev: v2.3.0 hooks: - id: codespell - args: ['--write-changes'] + args: ['--write-changes', '-L bootup,assertIn'] exclude: \.(svg|pyc|drawio)$ diff --git a/ur_bringup/CHANGELOG.rst b/ur_bringup/CHANGELOG.rst index 3c02b7ab0..23a64b6f2 100644 --- a/ur_bringup/CHANGELOG.rst +++ b/ur_bringup/CHANGELOG.rst @@ -88,7 +88,7 @@ Changelog for package ur_bringup * Update MoveIt file for working with simulation. (`#278 `_) * Start the tool communication script if the flag is set (`#267 `_) * Used ``spawner`` instead of ``spanwer.py`` in launch files (`#293 `_) -* Do not start dashboard client if FakeHardware simuation is used. (`#286 `_) +* Do not start dashboard client if FakeHardware simulation is used. (`#286 `_) * Use scaled trajectory controller per default. (`#287 `_) * Separate control node (`#281 `_) * Fix launch file arguments. (`#243 `_) diff --git a/ur_controllers/CHANGELOG.rst b/ur_controllers/CHANGELOG.rst index 08ef6265d..c229959b8 100644 --- a/ur_controllers/CHANGELOG.rst +++ b/ur_controllers/CHANGELOG.rst @@ -87,7 +87,7 @@ Changelog for package ur_controllers * Adapt ros control api (`#448 `_) * scaled jtc: Use get_interface_name instead of get_name * Migrate from stopped controllers to inactive controllers - stopped controllers has been depreated upstream + stopped controllers has been deprecated upstream * Contributors: Felix Exner 2.2.2 (2022-07-19) diff --git a/ur_robot_driver/CHANGELOG.rst b/ur_robot_driver/CHANGELOG.rst index 48fa56406..ef79e8d8d 100644 --- a/ur_robot_driver/CHANGELOG.rst +++ b/ur_robot_driver/CHANGELOG.rst @@ -132,7 +132,7 @@ * Adapt ros control api (`#448 `_) * scaled jtc: Use get_interface_name instead of get_name * Migrate from stopped controllers to inactive controllers - stopped controllers has been depreated upstream + stopped controllers has been deprecated upstream * Contributors: Felix Exner 2.2.2 (2022-07-19) From 70b5803e57ab25212f76163a17bae8db7c77421a Mon Sep 17 00:00:00 2001 From: Felix Exner Date: Thu, 16 Jan 2025 14:02:00 +0100 Subject: [PATCH 034/133] ur_moveit_config: Do not change default controller when using fake hardware (#1237) --- ur_moveit_config/launch/ur_moveit.launch.py | 13 +------------ 1 file changed, 1 insertion(+), 12 deletions(-) diff --git a/ur_moveit_config/launch/ur_moveit.launch.py b/ur_moveit_config/launch/ur_moveit.launch.py index b1858d519..3ceb5558b 100644 --- a/ur_moveit_config/launch/ur_moveit.launch.py +++ b/ur_moveit_config/launch/ur_moveit.launch.py @@ -43,7 +43,6 @@ FindExecutable, LaunchConfiguration, PathJoinSubstitution, - OrSubstitution, ) @@ -51,7 +50,6 @@ def launch_setup(context, *args, **kwargs): # Initialize Arguments ur_type = LaunchConfiguration("ur_type") - use_fake_hardware = LaunchConfiguration("use_fake_hardware") safety_limits = LaunchConfiguration("safety_limits") safety_pos_margin = LaunchConfiguration("safety_pos_margin") safety_k_position = LaunchConfiguration("safety_k_position") @@ -179,9 +177,7 @@ def launch_setup(context, *args, **kwargs): # Trajectory Execution Configuration controllers_yaml = load_yaml("ur_moveit_config", "config/controllers.yaml") # the scaled_joint_trajectory_controller does not work on fake hardware - change_controllers = context.perform_substitution( - OrSubstitution(use_fake_hardware, use_sim_time) - ) + change_controllers = context.perform_substitution(use_sim_time) if change_controllers == "true": controllers_yaml["scaled_joint_trajectory_controller"]["default"] = False controllers_yaml["joint_trajectory_controller"]["default"] = True @@ -287,13 +283,6 @@ def generate_launch_description(): choices=["ur3", "ur3e", "ur5", "ur5e", "ur10", "ur10e", "ur16e", "ur20", "ur30"], ) ) - declared_arguments.append( - DeclareLaunchArgument( - "use_fake_hardware", - default_value="false", - description="Indicate whether robot is running with fake hardware mirroring command to its states.", - ) - ) declared_arguments.append( DeclareLaunchArgument( "safety_limits", From 0ab144ea8e1ddffc9b708e4b193c7619e311878a Mon Sep 17 00:00:00 2001 From: Felix Exner Date: Mon, 20 Jan 2025 09:05:28 +0100 Subject: [PATCH 035/133] update control_toolbox branch (#1241) control_toolbox has branched out for Humble. This updates the repos file accordingly. --- Universal_Robots_ROS2_Driver.humble.repos | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Universal_Robots_ROS2_Driver.humble.repos b/Universal_Robots_ROS2_Driver.humble.repos index d492a0811..c8d2d0906 100644 --- a/Universal_Robots_ROS2_Driver.humble.repos +++ b/Universal_Robots_ROS2_Driver.humble.repos @@ -30,7 +30,7 @@ repositories: control_toolbox: type: git url: https://github.com/ros-controls/control_toolbox.git - version: ros2-master + version: humble realtime_tools: type: git url: https://github.com/ros-controls/realtime_tools.git From d382fa399b8251f3c10f6d39f7aa79867aff752c Mon Sep 17 00:00:00 2001 From: "mergify[bot]" <37929162+mergify[bot]@users.noreply.github.com> Date: Mon, 20 Jan 2025 09:31:53 +0100 Subject: [PATCH 036/133] fix sphinx doc link in ur_robot_driver (#1240) (#1242) --- ur_robot_driver/doc/make.bat | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ur_robot_driver/doc/make.bat b/ur_robot_driver/doc/make.bat index 27f573b87..558ef605a 100644 --- a/ur_robot_driver/doc/make.bat +++ b/ur_robot_driver/doc/make.bat @@ -21,7 +21,7 @@ if errorlevel 9009 ( echo.may add the Sphinx directory to PATH. echo. echo.If you don't have Sphinx installed, grab it from - echo.http://sphinx-doc.org/ + echo.https://sphinx-doc.org/ exit /b 1 ) From eb5f5b8fac66e1bf4732cb7d83bfba37c3d716b9 Mon Sep 17 00:00:00 2001 From: "mergify[bot]" <37929162+mergify[bot]@users.noreply.github.com> Date: Mon, 20 Jan 2025 09:32:22 +0100 Subject: [PATCH 037/133] Check quaternions for equal dot_product instead of comparing their components individually (backport #1238) (#1243) The lookup can result in quaternions that look different component-wise while still being very similar. Instead of comparing them component-wise to decide whether they are similar, we check whether their dot-product is equal to 1.0. That should be more robust. (cherry picked from commit fa82013dd789e7569725c5c8e243bdcc4e643124) Co-authored-by: Felix Exner --- .../test/integration_test_force_mode.py | 28 ++++++------------- 1 file changed, 9 insertions(+), 19 deletions(-) diff --git a/ur_robot_driver/test/integration_test_force_mode.py b/ur_robot_driver/test/integration_test_force_mode.py index 5c793081b..a1dc89f23 100644 --- a/ur_robot_driver/test/integration_test_force_mode.py +++ b/ur_robot_driver/test/integration_test_force_mode.py @@ -67,6 +67,11 @@ TIMEOUT_EXECUTE_TRAJECTORY = 30 +def are_quaternions_same(q1, q2, tolerance): + dot_product = q1.x * q2.x + q1.y * q2.y + q1.z * q2.z + q1.w * q2.w + return (abs(dot_product) - 1.0) < tolerance + + @pytest.mark.launch_test @launch_testing.parametrize( "tf_prefix", @@ -205,25 +210,10 @@ def test_force_mode_controller(self, tf_prefix): trans_before.transform.translation.z, delta=0.001, ) - self.assertAlmostEqual( - trans_after.transform.rotation.x, - trans_before.transform.rotation.x, - delta=0.01, - ) - self.assertAlmostEqual( - trans_after.transform.rotation.y, - trans_before.transform.rotation.y, - delta=0.01, - ) - self.assertAlmostEqual( - trans_after.transform.rotation.z, - trans_before.transform.rotation.z, - delta=0.01, - ) - self.assertAlmostEqual( - trans_after.transform.rotation.w, - trans_before.transform.rotation.w, - delta=0.01, + self.assertTrue( + are_quaternions_same( + trans_after.transform.rotation, trans_before.transform.rotation, 0.001 + ) ) res = self._force_mode_controller_interface.stop_force_mode() From 8e8f9425699def9db11f31b634060c23bc78bf4c Mon Sep 17 00:00:00 2001 From: "mergify[bot]" <37929162+mergify[bot]@users.noreply.github.com> Date: Tue, 21 Jan 2025 08:50:39 +0100 Subject: [PATCH 038/133] Remove Iron from the active distributions (backport #1239) (#1244) * Remove Iron from the active distributions * Remove unused foxy and galactic builds --- .github/workflows/iron-binary-main.yml | 20 +++++++++ README.md | 58 ++++++++++++-------------- ci_status.md | 27 ------------ 3 files changed, 47 insertions(+), 58 deletions(-) create mode 100644 .github/workflows/iron-binary-main.yml diff --git a/.github/workflows/iron-binary-main.yml b/.github/workflows/iron-binary-main.yml new file mode 100644 index 000000000..cd70d5602 --- /dev/null +++ b/.github/workflows/iron-binary-main.yml @@ -0,0 +1,20 @@ +name: Iron Binary Build Main +on: + workflow_dispatch: + pull_request: + branches: + - iron + push: + branches: + - iron + schedule: + # Run every morning to detect flakiness and broken dependencies + - cron: '13 5 * * *' + +jobs: + iron_binary_main: + uses: ./.github/workflows/reusable_ici.yml + with: + ros_distro: iron + ros_repo: main + ref_for_scheduled_build: iron diff --git a/README.md b/README.md index 432604d11..4a5f1e1e7 100644 --- a/README.md +++ b/README.md @@ -18,54 +18,40 @@ Check also [presentations and videos](ur_robot_driver/doc/resources/README.md) a - - - + + - - - + + - - - - -
ROS2 DistroFoxy (EOL)Galactic (EOL) HumbleIronJazzy Rolling
Branchfoxygalactic humbleironmain main
Release status - - Foxy Binary Build -
- 		- - Galactic Binary Build -
- 		+
-
- - - - + 		+
+ + + + 		-
- - - - + 		+
+ + + +
@@ -76,6 +62,16 @@ A more [detailed build status](ci_status.md) shows the state of all CI workflows Please note that the detailed view is intended for developers, while the one here should give end users an overview of the current released state. +### EOL distros +The following distributions are End-Of-Line (EOL). Branches for these exist and released packages +are probably available for an unknown amount of time, but it is recommended to upgrade to a +supported distribution. +For EOL distributions the nightly binary builds from our CI are shown. EOL distributions will +receive no more updates and may be lacking features. + +* Foxy (branch: [foxy](https://github.com/UniversalRobots/Universal_Robots_ROS2_Driver/tree/foxy)) [![Foxy Binary Build](https://github.com/UniversalRobots/Universal_Robots_ROS2_Driver/actions/workflows/foxy-binary-build.yml/badge.svg?event=schedule)](https://github.com/UniversalRobots/Universal_Robots_ROS2_Driver/actions/workflows/foxy-binary-build.yml?query=event%3Aschedule++) +* Galactic (branch: [galactic](https://github.com/UniversalRobots/Universal_Robots_ROS2_Driver/tree/galactic)) [![Galactic Binary Build](https://github.com/UniversalRobots/Universal_Robots_ROS2_Driver/actions/workflows/galactic-binary-build.yml/badge.svg?event=schedule)](https://github.com/UniversalRobots/Universal_Robots_ROS2_Driver/actions/workflows/galactic-binary-build.yml?query=event%3Aschedule++) +* Iron (branch: [iron](https://github.com/UniversalRobots/Universal_Robots_ROS2_Driver/tree/iron)) [![Iron Binary Build](https://github.com/UniversalRobots/Universal_Robots_ROS2_Driver/actions/workflows/iron-binary-main.yml/badge.svg?event=schedule)](https://github.com/UniversalRobots/Universal_Robots_ROS2_Driver/actions/workflows/iron-binary-main.yml?query=event%3Aschedule++) ## Packages in the Repository: diff --git a/ci_status.md b/ci_status.md index 4df0e8f42..e8cd551e7 100644 --- a/ci_status.md +++ b/ci_status.md @@ -7,13 +7,11 @@ red pipeline there should be a corresponding issue labeled with [ci-failure](htt - - @@ -36,24 +34,6 @@ red pipeline there should be a corresponding issue labeled with [ci-failure](htt alt="Humble Semi-Binary Testing"/> - - @@ -103,7 +91,7 @@ red pipeline there should be a corresponding issue labeled with [ci-failure](htt Uses repos file: `src/Universal_Robots_ROS2_Driver/Universal_Robots_ROS2_Driver-not-released..repos` -1. Semi-binary builds - against released core ROS packages (main and testing), but the immediate dependencies are pulled from source. +1. Semi-binary builds - against released core ROS packages, but the immediate dependencies are pulled from source. Shows that local build with dependencies is possible and if this fails we can expect that after the next package sync we will not be able to build. Uses repos file: `src/Universal_Robots_ROS2_Driver/Universal_Robots_ROS2_Driver.repos` From 5059942e2fedb1cea21d42d30c076e5af7085a55 Mon Sep 17 00:00:00 2001 From: "mergify[bot]" <37929162+mergify[bot]@users.noreply.github.com> Date: Wed, 2 Apr 2025 13:09:54 +0200 Subject: [PATCH 057/133] Use UrDriverConfig struct to initialize UrDriver (backport of #1328) (#1330) The initializer list has been deprecated which raises a warning in the buildfarm. --- ur_robot_driver/src/hardware_interface.cpp | 23 +++++++++++++++++----- 1 file changed, 18 insertions(+), 5 deletions(-) diff --git a/ur_robot_driver/src/hardware_interface.cpp b/ur_robot_driver/src/hardware_interface.cpp index 20b789608..1286e773a 100644 --- a/ur_robot_driver/src/hardware_interface.cpp +++ b/ur_robot_driver/src/hardware_interface.cpp @@ -522,11 +522,24 @@ URPositionHardwareInterface::on_configure(const rclcpp_lifecycle::State& previou registerUrclLogHandler(tf_prefix); try { rtde_comm_has_been_started_ = false; - ur_driver_ = std::make_unique( - robot_ip, script_filename, output_recipe_filename, input_recipe_filename, - std::bind(&URPositionHardwareInterface::handleRobotProgramState, this, std::placeholders::_1), headless_mode, - std::move(tool_comm_setup), (uint32_t)reverse_port, (uint32_t)script_sender_port, servoj_gain, - servoj_lookahead_time, non_blocking_read_, reverse_ip, trajectory_port, script_command_port); + urcl::UrDriverConfiguration driver_config; + driver_config.robot_ip = robot_ip; + driver_config.script_file = script_filename; + driver_config.output_recipe_file = output_recipe_filename; + driver_config.input_recipe_file = input_recipe_filename; + driver_config.headless_mode = headless_mode; + driver_config.reverse_port = static_cast(reverse_port); + driver_config.script_sender_port = static_cast(script_sender_port); + driver_config.trajectory_port = static_cast(trajectory_port); + driver_config.script_command_port = static_cast(script_command_port); + driver_config.reverse_ip = reverse_ip; + driver_config.servoj_gain = static_cast(servoj_gain); + driver_config.servoj_lookahead_time = servoj_lookahead_time; + driver_config.non_blocking_read = non_blocking_read_; + driver_config.tool_comm_setup = std::move(tool_comm_setup); + driver_config.handle_program_state = + std::bind(&URPositionHardwareInterface::handleRobotProgramState, this, std::placeholders::_1); + ur_driver_ = std::make_unique(driver_config); } catch (urcl::ToolCommNotAvailable& e) { RCLCPP_FATAL_STREAM(rclcpp::get_logger("URPositionHardwareInterface"), "See parameter use_tool_communication"); From 5ba9b5c3413633cbc1d67a00b570fc22fec67bcd Mon Sep 17 00:00:00 2001 From: "mergify[bot]" <37929162+mergify[bot]@users.noreply.github.com> Date: Fri, 4 Apr 2025 09:37:42 +0200 Subject: [PATCH 058/133] Add support for UR7e and UR12e (#1332) --- README.md | 2 +- ur_bringup/launch/ur12e.launch.py | 99 +++++++++++++++++++ ur_bringup/launch/ur7e.launch.py | 99 +++++++++++++++++++ ur_bringup/launch/ur_control.launch.py | 2 + ur_moveit_config/launch/ur_moveit.launch.py | 14 ++- ur_robot_driver/doc/usage/startup.rst | 4 +- ur_robot_driver/launch/ur12e.launch.py | 101 ++++++++++++++++++++ ur_robot_driver/launch/ur7e.launch.py | 101 ++++++++++++++++++++ ur_robot_driver/launch/ur_control.launch.py | 14 ++- ur_robot_driver/test/test_common.py | 14 ++- 10 files changed, 444 insertions(+), 6 deletions(-) create mode 100644 ur_bringup/launch/ur12e.launch.py create mode 100644 ur_bringup/launch/ur7e.launch.py create mode 100644 ur_robot_driver/launch/ur12e.launch.py create mode 100644 ur_robot_driver/launch/ur7e.launch.py diff --git a/README.md b/README.md index 665854712..29b73e43c 100644 --- a/README.md +++ b/README.md @@ -117,7 +117,7 @@ For getting started, you'll basically need three steps: details. ```bash - # Replace ur5e with one of ur3, ur3e, ur5, ur5e, ur10, ur10e, ur16e, ur20, ur30 + # Replace ur5e with one of ur3, ur3e, ur5, ur5e, ur7e, ur10, ur10e, ur12e, ur16e, ur20, ur30 # Replace the IP address with the IP address of your actual robot / URSim ros2 launch ur_robot_driver ur_control.launch.py ur_type:=ur5e robot_ip:=192.168.56.101 ``` diff --git a/ur_bringup/launch/ur12e.launch.py b/ur_bringup/launch/ur12e.launch.py new file mode 100644 index 000000000..5650225f7 --- /dev/null +++ b/ur_bringup/launch/ur12e.launch.py @@ -0,0 +1,99 @@ +# Copyright (c) 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. + +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import LaunchConfiguration, ThisLaunchFileDir + + +def generate_launch_description(): + # Declare arguments + declared_arguments = [] + declared_arguments.append( + DeclareLaunchArgument( + "robot_ip", + description="IP address by which the robot can be reached.", + ) + ) + declared_arguments.append( + DeclareLaunchArgument( + "use_fake_hardware", + default_value="false", + description="Start robot with fake hardware mirroring command to its states.", + ) + ) + declared_arguments.append( + DeclareLaunchArgument( + "fake_sensor_commands", + default_value="false", + description="Enable fake command interfaces for sensors used for simple simulations. \ + Used only if 'use_fake_hardware' parameter is true.", + ) + ) + declared_arguments.append( + DeclareLaunchArgument( + "initial_joint_controller", + default_value="scaled_joint_trajectory_controller", + description="Initially loaded robot controller.", + choices=[ + "scaled_joint_trajectory_controller", + "joint_trajectory_controller", + "forward_velocity_controller", + "forward_position_controller", + ], + ) + ) + declared_arguments.append( + DeclareLaunchArgument( + "activate_joint_controller", + default_value="true", + description="Activate loaded joint controller.", + ) + ) + + # Initialize Arguments + robot_ip = LaunchConfiguration("robot_ip") + use_fake_hardware = LaunchConfiguration("use_fake_hardware") + fake_sensor_commands = LaunchConfiguration("fake_sensor_commands") + initial_joint_controller = LaunchConfiguration("initial_joint_controller") + activate_joint_controller = LaunchConfiguration("activate_joint_controller") + + base_launch = IncludeLaunchDescription( + PythonLaunchDescriptionSource([ThisLaunchFileDir(), "/ur_control.launch.py"]), + launch_arguments={ + "ur_type": "ur12e", + "robot_ip": robot_ip, + "use_fake_hardware": use_fake_hardware, + "fake_sensor_commands": fake_sensor_commands, + "initial_joint_controller": initial_joint_controller, + "activate_joint_controller": activate_joint_controller, + }.items(), + ) + + return LaunchDescription(declared_arguments + [base_launch]) diff --git a/ur_bringup/launch/ur7e.launch.py b/ur_bringup/launch/ur7e.launch.py new file mode 100644 index 000000000..7eade6f25 --- /dev/null +++ b/ur_bringup/launch/ur7e.launch.py @@ -0,0 +1,99 @@ +# Copyright (c) 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. + +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import LaunchConfiguration, ThisLaunchFileDir + + +def generate_launch_description(): + # Declare arguments + declared_arguments = [] + declared_arguments.append( + DeclareLaunchArgument( + "robot_ip", + description="IP address by which the robot can be reached.", + ) + ) + declared_arguments.append( + DeclareLaunchArgument( + "use_fake_hardware", + default_value="false", + description="Start robot with fake hardware mirroring command to its states.", + ) + ) + declared_arguments.append( + DeclareLaunchArgument( + "fake_sensor_commands", + default_value="false", + description="Enable fake command interfaces for sensors used for simple simulations. \ + Used only if 'use_fake_hardware' parameter is true.", + ) + ) + declared_arguments.append( + DeclareLaunchArgument( + "initial_joint_controller", + default_value="scaled_joint_trajectory_controller", + description="Initially loaded robot controller.", + choices=[ + "scaled_joint_trajectory_controller", + "joint_trajectory_controller", + "forward_velocity_controller", + "forward_position_controller", + ], + ) + ) + declared_arguments.append( + DeclareLaunchArgument( + "activate_joint_controller", + default_value="true", + description="Activate loaded joint controller.", + ) + ) + + # Initialize Arguments + robot_ip = LaunchConfiguration("robot_ip") + use_fake_hardware = LaunchConfiguration("use_fake_hardware") + fake_sensor_commands = LaunchConfiguration("fake_sensor_commands") + initial_joint_controller = LaunchConfiguration("initial_joint_controller") + activate_joint_controller = LaunchConfiguration("activate_joint_controller") + + base_launch = IncludeLaunchDescription( + PythonLaunchDescriptionSource([ThisLaunchFileDir(), "/ur_control.launch.py"]), + launch_arguments={ + "ur_type": "ur7e", + "robot_ip": robot_ip, + "use_fake_hardware": use_fake_hardware, + "fake_sensor_commands": fake_sensor_commands, + "initial_joint_controller": initial_joint_controller, + "activate_joint_controller": activate_joint_controller, + }.items(), + ) + + return LaunchDescription(declared_arguments + [base_launch]) diff --git a/ur_bringup/launch/ur_control.launch.py b/ur_bringup/launch/ur_control.launch.py index 8b1b395ca..c5139376e 100644 --- a/ur_bringup/launch/ur_control.launch.py +++ b/ur_bringup/launch/ur_control.launch.py @@ -55,8 +55,10 @@ def generate_launch_description(): "ur3e", "ur5", "ur5e", + "ur7e", "ur10", "ur10e", + "ur12e", "ur16e", "ur20", "ur30", diff --git a/ur_moveit_config/launch/ur_moveit.launch.py b/ur_moveit_config/launch/ur_moveit.launch.py index 3ceb5558b..f6bd72d34 100644 --- a/ur_moveit_config/launch/ur_moveit.launch.py +++ b/ur_moveit_config/launch/ur_moveit.launch.py @@ -280,7 +280,19 @@ def generate_launch_description(): DeclareLaunchArgument( "ur_type", description="Type/series of used UR robot.", - choices=["ur3", "ur3e", "ur5", "ur5e", "ur10", "ur10e", "ur16e", "ur20", "ur30"], + choices=[ + "ur3", + "ur3e", + "ur5", + "ur5e", + "ur7e", + "ur10", + "ur10e", + "ur12e", + "ur16e", + "ur20", + "ur30", + ], ) ) declared_arguments.append( diff --git a/ur_robot_driver/doc/usage/startup.rst b/ur_robot_driver/doc/usage/startup.rst index 6b0644563..a78ae6fdf 100644 --- a/ur_robot_driver/doc/usage/startup.rst +++ b/ur_robot_driver/doc/usage/startup.rst @@ -22,8 +22,8 @@ nodes for UR robots. The only required arguments are the ``ur_type`` and ``robot $ ros2 launch ur_robot_driver ur_control.launch.py ur_type:=ur5e robot_ip:=192.168.56.101 -Allowed ``ur_type`` strings: ``ur3``, ``ur3e``, ``ur5``, ``ur5e``, ``ur10``, ``ur10e``, ``ur16e``, -``ur20``, ``ur30``. +Allowed ``ur_type`` strings: ``ur3``, ``ur3e``, ``ur5``, ``ur5e``, ``ur7e``, ``ur10``, ``ur10e``, +``ur12e``, ``ur16e``, ``ur20``, ``ur30``. Other important arguments are: diff --git a/ur_robot_driver/launch/ur12e.launch.py b/ur_robot_driver/launch/ur12e.launch.py new file mode 100644 index 000000000..a2f2d7826 --- /dev/null +++ b/ur_robot_driver/launch/ur12e.launch.py @@ -0,0 +1,101 @@ +# Copyright (c) 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. + +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import LaunchConfiguration, ThisLaunchFileDir + + +def generate_launch_description(): + # Declare arguments + declared_arguments = [] + declared_arguments.append( + DeclareLaunchArgument( + "robot_ip", + description="IP address by which the robot can be reached.", + ) + ) + declared_arguments.append( + DeclareLaunchArgument( + "use_mock_hardware", + default_value="false", + description="Start robot with mock hardware mirroring command to its states.", + ) + ) + declared_arguments.append( + DeclareLaunchArgument( + "mock_sensor_commands", + default_value="false", + description="Enable mock command interfaces for sensors used for simple simulations. " + "Used only if 'use_mock_hardware' parameter is true.", + ) + ) + declared_arguments.append( + DeclareLaunchArgument( + "initial_joint_controller", + default_value="scaled_joint_trajectory_controller", + description="Initially loaded robot controller.", + choices=[ + "scaled_joint_trajectory_controller", + "joint_trajectory_controller", + "forward_velocity_controller", + "forward_position_controller", + "freedrive_mode_controller", + "passthrough_trajectory_controller", + ], + ) + ) + declared_arguments.append( + DeclareLaunchArgument( + "activate_joint_controller", + default_value="true", + description="Activate loaded joint controller.", + ) + ) + + # Initialize Arguments + robot_ip = LaunchConfiguration("robot_ip") + use_mock_hardware = LaunchConfiguration("use_mock_hardware") + mock_sensor_commands = LaunchConfiguration("mock_sensor_commands") + initial_joint_controller = LaunchConfiguration("initial_joint_controller") + activate_joint_controller = LaunchConfiguration("activate_joint_controller") + + base_launch = IncludeLaunchDescription( + PythonLaunchDescriptionSource([ThisLaunchFileDir(), "/ur_control.launch.py"]), + launch_arguments={ + "ur_type": "ur12e", + "robot_ip": robot_ip, + "use_mock_hardware": use_mock_hardware, + "mock_sensor_commands": mock_sensor_commands, + "initial_joint_controller": initial_joint_controller, + "activate_joint_controller": activate_joint_controller, + }.items(), + ) + + return LaunchDescription(declared_arguments + [base_launch]) diff --git a/ur_robot_driver/launch/ur7e.launch.py b/ur_robot_driver/launch/ur7e.launch.py new file mode 100644 index 000000000..de53abf99 --- /dev/null +++ b/ur_robot_driver/launch/ur7e.launch.py @@ -0,0 +1,101 @@ +# Copyright (c) 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. + +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import LaunchConfiguration, ThisLaunchFileDir + + +def generate_launch_description(): + # Declare arguments + declared_arguments = [] + declared_arguments.append( + DeclareLaunchArgument( + "robot_ip", + description="IP address by which the robot can be reached.", + ) + ) + declared_arguments.append( + DeclareLaunchArgument( + "use_mock_hardware", + default_value="false", + description="Start robot with mock hardware mirroring command to its states.", + ) + ) + declared_arguments.append( + DeclareLaunchArgument( + "mock_sensor_commands", + default_value="false", + description="Enable mock command interfaces for sensors used for simple simulations. " + "Used only if 'use_mock_hardware' parameter is true.", + ) + ) + declared_arguments.append( + DeclareLaunchArgument( + "initial_joint_controller", + default_value="scaled_joint_trajectory_controller", + description="Initially loaded robot controller.", + choices=[ + "scaled_joint_trajectory_controller", + "joint_trajectory_controller", + "forward_velocity_controller", + "forward_position_controller", + "freedrive_mode_controller", + "passthrough_trajectory_controller", + ], + ) + ) + declared_arguments.append( + DeclareLaunchArgument( + "activate_joint_controller", + default_value="true", + description="Activate loaded joint controller.", + ) + ) + + # Initialize Arguments + robot_ip = LaunchConfiguration("robot_ip") + use_mock_hardware = LaunchConfiguration("use_mock_hardware") + mock_sensor_commands = LaunchConfiguration("mock_sensor_commands") + initial_joint_controller = LaunchConfiguration("initial_joint_controller") + activate_joint_controller = LaunchConfiguration("activate_joint_controller") + + base_launch = IncludeLaunchDescription( + PythonLaunchDescriptionSource([ThisLaunchFileDir(), "/ur_control.launch.py"]), + launch_arguments={ + "ur_type": "ur7e", + "robot_ip": robot_ip, + "use_mock_hardware": use_mock_hardware, + "mock_sensor_commands": mock_sensor_commands, + "initial_joint_controller": initial_joint_controller, + "activate_joint_controller": activate_joint_controller, + }.items(), + ) + + return LaunchDescription(declared_arguments + [base_launch]) diff --git a/ur_robot_driver/launch/ur_control.launch.py b/ur_robot_driver/launch/ur_control.launch.py index 4c47b07b3..03f85d991 100644 --- a/ur_robot_driver/launch/ur_control.launch.py +++ b/ur_robot_driver/launch/ur_control.launch.py @@ -395,7 +395,19 @@ def generate_launch_description(): DeclareLaunchArgument( "ur_type", description="Type/series of used UR robot.", - choices=["ur3", "ur3e", "ur5", "ur5e", "ur10", "ur10e", "ur16e", "ur20", "ur30"], + choices=[ + "ur3", + "ur3e", + "ur5", + "ur5e", + "ur7e", + "ur10", + "ur10e", + "ur12e", + "ur16e", + "ur20", + "ur30", + ], ) ) declared_arguments.append( diff --git a/ur_robot_driver/test/test_common.py b/ur_robot_driver/test/test_common.py index 927542676..018ec74d2 100644 --- a/ur_robot_driver/test/test_common.py +++ b/ur_robot_driver/test/test_common.py @@ -291,7 +291,19 @@ def _declare_launch_arguments(): "ur_type", default_value="ur5e", description="Type/series of used UR robot.", - choices=["ur3", "ur3e", "ur5", "ur5e", "ur10", "ur10e", "ur16e", "ur20", "ur30"], + choices=[ + "ur3", + "ur3e", + "ur5", + "ur5e", + "ur7e", + "ur10", + "ur10e", + "ur12e", + "ur16e", + "ur20", + "ur30", + ], ) ) From 81c380be3dce6f7b544ca8fd92f6ed6b2ab9c048 Mon Sep 17 00:00:00 2001 From: "mergify[bot]" <37929162+mergify[bot]@users.noreply.github.com> Date: Mon, 7 Apr 2025 09:03:36 +0200 Subject: [PATCH 059/133] Support PolyScopeX robots (backport of #1318) (#1333) * Shutdown dashboard client when connected to PolyScopeX robot Before trying to establish a connection to the dashboard server, check the version of the connected robot. If it is a PolyScopeX robot, shutdown the dashboard client. * Make robot_state_helper use primary_client whenever possible --- ur_robot_driver/CMakeLists.txt | 1 + .../ur_robot_driver/dashboard_client_ros.hpp | 5 +- .../ur_robot_driver/robot_state_helper.hpp | 12 +- ur_robot_driver/launch/ur_control.launch.py | 2 + ur_robot_driver/src/dashboard_client_node.cpp | 14 ++- ur_robot_driver/src/dashboard_client_ros.cpp | 15 ++- ur_robot_driver/src/robot_state_helper.cpp | 111 ++++++++++-------- .../src/robot_state_helper_node.cpp | 9 +- 8 files changed, 110 insertions(+), 59 deletions(-) diff --git a/ur_robot_driver/CMakeLists.txt b/ur_robot_driver/CMakeLists.txt index 0a6f1b747..baba5ffbd 100644 --- a/ur_robot_driver/CMakeLists.txt +++ b/ur_robot_driver/CMakeLists.txt @@ -113,6 +113,7 @@ ament_target_dependencies(controller_stopper_node ${${PROJECT_NAME}_EXPORTED_TAR add_executable(robot_state_helper src/robot_state_helper.cpp src/robot_state_helper_node.cpp + src/urcl_log_handler.cpp ) target_link_libraries(robot_state_helper ur_client_library::urcl) ament_target_dependencies(robot_state_helper ${${PROJECT_NAME}_EXPORTED_TARGETS} ${THIS_PACKAGE_INCLUDE_DEPENDS}) diff --git a/ur_robot_driver/include/ur_robot_driver/dashboard_client_ros.hpp b/ur_robot_driver/include/ur_robot_driver/dashboard_client_ros.hpp index 8349ec692..6c493246f 100644 --- a/ur_robot_driver/include/ur_robot_driver/dashboard_client_ros.hpp +++ b/ur_robot_driver/include/ur_robot_driver/dashboard_client_ros.hpp @@ -51,7 +51,7 @@ // UR client library #include "ur_client_library/ur/dashboard_client.h" #include "ur_client_library/exceptions.h" -#include "ur_dashboard_msgs/msg/program_state.hpp" +#include "ur_client_library/primary/primary_client.h" #include "ur_dashboard_msgs/srv/add_to_log.hpp" #include "ur_dashboard_msgs/srv/get_loaded_program.hpp" #include "ur_dashboard_msgs/srv/get_program_state.hpp" @@ -120,6 +120,9 @@ class DashboardClientROS std::shared_ptr node_; urcl::DashboardClient client_; + urcl::comm::INotifier notifier_; + urcl::primary_interface::PrimaryClient primary_client_; + // Commanding services rclcpp::Service::SharedPtr brake_release_service_; rclcpp::Service::SharedPtr clear_operational_mode_service_; diff --git a/ur_robot_driver/include/ur_robot_driver/robot_state_helper.hpp b/ur_robot_driver/include/ur_robot_driver/robot_state_helper.hpp index d3c03189b..5b7393457 100644 --- a/ur_robot_driver/include/ur_robot_driver/robot_state_helper.hpp +++ b/ur_robot_driver/include/ur_robot_driver/robot_state_helper.hpp @@ -29,7 +29,7 @@ #ifndef UR_ROBOT_DRIVER__ROBOT_STATE_HELPER_HPP_ #define UR_ROBOT_DRIVER__ROBOT_STATE_HELPER_HPP_ -#include +#include #include #include "rclcpp/rclcpp.hpp" @@ -41,6 +41,7 @@ #include "ur_dashboard_msgs/msg/safety_mode.hpp" #include "ur_dashboard_msgs/msg/robot_mode.hpp" #include "ur_client_library/ur/datatypes.h" +#include "ur_client_library/primary/primary_client.h" namespace ur_robot_driver { @@ -90,6 +91,10 @@ class RobotStateHelper std::atomic program_running_; std::mutex goal_mutex_; + std::string robot_ip_; + urcl::comm::INotifier notifier_; + std::shared_ptr primary_client_; + rclcpp_action::Server::SharedPtr set_mode_as_; rclcpp::CallbackGroup::SharedPtr robot_mode_sub_cb_; @@ -100,12 +105,7 @@ class RobotStateHelper rclcpp::CallbackGroup::SharedPtr service_cb_grp_; - rclcpp::Client::SharedPtr unlock_protective_stop_srv_; rclcpp::Client::SharedPtr restart_safety_srv_; - rclcpp::Client::SharedPtr power_on_srv_; - rclcpp::Client::SharedPtr power_off_srv_; - rclcpp::Client::SharedPtr brake_release_srv_; - rclcpp::Client::SharedPtr stop_program_srv_; rclcpp::Client::SharedPtr play_program_srv_; rclcpp::Client::SharedPtr resend_robot_program_srv_; }; diff --git a/ur_robot_driver/launch/ur_control.launch.py b/ur_robot_driver/launch/ur_control.launch.py index 03f85d991..10d59b017 100644 --- a/ur_robot_driver/launch/ur_control.launch.py +++ b/ur_robot_driver/launch/ur_control.launch.py @@ -262,8 +262,10 @@ def launch_setup(context, *args, **kwargs): executable="robot_state_helper", name="ur_robot_state_helper", output="screen", + condition=UnlessCondition(use_fake_hardware), parameters=[ {"headless_mode": headless_mode}, + {"robot_ip": robot_ip}, ], ) diff --git a/ur_robot_driver/src/dashboard_client_node.cpp b/ur_robot_driver/src/dashboard_client_node.cpp index 91cdf9f97..da9156b5e 100644 --- a/ur_robot_driver/src/dashboard_client_node.cpp +++ b/ur_robot_driver/src/dashboard_client_node.cpp @@ -37,9 +37,10 @@ #include "ur_robot_driver/dashboard_client_ros.hpp" +#include #include -#include "rclcpp/rclcpp.hpp" +#include #include "ur_robot_driver/urcl_log_handler.hpp" int main(int argc, char** argv) @@ -53,7 +54,16 @@ int main(int argc, char** argv) ur_robot_driver::registerUrclLogHandler(""); // Set empty tf_prefix at the moment - ur_robot_driver::DashboardClientROS client(node, robot_ip); + std::shared_ptr client; + try { + client = std::make_shared(node, robot_ip); + } catch (const urcl::UrException& e) { + RCLCPP_WARN(rclcpp::get_logger("Dashboard_Client"), + "%s This warning is expected on a PolyScopeX robot. If you don't want to see this warning, " + "please don't start the dashboard client. Exiting dashboard client now.", + e.what()); + return 0; + } rclcpp::spin(node); diff --git a/ur_robot_driver/src/dashboard_client_ros.cpp b/ur_robot_driver/src/dashboard_client_ros.cpp index d4b866aef..77068fdc1 100644 --- a/ur_robot_driver/src/dashboard_client_ros.cpp +++ b/ur_robot_driver/src/dashboard_client_ros.cpp @@ -37,16 +37,27 @@ */ //---------------------------------------------------------------------- -#include +#include +#include #include +#include + namespace ur_robot_driver { DashboardClientROS::DashboardClientROS(const rclcpp::Node::SharedPtr& node, const std::string& robot_ip) - : node_(node), client_(robot_ip) + : node_(node), client_(robot_ip), primary_client_(robot_ip, notifier_) { node_->declare_parameter("receive_timeout", 1); + + primary_client_.start(10, std::chrono::seconds(10)); + auto robot_version = primary_client_.getRobotVersion(); + + if (robot_version->major > 5) { + throw(urcl::UrException("The dashboard server is only available for CB3 and e-Series robots.")); + } + connect(); // Service to release the brakes. If the robot is currently powered off, it will get powered on on the fly. diff --git a/ur_robot_driver/src/robot_state_helper.cpp b/ur_robot_driver/src/robot_state_helper.cpp index 0f35df6d7..5adefe2a1 100644 --- a/ur_robot_driver/src/robot_state_helper.cpp +++ b/ur_robot_driver/src/robot_state_helper.cpp @@ -27,15 +27,12 @@ // POSSIBILITY OF SUCH DAMAGE. #include +#include +#include #include #include "rclcpp/rclcpp.hpp" #include "rclcpp_action/create_server.hpp" -#include "std_srvs/srv/trigger.hpp" - -#include "ur_dashboard_msgs/action/set_mode.hpp" -#include "ur_dashboard_msgs/msg/safety_mode.hpp" -#include "ur_dashboard_msgs/msg/robot_mode.hpp" #include "ur_client_library/ur/datatypes.h" namespace ur_robot_driver @@ -66,28 +63,27 @@ RobotStateHelper::RobotStateHelper(const rclcpp::Node::SharedPtr& node) node->declare_parameter("headless_mode", false); headless_mode_ = node->get_parameter("headless_mode").as_bool(); - // Service to unlock protective stop - unlock_protective_stop_srv_ = node_->create_client( - "dashboard_client/unlock_protective_stop", rmw_qos_profile_services_default, service_cb_grp_); - // Service to restart safety - restart_safety_srv_ = node_->create_client("dashboard_client/restart_safety", + node->declare_parameter("robot_ip", "192.168.56.101"); + robot_ip_ = node->get_parameter("robot_ip").as_string(); + + primary_client_ = std::make_shared(robot_ip_, notifier_); + + primary_client_->start(0, std::chrono::seconds(10)); + auto robot_version = primary_client_->getRobotVersion(); + + if (robot_version->major > 5) { + RCLCPP_WARN(rclcpp::get_logger("robot_state_helper"), "Running on a PolyScopeX robot. The dashboard server is not " + "available, therefore the robot_state_helper cannot start " + "PolyScope programs and restart the safety."); + } else { + // Service to restart safety + restart_safety_srv_ = node_->create_client( + "dashboard_client/restart_safety", rmw_qos_profile_services_default, service_cb_grp_); + // Service to start UR program execution on the robot + play_program_srv_ = node_->create_client("dashboard_client/play", rmw_qos_profile_services_default, service_cb_grp_); - // Service to power on the robot - power_on_srv_ = node_->create_client("dashboard_client/power_on", - rmw_qos_profile_services_default, service_cb_grp_); - // Service to power off the robot - power_off_srv_ = node_->create_client("dashboard_client/power_off", - rmw_qos_profile_services_default, service_cb_grp_); - // Service to release the robot's brakes - brake_release_srv_ = node_->create_client("dashboard_client/brake_release", - rmw_qos_profile_services_default, service_cb_grp_); - // Service to stop UR program execution on the robot - stop_program_srv_ = node_->create_client("dashboard_client/stop", - rmw_qos_profile_services_default, service_cb_grp_); - // Service to start UR program execution on the robot - play_program_srv_ = node_->create_client("dashboard_client/play", - rmw_qos_profile_services_default, service_cb_grp_); - play_program_srv_->wait_for_service(); + play_program_srv_->wait_for_service(); + } resend_robot_program_srv_ = node_->create_client( "io_and_status_controller/resend_robot_program", rmw_qos_profile_services_default, service_cb_grp_); @@ -136,7 +132,13 @@ bool RobotStateHelper::recoverFromSafety() { switch (safety_mode_) { case urcl::SafetyMode::PROTECTIVE_STOP: - return safeDashboardTrigger(this->unlock_protective_stop_srv_); + try { + primary_client_->commandUnlockProtectiveStop(); + } catch (const urcl::UrException& e) { + RCLCPP_WARN_STREAM(rclcpp::get_logger("robot_state_helper"), e.what()); + return false; + } + return true; case urcl::SafetyMode::SYSTEM_EMERGENCY_STOP:; case urcl::SafetyMode::ROBOT_EMERGENCY_STOP: RCLCPP_WARN_STREAM(rclcpp::get_logger("robot_state_helper"), "The robot is currently in safety mode." @@ -145,7 +147,11 @@ bool RobotStateHelper::recoverFromSafety() return false; case urcl::SafetyMode::VIOLATION:; case urcl::SafetyMode::FAULT: - return safeDashboardTrigger(this->restart_safety_srv_); + if (restart_safety_srv_ != nullptr) { + return safeDashboardTrigger(this->restart_safety_srv_); + } else { + return false; + } default: // nothing to do RCLCPP_DEBUG_STREAM(rclcpp::get_logger("robot_state_helper"), "No safety recovery needed."); @@ -155,15 +161,22 @@ bool RobotStateHelper::recoverFromSafety() bool RobotStateHelper::jumpToRobotMode(const urcl::RobotMode target_mode) { - switch (target_mode) { - case urcl::RobotMode::POWER_OFF: - return safeDashboardTrigger(this->power_off_srv_); - case urcl::RobotMode::IDLE: - return safeDashboardTrigger(this->power_on_srv_); - case urcl::RobotMode::RUNNING: - return safeDashboardTrigger(this->brake_release_srv_); - default: - RCLCPP_ERROR_STREAM(rclcpp::get_logger("robot_state_helper"), "Unreachable target robot mode."); + try { + switch (target_mode) { + case urcl::RobotMode::POWER_OFF: + primary_client_->commandPowerOff(); + return true; + case urcl::RobotMode::IDLE: + primary_client_->commandPowerOn(); + return true; + case urcl::RobotMode::RUNNING: + primary_client_->commandBrakeRelease(); + return true; + default: + RCLCPP_ERROR_STREAM(rclcpp::get_logger("robot_state_helper"), "Unreachable target robot mode."); + } + } catch (const urcl::UrException& e) { + RCLCPP_ERROR_STREAM(rclcpp::get_logger("robot_state_helper"), e.what()); } return false; } @@ -247,14 +260,13 @@ void RobotStateHelper::setModeAcceptCallback(const std::shared_ptrstop_program_srv_)) { - auto start = std::chrono::steady_clock::now(); - while (program_running_ && std::chrono::steady_clock::now() - start < std::chrono::seconds(1)) { - std::this_thread::sleep_for(std::chrono::milliseconds(2)); - return true; - } + try { + primary_client_->commandStop(); + } catch (const urcl::UrException& e) { + RCLCPP_ERROR_STREAM(rclcpp::get_logger("robot_state_helper"), e.what()); + return false; } - return false; + return true; } void RobotStateHelper::setModeExecute(const std::shared_ptr goal_handle) @@ -344,9 +356,14 @@ void RobotStateHelper::setModeExecute(const std::shared_ptrsuccess = safeDashboardTrigger(this->resend_robot_program_srv_); } else { - // The dashboard denies playing immediately after switching the mode to RUNNING - sleep(1); - result_->success = safeDashboardTrigger(this->play_program_srv_); + if (play_program_srv_ == nullptr) { + result_->success = false; + result_->message = "Play program service not available on this robot."; + } else { + // The dashboard denies playing immediately after switching the mode to RUNNING + sleep(1); + result_->success = safeDashboardTrigger(this->play_program_srv_); + } } } if (result_->success) { diff --git a/ur_robot_driver/src/robot_state_helper_node.cpp b/ur_robot_driver/src/robot_state_helper_node.cpp index 37bf9a902..331d7075f 100755 --- a/ur_robot_driver/src/robot_state_helper_node.cpp +++ b/ur_robot_driver/src/robot_state_helper_node.cpp @@ -27,12 +27,19 @@ // POSSIBILITY OF SUCH DAMAGE. #include "ur_robot_driver/robot_state_helper.hpp" +#include "ur_robot_driver/urcl_log_handler.hpp" int main(int argc, char** argv) { rclcpp::init(argc, argv); rclcpp::Node::SharedPtr node = rclcpp::Node::make_shared("robot_state_helper"); - ur_robot_driver::RobotStateHelper state_helper(node); + ur_robot_driver::registerUrclLogHandler(""); // Set empty tf_prefix at the moment + std::shared_ptr robot_state_helper; + try { + robot_state_helper = std::make_shared(node); + } catch (const urcl::UrException& e) { + RCLCPP_ERROR(rclcpp::get_logger("robot_state_helper"), "%s", e.what()); + } rclcpp::executors::MultiThreadedExecutor executor; executor.add_node(node); From 72c0b03716e51307254755d8cc16ba96f3e79039 Mon Sep 17 00:00:00 2001 From: "mergify[bot]" <37929162+mergify[bot]@users.noreply.github.com> Date: Mon, 7 Apr 2025 12:03:53 +0200 Subject: [PATCH 060/133] Start executing passthrough trajectories earlier than all points are transferred. (backport of #1313) (#1335) * 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. Co-authored-by: Felix Exner --- .../passthrough_trajectory_controller.hpp | 2 + .../src/passthrough_trajectory_controller.cpp | 17 ++- .../ur_robot_driver/hardware_interface.hpp | 4 +- ur_robot_driver/src/hardware_interface.cpp | 141 ++++++++++++------ 4 files changed, 113 insertions(+), 51 deletions(-) diff --git a/ur_controllers/include/ur_controllers/passthrough_trajectory_controller.hpp b/ur_controllers/include/ur_controllers/passthrough_trajectory_controller.hpp index 3d9c785be..8e34503a1 100644 --- a/ur_controllers/include/ur_controllers/passthrough_trajectory_controller.hpp +++ b/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> scaling_state_interface_; std::optional> abort_command_interface_; + std::optional> trajectory_size_command_interface_; std::optional> transfer_command_interface_; std::optional> time_from_start_command_interface_; diff --git a/ur_controllers/src/passthrough_trajectory_controller.cpp b/ur_controllers/src/passthrough_trajectory_controller.cpp index dc2d90dae..62776e86b 100644 --- a/ur_controllers/src/passthrough_trajectory_controller.cpp +++ b/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,8 @@ 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)); - transfer_command_interface_->get().set_value(TRANSFER_STATE_WAITING_FOR_POINT); + transfer_command_interface_->get().set_value(TRANSFER_STATE_NEW_TRAJECTORY); + trajectory_size_command_interface_->get().set_value(static_cast(active_joint_traj_.points.size())); } auto active_goal_time_tol = goal_time_tolerance_.readFromRT(); auto joint_mapping = joint_trajectory_mapping_.readFromRT(); diff --git a/ur_robot_driver/include/ur_robot_driver/hardware_interface.hpp b/ur_robot_driver/include/ur_robot_driver/hardware_interface.hpp index 845720fec..87e2fd59f 100644 --- a/ur_robot_driver/include/ur_robot_driver/hardware_interface.hpp +++ b/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> accelerations); - bool has_velocities(std::vector> velocities); + bool is_valid_joint_information(std::vector> 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_; diff --git a/ur_robot_driver/src/hardware_interface.cpp b/ur_robot_driver/src/hardware_interface.cpp index 1286e773a..dc9f28ec9 100644 --- a/ur_robot_driver/src/hardware_interface.cpp +++ b/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 #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 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), @@ -1335,59 +1340,104 @@ 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(rclcpp::get_logger("URPositionHardwareInterface"), "Got a new trajectory with %lu points.", + static_cast(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(ur_driver_->getControlFrequency()) || + 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(rclcpp::get_logger("URPositionHardwareInterface"), "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(rclcpp::get_logger("URPositionHardwareInterface"), "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 +1452,9 @@ void URPositionHardwareInterface::trajectory_done_callback(urcl::control::Trajec return; } -bool URPositionHardwareInterface::has_velocities(std::vector> velocities) -{ - return (velocities.size() > 0); -} - -bool URPositionHardwareInterface::has_accelerations(std::vector> accelerations) +bool URPositionHardwareInterface::is_valid_joint_information(std::vector> data) { - return (accelerations.size() > 0); + return (data.size() > 0 && !std::isnan(data[0][0])); } } // namespace ur_robot_driver From 9be488005f1c61e5d11f28ef5fd2eeb38a790926 Mon Sep 17 00:00:00 2001 From: Felix Exner Date: Thu, 10 Apr 2025 11:54:40 +0200 Subject: [PATCH 061/133] Update changelogs for upcoming release --- ur/CHANGELOG.rst | 3 +++ ur_bringup/CHANGELOG.rst | 5 +++++ ur_calibration/CHANGELOG.rst | 5 +++++ ur_controllers/CHANGELOG.rst | 6 ++++++ ur_dashboard_msgs/CHANGELOG.rst | 3 +++ ur_moveit_config/CHANGELOG.rst | 5 +++++ ur_robot_driver/CHANGELOG.rst | 9 +++++++++ 7 files changed, 36 insertions(+) diff --git a/ur/CHANGELOG.rst b/ur/CHANGELOG.rst index 32970a6cd..9e12c1f41 100644 --- a/ur/CHANGELOG.rst +++ b/ur/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package ur ^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 2.6.0 (2025-03-17) ------------------ diff --git a/ur_bringup/CHANGELOG.rst b/ur_bringup/CHANGELOG.rst index 1edeb6054..94467424b 100644 --- a/ur_bringup/CHANGELOG.rst +++ b/ur_bringup/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package ur_bringup ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Add support for UR7e and UR12e (`#1332 `_) +* Contributors: mergify[bot] + 2.6.0 (2025-03-17) ------------------ * Fix doc links (`#1247 `_) diff --git a/ur_calibration/CHANGELOG.rst b/ur_calibration/CHANGELOG.rst index 0915682dd..545e61fd9 100644 --- a/ur_calibration/CHANGELOG.rst +++ b/ur_calibration/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package ur_calibration ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Use modern CMake to link against yaml-cpp (backport of `#1295 `_) (`#1304 `_) +* Contributors: mergify[bot] + 2.6.0 (2025-03-17) ------------------ diff --git a/ur_controllers/CHANGELOG.rst b/ur_controllers/CHANGELOG.rst index 8c62ebfe4..815781b64 100644 --- a/ur_controllers/CHANGELOG.rst +++ b/ur_controllers/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package ur_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Start executing passthrough trajectories earlier than all points are transferred. (backport of `#1313 `_) (`#1335 `_) +* Fix passthrough controller to not read non-existing state_interfaces (`#1314 `_) (`#1316 `_) +* Contributors: mergify[bot] + 2.6.0 (2025-03-17) ------------------ * ur_controllers: doc -- Fix link to index page of driver (backport of `#1284 `_) (`#1285 `_) diff --git a/ur_dashboard_msgs/CHANGELOG.rst b/ur_dashboard_msgs/CHANGELOG.rst index 75cf38339..96da3079c 100644 --- a/ur_dashboard_msgs/CHANGELOG.rst +++ b/ur_dashboard_msgs/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package ur_dashboard_msgs ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 2.6.0 (2025-03-17) ------------------ * Port robot_state_helper to ROS2 (backport of `#933 `_) (`#1286 `_) diff --git a/ur_moveit_config/CHANGELOG.rst b/ur_moveit_config/CHANGELOG.rst index 884b01ad0..3d930b086 100644 --- a/ur_moveit_config/CHANGELOG.rst +++ b/ur_moveit_config/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package ur_moveit_config ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Add support for UR7e and UR12e (`#1332 `_) +* Contributors: mergify[bot] + 2.6.0 (2025-03-17) ------------------ diff --git a/ur_robot_driver/CHANGELOG.rst b/ur_robot_driver/CHANGELOG.rst index f4e325129..91d743a9b 100644 --- a/ur_robot_driver/CHANGELOG.rst +++ b/ur_robot_driver/CHANGELOG.rst @@ -1,3 +1,12 @@ +Forthcoming +----------- +* Start executing passthrough trajectories earlier than all points are transferred. (backport of `#1313 `_) (`#1335 `_) +* Support PolyScopeX robots (backport of `#1318 `_) (`#1333 `_) +* Add support for UR7e and UR12e (`#1332 `_) +* Use UrDriverConfig struct to initialize UrDriver (backport of `#1328 `_) (`#1330 `_) +* Fix passthrough controller to not read non-existing state_interfaces (`#1314 `_) (`#1316 `_) +* Contributors: mergify[bot] + 2.6.0 (2025-03-17) ------------------ * Update transformForceTorque to handle whether it is a cb3 or an e-Series robot (backport of `#1287 `_) (`#1299 `_) From 4b94c97a74977f0aa3727b0509c56c9400ef053a Mon Sep 17 00:00:00 2001 From: Felix Exner Date: Thu, 10 Apr 2025 11:54:53 +0200 Subject: [PATCH 062/133] 2.7.0 --- ur/CHANGELOG.rst | 4 ++-- ur/package.xml | 2 +- ur_bringup/CHANGELOG.rst | 4 ++-- ur_bringup/package.xml | 2 +- ur_calibration/CHANGELOG.rst | 4 ++-- ur_calibration/package.xml | 2 +- ur_controllers/CHANGELOG.rst | 4 ++-- ur_controllers/package.xml | 2 +- ur_dashboard_msgs/CHANGELOG.rst | 4 ++-- ur_dashboard_msgs/package.xml | 2 +- ur_moveit_config/CHANGELOG.rst | 4 ++-- ur_moveit_config/package.xml | 2 +- ur_robot_driver/CHANGELOG.rst | 4 ++-- ur_robot_driver/package.xml | 2 +- 14 files changed, 21 insertions(+), 21 deletions(-) diff --git a/ur/CHANGELOG.rst b/ur/CHANGELOG.rst index 9e12c1f41..3ccb800fc 100644 --- a/ur/CHANGELOG.rst +++ b/ur/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package ur ^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.7.0 (2025-04-10) +------------------ 2.6.0 (2025-03-17) ------------------ diff --git a/ur/package.xml b/ur/package.xml index 2a7a97172..84801aae6 100644 --- a/ur/package.xml +++ b/ur/package.xml @@ -2,7 +2,7 @@ ur - 2.6.0 + 2.7.0 Metapackage for universal robots Felix Exner diff --git a/ur_bringup/CHANGELOG.rst b/ur_bringup/CHANGELOG.rst index 94467424b..6c0177c1b 100644 --- a/ur_bringup/CHANGELOG.rst +++ b/ur_bringup/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package ur_bringup ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.7.0 (2025-04-10) +------------------ * Add support for UR7e and UR12e (`#1332 `_) * Contributors: mergify[bot] diff --git a/ur_bringup/package.xml b/ur_bringup/package.xml index f6c94f8b6..6a7e9c55a 100644 --- a/ur_bringup/package.xml +++ b/ur_bringup/package.xml @@ -2,7 +2,7 @@ ur_bringup - 2.6.0 + 2.7.0 Launch file and run-time configurations, e.g. controllers. Felix Exner diff --git a/ur_calibration/CHANGELOG.rst b/ur_calibration/CHANGELOG.rst index 545e61fd9..1986b52f3 100644 --- a/ur_calibration/CHANGELOG.rst +++ b/ur_calibration/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package ur_calibration ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.7.0 (2025-04-10) +------------------ * Use modern CMake to link against yaml-cpp (backport of `#1295 `_) (`#1304 `_) * Contributors: mergify[bot] diff --git a/ur_calibration/package.xml b/ur_calibration/package.xml index 2bf3dbfe0..42a9eb29d 100644 --- a/ur_calibration/package.xml +++ b/ur_calibration/package.xml @@ -1,7 +1,7 @@ ur_calibration - 2.6.0 + 2.7.0 Package for extracting the factory calibration from a UR robot and change it such that it can be used by ur_description to gain a correct URDF Felix Exner diff --git a/ur_controllers/CHANGELOG.rst b/ur_controllers/CHANGELOG.rst index 815781b64..13ae58dcd 100644 --- a/ur_controllers/CHANGELOG.rst +++ b/ur_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package ur_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.7.0 (2025-04-10) +------------------ * Start executing passthrough trajectories earlier than all points are transferred. (backport of `#1313 `_) (`#1335 `_) * Fix passthrough controller to not read non-existing state_interfaces (`#1314 `_) (`#1316 `_) * Contributors: mergify[bot] diff --git a/ur_controllers/package.xml b/ur_controllers/package.xml index b0948f78b..19eda47d5 100644 --- a/ur_controllers/package.xml +++ b/ur_controllers/package.xml @@ -2,7 +2,7 @@ ur_controllers - 2.6.0 + 2.7.0 Provides controllers that use the speed scaling interface of Universal Robots. Felix Exner diff --git a/ur_dashboard_msgs/CHANGELOG.rst b/ur_dashboard_msgs/CHANGELOG.rst index 96da3079c..0b2f985f6 100644 --- a/ur_dashboard_msgs/CHANGELOG.rst +++ b/ur_dashboard_msgs/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package ur_dashboard_msgs ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.7.0 (2025-04-10) +------------------ 2.6.0 (2025-03-17) ------------------ diff --git a/ur_dashboard_msgs/package.xml b/ur_dashboard_msgs/package.xml index f6effe872..8af37e620 100644 --- a/ur_dashboard_msgs/package.xml +++ b/ur_dashboard_msgs/package.xml @@ -2,7 +2,7 @@ ur_dashboard_msgs - 2.6.0 + 2.7.0 Messages around the UR Dashboard server. Felix Exner diff --git a/ur_moveit_config/CHANGELOG.rst b/ur_moveit_config/CHANGELOG.rst index 3d930b086..c8ab625ac 100644 --- a/ur_moveit_config/CHANGELOG.rst +++ b/ur_moveit_config/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package ur_moveit_config ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.7.0 (2025-04-10) +------------------ * Add support for UR7e and UR12e (`#1332 `_) * Contributors: mergify[bot] diff --git a/ur_moveit_config/package.xml b/ur_moveit_config/package.xml index a03e9943e..514449019 100644 --- a/ur_moveit_config/package.xml +++ b/ur_moveit_config/package.xml @@ -2,7 +2,7 @@ ur_moveit_config - 2.6.0 + 2.7.0 An example package with MoveIt2 configurations for UR robots. diff --git a/ur_robot_driver/CHANGELOG.rst b/ur_robot_driver/CHANGELOG.rst index 91d743a9b..a67312b3b 100644 --- a/ur_robot_driver/CHANGELOG.rst +++ b/ur_robot_driver/CHANGELOG.rst @@ -1,5 +1,5 @@ -Forthcoming ------------ +2.7.0 (2025-04-10) +------------------ * Start executing passthrough trajectories earlier than all points are transferred. (backport of `#1313 `_) (`#1335 `_) * Support PolyScopeX robots (backport of `#1318 `_) (`#1333 `_) * Add support for UR7e and UR12e (`#1332 `_) diff --git a/ur_robot_driver/package.xml b/ur_robot_driver/package.xml index 8953d242b..0ff74f09a 100644 --- a/ur_robot_driver/package.xml +++ b/ur_robot_driver/package.xml @@ -2,7 +2,7 @@ ur_robot_driver - 2.6.0 + 2.7.0 The new driver for Universal Robots UR3, UR5 and UR10 robots with CB3 controllers and the e-series. Felix Exner From fe78fb4da152460613036aa2c581bdb48f9ee118 Mon Sep 17 00:00:00 2001 From: "mergify[bot]" <37929162+mergify[bot]@users.noreply.github.com> Date: Tue, 15 Apr 2025 15:37:51 +0200 Subject: [PATCH 063/133] Added controller to enable and disable tool contact (backport of #940) (#1337) * This adds a new controller to enable tool contact mode on the robot. * Only append to start_modes in prepare_switch --- ur_controllers/CMakeLists.txt | 8 + ur_controllers/controller_plugins.xml | 5 + ur_controllers/doc/index.rst | 44 +++ .../tool_contact_controller.hpp | 135 +++++++ .../src/tool_contact_controller.cpp | 345 ++++++++++++++++++ .../tool_contact_controller_parameters.yaml | 11 + ur_robot_driver/CMakeLists.txt | 4 + ur_robot_driver/config/ur_controllers.yaml | 7 + .../ur_robot_driver/hardware_interface.hpp | 10 + ur_robot_driver/launch/ur_control.launch.py | 1 + ur_robot_driver/src/hardware_interface.cpp | 209 ++++++++--- .../integration_test_controller_switch.py | 89 +++++ .../test/integration_test_tool_contact.py | 162 ++++++++ ur_robot_driver/test/test_common.py | 18 +- 14 files changed, 998 insertions(+), 50 deletions(-) create mode 100644 ur_controllers/include/ur_controllers/tool_contact_controller.hpp create mode 100644 ur_controllers/src/tool_contact_controller.cpp create mode 100644 ur_controllers/src/tool_contact_controller_parameters.yaml create mode 100644 ur_robot_driver/test/integration_test_tool_contact.py diff --git a/ur_controllers/CMakeLists.txt b/ur_controllers/CMakeLists.txt index 79a89acbd..4d172d22b 100644 --- a/ur_controllers/CMakeLists.txt +++ b/ur_controllers/CMakeLists.txt @@ -51,6 +51,11 @@ set(THIS_PACKAGE_INCLUDE_DEPENDS include_directories(include) +generate_parameter_library( + tool_contact_controller_parameters + src/tool_contact_controller_parameters.yaml +) + generate_parameter_library( force_mode_controller_parameters src/force_mode_controller_parameters.yaml @@ -87,6 +92,7 @@ generate_parameter_library( ) add_library(${PROJECT_NAME} SHARED + src/tool_contact_controller.cpp src/force_mode_controller.cpp src/scaled_joint_trajectory_controller.cpp src/speed_scaling_state_broadcaster.cpp @@ -99,6 +105,7 @@ target_include_directories(${PROJECT_NAME} PRIVATE include ) target_link_libraries(${PROJECT_NAME} + tool_contact_controller_parameters force_mode_controller_parameters gpio_controller_parameters speed_scaling_state_broadcaster_parameters @@ -107,6 +114,7 @@ target_link_libraries(${PROJECT_NAME} passthrough_trajectory_controller_parameters ur_configuration_controller_parameters ) + ament_target_dependencies(${PROJECT_NAME} ${THIS_PACKAGE_INCLUDE_DEPENDS} ) diff --git a/ur_controllers/controller_plugins.xml b/ur_controllers/controller_plugins.xml index ec15809a1..f4c916aa3 100644 --- a/ur_controllers/controller_plugins.xml +++ b/ur_controllers/controller_plugins.xml @@ -34,4 +34,9 @@ Controller used to get and change the configuration of the robot + + + Controller to use the tool contact functionality of the robot. + + diff --git a/ur_controllers/doc/index.rst b/ur_controllers/doc/index.rst index b33350889..06ae93491 100644 --- a/ur_controllers/doc/index.rst +++ b/ur_controllers/doc/index.rst @@ -11,6 +11,7 @@ robot family. Currently this contains: but it uses the speed scaling reported to align progress of the trajectory between the robot and controller. * A **io_and_status_controller** that allows setting I/O ports, controlling some UR-specific functionality and publishes status information about the robot. +* A **tool_contact_controller** that exposes an action to enable the tool contact function on the robot. About this package ------------------ @@ -378,3 +379,46 @@ The controller provides the ``~/enable_freedrive_mode`` topic of type ``[std_msg * to deactivate freedrive mode is enough to publish a ``False`` msg on the indicated topic or to deactivate the controller or to stop publishing ``True`` on the enable topic and wait for the controller timeout. + +.. _tool_contact_controller: + +ur_controllers/ToolContactController +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +This controller can enable tool contact on the robot. When tool contact is enabled, and the robot +senses whether the tool has made contact with something. When that happens, it will stop all +motion, and retract to where it first sensed the contact. + +This controller can be used with any of the motion controllers. + +The controller is not a direct representation of the URScript function `tool_contact(direction) +`_, +as it does not allow for choosing the direction. The direction of tool contact will always be the +current TCP direction of movement. + +Parameters +"""""""""" + ++-------------------------+--------+---------------+---------------------------------------------------------------------------------------+ +| Parameter name | Type | Default value | Description | +| | | | | ++-------------------------+--------+---------------+---------------------------------------------------------------------------------------+ +| ``tf_prefix`` | string | | Urdf prefix of the corresponding arm | ++-------------------------+--------+---------------+---------------------------------------------------------------------------------------+ +| ``action_monitor_rate`` | double | 20.0 | The rate at which the action should be monitored in Hz. | ++-------------------------+--------+---------------+---------------------------------------------------------------------------------------+ + +Action interface / usage +"""""""""""""""""""""""" +The controller provides one action for enabling tool contact. For the controller to accept action goals it needs to be in ``active`` state. + +* ``~/detect_tool_contact [ur_msgs/action/ToolContact]`` + + The action definition of ``ur_msgs/action/ToolContact`` has no fields, as a call to the action implicitly means that tool contact should be enabled. + The result of the action is available through the status of the action itself. If the action succeeds it means that tool contact was detected, otherwise tool contact will remain active until it is either cancelled by the user, or aborted by the hardware. + The action provides no feedback. + + The action can be called from the command line using the following command, when the controller is active: + + .. code-block:: + + ros2 action send_goal /tool_contact_controller/detect_tool_contact ur_msgs/action/ToolContact diff --git a/ur_controllers/include/ur_controllers/tool_contact_controller.hpp b/ur_controllers/include/ur_controllers/tool_contact_controller.hpp new file mode 100644 index 000000000..809628e13 --- /dev/null +++ b/ur_controllers/include/ur_controllers/tool_contact_controller.hpp @@ -0,0 +1,135 @@ +// 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-07 + * + * + * + * + */ +//---------------------------------------------------------------------- + +#ifndef UR_CONTROLLERS__TOOL_CONTACT_CONTROLLER_HPP_ +#define UR_CONTROLLERS__TOOL_CONTACT_CONTROLLER_HPP_ + +#include +#include +#include + +#include +#include "std_msgs/msg/bool.hpp" +#include +#include +#include +#include +#include + +#include +#include + +#include +#include "ur_controllers/tool_contact_controller_parameters.hpp" + +namespace ur_controllers +{ +class ToolContactController : public controller_interface::ControllerInterface +{ +public: + ToolContactController() = default; + ~ToolContactController() override = default; + + controller_interface::CallbackReturn on_init() override; + + controller_interface::InterfaceConfiguration command_interface_configuration() const override; + + controller_interface::InterfaceConfiguration state_interface_configuration() const override; + + controller_interface::CallbackReturn on_configure(const rclcpp_lifecycle::State& previous_state) override; + + controller_interface::CallbackReturn on_activate(const rclcpp_lifecycle::State& previous_state) override; + + controller_interface::CallbackReturn on_deactivate(const rclcpp_lifecycle::State& previous_state) override; + + controller_interface::CallbackReturn on_shutdown(const rclcpp_lifecycle::State& previous_state) override; + + controller_interface::return_type update(const rclcpp::Time& time, const rclcpp::Duration& period) override; + +private: + using RealtimeGoalHandle = realtime_tools::RealtimeServerGoalHandle; + using RealtimeGoalHandlePtr = std::shared_ptr; + using RealtimeGoalHandleBuffer = realtime_tools::RealtimeBuffer; + + RealtimeGoalHandleBuffer rt_active_goal_; ///< Currently active action goal, if any. + ur_msgs::action::ToolContact::Feedback::SharedPtr feedback_; ///< preallocated feedback + rclcpp::TimerBase::SharedPtr goal_handle_timer_; ///< Timer to frequently check on the running goal + + // non-rt function that will be called with action_monitor_period to monitor the rt action + rclcpp::Duration action_monitor_period_ = rclcpp::Duration::from_seconds(0.05); + void action_handler(); + + rclcpp_action::GoalResponse goal_received_callback(const rclcpp_action::GoalUUID& /*uuid*/, + std::shared_ptr goal); + + void + goal_accepted_callback(std::shared_ptr> goal_handle); + + rclcpp_action::CancelResponse goal_canceled_callback( + const std::shared_ptr> goal_handle); + + std::atomic tool_contact_enable_ = false; + std::atomic tool_contact_active_ = false; + std::atomic tool_contact_abort_ = false; + std::atomic change_requested_ = false; + std::atomic logged_once_ = false; + std::atomic should_reset_goal = false; + + std::optional> tool_contact_result_interface_; + std::optional> major_version_state_interface_; + std::optional> tool_contact_state_interface_; + std::optional> tool_contact_set_state_interface_; + + rclcpp_action::Server::SharedPtr tool_contact_action_server_; + + std::shared_ptr tool_contact_param_listener_; + tool_contact_controller::Params tool_contact_params_; + + static constexpr double TOOL_CONTACT_STANDBY = 1.0; + static constexpr double TOOL_CONTACT_WAITING_BEGIN = 2.0; + static constexpr double TOOL_CONTACT_EXECUTING = 3.0; + static constexpr double TOOL_CONTACT_FAILURE_BEGIN = 4.0; + static constexpr double TOOL_CONTACT_WAITING_END = 5.0; + static constexpr double TOOL_CONTACT_SUCCESS_END = 6.0; + static constexpr double TOOL_CONTACT_FAILURE_END = 7.0; +}; +} // namespace ur_controllers + +#endif // UR_CONTROLLERS__TOOL_CONTACT_CONTROLLER_HPP_ diff --git a/ur_controllers/src/tool_contact_controller.cpp b/ur_controllers/src/tool_contact_controller.cpp new file mode 100644 index 000000000..efa9b76d1 --- /dev/null +++ b/ur_controllers/src/tool_contact_controller.cpp @@ -0,0 +1,345 @@ +// 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-07 + * + * + * + * + */ +//---------------------------------------------------------------------- + +#include +#include +#include +#include +#include + +namespace ur_controllers +{ + +controller_interface::CallbackReturn ToolContactController::on_init() +{ + tool_contact_param_listener_ = std::make_shared(get_node()); + tool_contact_params_ = tool_contact_param_listener_->get_params(); + feedback_ = std::make_shared(); + return controller_interface::CallbackReturn::SUCCESS; +} + +controller_interface::CallbackReturn +ToolContactController::on_configure(const rclcpp_lifecycle::State& /* previous_state */) +{ + tool_contact_action_server_ = rclcpp_action::create_server( + get_node(), std::string(get_node()->get_name()) + "/detect_tool_contact", + std::bind(&ToolContactController::goal_received_callback, this, std::placeholders::_1, std::placeholders::_2), + std::bind(&ToolContactController::goal_canceled_callback, this, std::placeholders::_1), + std::bind(&ToolContactController::goal_accepted_callback, this, std::placeholders::_1)); + + return controller_interface::CallbackReturn::SUCCESS; +} + +controller_interface::InterfaceConfiguration ToolContactController::command_interface_configuration() const +{ + controller_interface::InterfaceConfiguration config; + config.type = controller_interface::interface_configuration_type::INDIVIDUAL; + const std::string tf_prefix = tool_contact_params_.tf_prefix; + + config.names.emplace_back(tf_prefix + "tool_contact/tool_contact_set_state"); + return config; +} + +controller_interface::InterfaceConfiguration ToolContactController::state_interface_configuration() const +{ + controller_interface::InterfaceConfiguration config; + config.type = controller_interface::interface_configuration_type::INDIVIDUAL; + + const std::string tf_prefix = tool_contact_params_.tf_prefix; + config.names.emplace_back(tf_prefix + "tool_contact/tool_contact_result"); + config.names.emplace_back(tf_prefix + "get_robot_software_version/get_version_major"); + config.names.emplace_back(tf_prefix + "tool_contact/tool_contact_state"); + return config; +} + +controller_interface::CallbackReturn +ToolContactController::on_activate(const rclcpp_lifecycle::State& /* previous_state */) +{ + { + const std::string interface_name = tool_contact_params_.tf_prefix + "tool_contact/tool_contact_state"; + auto it = std::find_if(state_interfaces_.begin(), state_interfaces_.end(), + [&](auto& interface) { return (interface.get_name() == interface_name); }); + if (it != state_interfaces_.end()) { + tool_contact_state_interface_ = *it; + } else { + RCLCPP_ERROR(get_node()->get_logger(), "Did not find '%s' in state interfaces.", interface_name.c_str()); + return controller_interface::CallbackReturn::ERROR; + } + } + { + const std::string interface_name = tool_contact_params_.tf_prefix + "tool_contact/tool_contact_set_state"; + auto it = std::find_if(command_interfaces_.begin(), command_interfaces_.end(), + [&](auto& interface) { return (interface.get_name() == interface_name); }); + if (it != command_interfaces_.end()) { + tool_contact_set_state_interface_ = *it; + tool_contact_set_state_interface_->get().set_value(TOOL_CONTACT_STANDBY); + } 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 interface_name = tool_contact_params_.tf_prefix + "tool_contact/tool_contact_result"; + auto it = std::find_if(state_interfaces_.begin(), state_interfaces_.end(), + [&](auto& interface) { return (interface.get_name() == interface_name); }); + if (it != state_interfaces_.end()) { + tool_contact_result_interface_ = *it; + } else { + RCLCPP_ERROR(get_node()->get_logger(), "Did not find '%s' in state interfaces.", interface_name.c_str()); + return controller_interface::CallbackReturn::ERROR; + } + } + { + const std::string interface_name = tool_contact_params_.tf_prefix + "get_robot_software_version/get_version_major"; + auto it = std::find_if(state_interfaces_.begin(), state_interfaces_.end(), + [&](auto& interface) { return (interface.get_name() == interface_name); }); + if (it != state_interfaces_.end()) { + major_version_state_interface_ = *it; + double major_version = major_version_state_interface_->get().get_value(); + if (major_version < 5) { + RCLCPP_ERROR(get_node()->get_logger(), "This feature is not supported on CB3 robots, controller will not be " + "started."); + return controller_interface::CallbackReturn::ERROR; + } + } else { + RCLCPP_ERROR(get_node()->get_logger(), "Did not find '%s' in state interfaces.", interface_name.c_str()); + return controller_interface::CallbackReturn::ERROR; + } + } + + action_monitor_period_ = rclcpp::Duration(rclcpp::Rate(tool_contact_params_.action_monitor_rate).period()); + return controller_interface::CallbackReturn::SUCCESS; +} + +controller_interface::CallbackReturn +ToolContactController::on_deactivate(const rclcpp_lifecycle::State& /* previous_state */) +{ + // Abort active goal (if any) + const auto active_goal = *rt_active_goal_.readFromRT(); + if (active_goal) { + RCLCPP_INFO(get_node()->get_logger(), "Aborting tool contact, as controller has been deactivated."); + // Mark the current goal as abort + auto result = std::make_shared(); + active_goal->setAborted(result); + should_reset_goal = true; + } + if (tool_contact_active_) { + tool_contact_active_ = false; + } + + return controller_interface::CallbackReturn::SUCCESS; +} + +controller_interface::CallbackReturn ToolContactController::on_shutdown(const rclcpp_lifecycle::State& + /* previous_state */) +{ + tool_contact_action_server_.reset(); + return controller_interface::CallbackReturn::SUCCESS; +} + +rclcpp_action::GoalResponse ToolContactController::goal_received_callback( + const rclcpp_action::GoalUUID& /*uuid*/, std::shared_ptr /* goal */) +{ + RCLCPP_INFO(get_node()->get_logger(), "New goal received."); + + if (get_state().id() != lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) { + RCLCPP_ERROR(get_node()->get_logger(), "Tool contact controller is not in active state, can not accept action " + "goals."); + return rclcpp_action::GoalResponse::REJECT; + } + + const auto active_goal = *rt_active_goal_.readFromNonRT(); + if (active_goal) { + RCLCPP_ERROR(get_node()->get_logger(), "Tool contact already active, rejecting goal."); + return rclcpp_action::GoalResponse::REJECT; + } + return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; +} + +void ToolContactController::goal_accepted_callback( + std::shared_ptr> goal_handle) +{ + RCLCPP_INFO(get_node()->get_logger(), "Goal accepted."); + tool_contact_enable_ = true; + tool_contact_abort_ = false; + RealtimeGoalHandlePtr rt_goal = std::make_shared(goal_handle); + rt_goal->execute(); + rt_active_goal_.writeFromNonRT(rt_goal); + goal_handle_timer_.reset(); + auto period = std::chrono::duration_cast( + std::chrono::nanoseconds(action_monitor_period_.nanoseconds())); + goal_handle_timer_ = get_node()->create_wall_timer(period, std::bind(&ToolContactController::action_handler, this)); + return; +} + +void ToolContactController::action_handler() +{ + const auto active_goal = *rt_active_goal_.readFromNonRT(); + if (active_goal) { + // Allow the goal to handle any actions it needs to perform + active_goal->runNonRealtime(); + // If one of the goal ending conditions were met, reset our active goal pointer + if (should_reset_goal) { + rt_active_goal_.writeFromNonRT(RealtimeGoalHandlePtr()); + should_reset_goal = false; + } + } +} + +rclcpp_action::CancelResponse ToolContactController::goal_canceled_callback( + const std::shared_ptr> goal_handle) +{ + // Check that cancel request refers to currently active goal (if any) + const auto active_goal = *rt_active_goal_.readFromNonRT(); + if (active_goal && active_goal->gh_ == goal_handle) { + RCLCPP_INFO(get_node()->get_logger(), "Cancel tool contact requested."); + + // Mark the current goal as canceled + auto result = std::make_shared(); + active_goal->setCanceled(result); + should_reset_goal = true; + tool_contact_abort_ = true; + tool_contact_enable_ = false; + } + + return rclcpp_action::CancelResponse::ACCEPT; +} + +controller_interface::return_type ToolContactController::update(const rclcpp::Time& /* time */, + const rclcpp::Duration& /* period */) +{ + // Abort takes priority + if (tool_contact_abort_) { + tool_contact_abort_ = false; + tool_contact_enable_ = false; + tool_contact_set_state_interface_->get().set_value(TOOL_CONTACT_WAITING_END); + } else if (tool_contact_enable_) { + tool_contact_enable_ = false; + tool_contact_set_state_interface_->get().set_value(TOOL_CONTACT_WAITING_BEGIN); + } + + const auto active_goal = *rt_active_goal_.readFromRT(); + const int state = static_cast(tool_contact_state_interface_->get().get_value()); + + switch (state) { + case static_cast(TOOL_CONTACT_EXECUTING): + { + tool_contact_active_ = true; + if (!logged_once_) { + RCLCPP_INFO(get_node()->get_logger(), "Tool contact enabled successfully."); + logged_once_ = true; + } + double result = tool_contact_result_interface_->get().get_value(); + if (result == 0.0) { + tool_contact_active_ = false; + RCLCPP_INFO(get_node()->get_logger(), "Tool contact finished successfully."); + + tool_contact_set_state_interface_->get().set_value(TOOL_CONTACT_WAITING_END); + if (active_goal) { + auto result = std::make_shared(); + active_goal->setSucceeded(result); + should_reset_goal = true; + } + } else if (result == 1.0) { + tool_contact_active_ = false; + RCLCPP_ERROR(get_node()->get_logger(), "Tool contact aborted by hardware."); + + tool_contact_set_state_interface_->get().set_value(TOOL_CONTACT_STANDBY); + if (active_goal) { + auto result = std::make_shared(); + active_goal->setAborted(result); + should_reset_goal = true; + } + } + } break; + + case static_cast(TOOL_CONTACT_FAILURE_BEGIN): + { + RCLCPP_ERROR(get_node()->get_logger(), "Tool contact could not be enabled."); + tool_contact_active_ = false; + tool_contact_set_state_interface_->get().set_value(TOOL_CONTACT_STANDBY); + + if (active_goal) { + auto result = std::make_shared(); + active_goal->setAborted(result); + should_reset_goal = true; + } + } break; + + case static_cast(TOOL_CONTACT_SUCCESS_END): + { + if (tool_contact_active_) { + RCLCPP_INFO(get_node()->get_logger(), "Tool contact disabled successfully."); + tool_contact_active_ = false; + + tool_contact_set_state_interface_->get().set_value(TOOL_CONTACT_STANDBY); + } + } break; + + case static_cast(TOOL_CONTACT_FAILURE_END): + { + RCLCPP_ERROR(get_node()->get_logger(), "Tool contact could not be disabled."); + + tool_contact_set_state_interface_->get().set_value(TOOL_CONTACT_STANDBY); + + if (active_goal) { + auto result = std::make_shared(); + active_goal->setAborted(result); + should_reset_goal = true; + } + } break; + case static_cast(TOOL_CONTACT_STANDBY): + logged_once_ = false; + break; + default: + break; + } + if (active_goal) { + active_goal->setFeedback(feedback_); + } + + return controller_interface::return_type::OK; +} + +} // namespace ur_controllers + +#include "pluginlib/class_list_macros.hpp" + +PLUGINLIB_EXPORT_CLASS(ur_controllers::ToolContactController, controller_interface::ControllerInterface) diff --git a/ur_controllers/src/tool_contact_controller_parameters.yaml b/ur_controllers/src/tool_contact_controller_parameters.yaml new file mode 100644 index 000000000..9640915a6 --- /dev/null +++ b/ur_controllers/src/tool_contact_controller_parameters.yaml @@ -0,0 +1,11 @@ +tool_contact_controller: + tf_prefix: { + type: string, + default_value: "", + description: "Urdf prefix of the corresponding arm" + } + action_monitor_rate: { + type: double, + default_value: 20.0, + description: "The rate at which the action should be monitored in Hz." + } diff --git a/ur_robot_driver/CMakeLists.txt b/ur_robot_driver/CMakeLists.txt index baba5ffbd..adb8f7119 100644 --- a/ur_robot_driver/CMakeLists.txt +++ b/ur_robot_driver/CMakeLists.txt @@ -222,5 +222,9 @@ if(BUILD_TESTING) TIMEOUT 500 ) + add_launch_test(test/integration_test_tool_contact.py + TIMEOUT + 800 + ) endif() endif() diff --git a/ur_robot_driver/config/ur_controllers.yaml b/ur_robot_driver/config/ur_controllers.yaml index 5e996bd24..333069294 100644 --- a/ur_robot_driver/config/ur_controllers.yaml +++ b/ur_robot_driver/config/ur_controllers.yaml @@ -42,6 +42,9 @@ controller_manager: ur_configuration_controller: type: ur_controllers/URConfigurationController + tool_contact_controller: + type: ur_controllers/ToolContactController + speed_scaling_state_broadcaster: ros__parameters: state_publish_rate: 100.0 @@ -175,3 +178,7 @@ tcp_pose_broadcaster: pose_name: $(var tf_prefix)tcp_pose tf: child_frame_id: $(var tf_prefix)tool0_controller + +tool_contact_controller: + ros__parameters: + tf_prefix: "$(var tf_prefix)" diff --git a/ur_robot_driver/include/ur_robot_driver/hardware_interface.hpp b/ur_robot_driver/include/ur_robot_driver/hardware_interface.hpp index 87e2fd59f..3ff25ebeb 100644 --- a/ur_robot_driver/include/ur_robot_driver/hardware_interface.hpp +++ b/ur_robot_driver/include/ur_robot_driver/hardware_interface.hpp @@ -80,6 +80,7 @@ enum StoppingInterface STOP_PASSTHROUGH, STOP_FORCE_MODE, STOP_FREEDRIVE, + STOP_TOOL_CONTACT, }; // We define our own quaternion to use it as a buffer, since we need to pass pointers to the state @@ -168,6 +169,8 @@ class URPositionHardwareInterface : public hardware_interface::SystemInterface void check_passthrough_trajectory_controller(); void trajectory_done_callback(urcl::control::TrajectoryResult result); bool is_valid_joint_information(std::vector> data); + void tool_contact_callback(urcl::control::ToolContactResult); + void check_tool_contact_controller(); urcl::vector6d_t urcl_position_commands_; urcl::vector6d_t urcl_position_commands_old_; @@ -229,6 +232,12 @@ class URPositionHardwareInterface : public hardware_interface::SystemInterface double get_robot_software_version_bugfix_; double get_robot_software_version_build_; + // Tool contact controller interface values + double tool_contact_set_state_; + double tool_contact_state_; + double tool_contact_result_; + bool tool_contact_controller_running_; + // Freedrive mode controller interface values bool freedrive_activated_; bool freedrive_mode_controller_running_; @@ -306,6 +315,7 @@ class URPositionHardwareInterface : public hardware_interface::SystemInterface const std::string PASSTHROUGH_GPIO = "trajectory_passthrough"; const std::string FORCE_MODE_GPIO = "force_mode"; const std::string FREEDRIVE_MODE_GPIO = "freedrive_mode"; + const std::string TOOL_CONTACT_GPIO = "tool_contact"; }; } // namespace ur_robot_driver diff --git a/ur_robot_driver/launch/ur_control.launch.py b/ur_robot_driver/launch/ur_control.launch.py index 10d59b017..d8f2af710 100644 --- a/ur_robot_driver/launch/ur_control.launch.py +++ b/ur_robot_driver/launch/ur_control.launch.py @@ -362,6 +362,7 @@ def controller_spawner(controllers, active=True): "force_mode_controller", "passthrough_trajectory_controller", "freedrive_mode_controller", + "tool_contact_controller", ] if activate_joint_controller.perform(context) == "true": controllers_active.append(initial_joint_controller.perform(context)) diff --git a/ur_robot_driver/src/hardware_interface.cpp b/ur_robot_driver/src/hardware_interface.cpp index dc9f28ec9..1719016cc 100644 --- a/ur_robot_driver/src/hardware_interface.cpp +++ b/ur_robot_driver/src/hardware_interface.cpp @@ -83,6 +83,7 @@ URPositionHardwareInterface::on_init(const hardware_interface::HardwareInfo& sys velocity_controller_running_ = false; freedrive_mode_controller_running_ = false; passthrough_trajectory_controller_running_ = false; + tool_contact_controller_running_ = false; runtime_state_ = static_cast(rtde::RUNTIME_STATE::STOPPED); pausing_state_ = PausingState::RUNNING; pausing_ramp_up_increment_ = 0.01; @@ -95,6 +96,9 @@ URPositionHardwareInterface::on_init(const hardware_interface::HardwareInfo& sys passthrough_trajectory_transfer_state_ = 0.0; passthrough_trajectory_abort_ = 0.0; passthrough_trajectory_size_ = 0.0; + tool_contact_result_ = NO_NEW_CMD_; + tool_contact_set_state_ = 0.0; + tool_contact_state_ = 0.0; trajectory_joint_positions_.reserve(32768); trajectory_joint_velocities_.reserve(32768); trajectory_joint_accelerations_.reserve(32768); @@ -267,6 +271,12 @@ std::vector URPositionHardwareInterface::exp state_interfaces.emplace_back(hardware_interface::StateInterface( tf_prefix + "get_robot_software_version", "get_version_build", &get_robot_software_version_build_)); + state_interfaces.emplace_back( + hardware_interface::StateInterface(tf_prefix + TOOL_CONTACT_GPIO, "tool_contact_result", &tool_contact_result_)); + + state_interfaces.emplace_back( + hardware_interface::StateInterface(tf_prefix + TOOL_CONTACT_GPIO, "tool_contact_state", &tool_contact_state_)); + return state_interfaces; } @@ -404,6 +414,9 @@ std::vector URPositionHardwareInterface::e &passthrough_trajectory_accelerations_[i])); } + command_interfaces.emplace_back(hardware_interface::CommandInterface( + tf_prefix + TOOL_CONTACT_GPIO, "tool_contact_set_state", &tool_contact_set_state_)); + return command_interfaces; } @@ -587,6 +600,9 @@ URPositionHardwareInterface::on_configure(const rclcpp_lifecycle::State& previou ur_driver_->registerTrajectoryDoneCallback( std::bind(&URPositionHardwareInterface::trajectory_done_callback, this, std::placeholders::_1)); + ur_driver_->registerToolContactResultCallback( + std::bind(&URPositionHardwareInterface::tool_contact_callback, this, std::placeholders::_1)); + return hardware_interface::CallbackReturn::SUCCESS; } @@ -805,6 +821,10 @@ hardware_interface::return_type URPositionHardwareInterface::write(const rclcpp: stop_force_mode(); } + if (tool_contact_controller_running_) { + check_tool_contact_controller(); + } + packet_read_ = false; } @@ -907,7 +927,7 @@ void URPositionHardwareInterface::checkAsyncIO() } if (!std::isnan(freedrive_mode_enable_) && ur_driver_ != nullptr) { - RCLCPP_INFO(rclcpp::get_logger("URPosistionHardwareInterface"), "Starting freedrive mode."); + RCLCPP_INFO(rclcpp::get_logger("URPositionHardwareInterface"), "Starting freedrive mode."); freedrive_mode_async_success_ = ur_driver_->writeFreedriveControlMessage(urcl::control::FreedriveControlMessage::FREEDRIVE_START); freedrive_mode_enable_ = NO_NEW_CMD_; @@ -916,7 +936,7 @@ void URPositionHardwareInterface::checkAsyncIO() if (!std::isnan(freedrive_mode_abort_) && freedrive_mode_abort_ == 1.0 && freedrive_activated_ && ur_driver_ != nullptr) { - RCLCPP_INFO(rclcpp::get_logger("URPosistionHardwareInterface"), "Stopping freedrive mode."); + RCLCPP_INFO(rclcpp::get_logger("URPositionHardwareInterface"), "Stopping freedrive mode."); freedrive_mode_async_success_ = ur_driver_->writeFreedriveControlMessage(urcl::control::FreedriveControlMessage::FREEDRIVE_STOP); freedrive_activated_ = false; @@ -924,6 +944,44 @@ void URPositionHardwareInterface::checkAsyncIO() } } +void URPositionHardwareInterface::check_tool_contact_controller() +{ + static double cmd_state; + cmd_state = tool_contact_set_state_; + + if (ur_driver_ != nullptr) { + if (cmd_state == 2.0) { + bool success = ur_driver_->startToolContact(); + if (success) { + // TOOL_CONTACT_EXECUTING + tool_contact_state_ = 3.0; + tool_contact_result_ = 3.0; + } else { + // TOOL_CONTACT_FAILURE_BEGIN + tool_contact_state_ = 4.0; + } + + } else if (cmd_state == 5.0) { + bool success = ur_driver_->endToolContact(); + if (success) { + // TOOL_CONTACT_SUCCESS_END + tool_contact_state_ = 6.0; + } else { + // TOOL_CONTACT_FAILURE_END + tool_contact_state_ = 7.0; + } + } else { + tool_contact_state_ = cmd_state; + } + } +} + +void URPositionHardwareInterface::tool_contact_callback(urcl::control::ToolContactResult result) +{ + tool_contact_result_ = static_cast(result); + return; +} + void URPositionHardwareInterface::updateNonDoubleValues() { for (size_t i = 0; i < 18; ++i) { @@ -1031,12 +1089,9 @@ hardware_interface::return_type URPositionHardwareInterface::prepare_command_mod if (freedrive_mode_controller_running_) { control_modes[i].push_back(FREEDRIVE_MODE_GPIO); } - } - - if (!std::all_of(start_modes_.begin() + 1, start_modes_.end(), - [&](const std::vector& other) { return other == start_modes_[0]; })) { - RCLCPP_ERROR(rclcpp::get_logger("URPositionHardwareInterface"), "Start modes of all joints have to be the same."); - return hardware_interface::return_type::ERROR; + if (tool_contact_controller_running_) { + control_modes[i].push_back(TOOL_CONTACT_GPIO); + } } // Starting interfaces @@ -1045,21 +1100,27 @@ hardware_interface::return_type URPositionHardwareInterface::prepare_command_mod for (const auto& key : start_interfaces) { for (auto i = 0u; i < info_.joints.size(); i++) { if (key == info_.joints[i].name + "/" + hardware_interface::HW_IF_POSITION) { - if (!start_modes_[i].empty()) { + if (std::any_of(start_modes_[i].begin(), start_modes_[i].end(), [&](const std::string& item) { + return item == hardware_interface::HW_IF_VELOCITY || item == PASSTHROUGH_GPIO || + item == FORCE_MODE_GPIO || item == FREEDRIVE_MODE_GPIO; + })) { RCLCPP_ERROR(rclcpp::get_logger("URPositionHardwareInterface"), "Attempting to start position control while " "there is another control mode already " "requested."); return hardware_interface::return_type::ERROR; } - start_modes_[i] = { hardware_interface::HW_IF_POSITION }; + start_modes_[i].push_back(hardware_interface::HW_IF_POSITION); } else if (key == info_.joints[i].name + "/" + hardware_interface::HW_IF_VELOCITY) { - if (!start_modes_[i].empty()) { + if (std::any_of(start_modes_[i].begin(), start_modes_[i].end(), [&](const std::string& item) { + return item == hardware_interface::HW_IF_POSITION || item == PASSTHROUGH_GPIO || + item == FORCE_MODE_GPIO || item == FREEDRIVE_MODE_GPIO; + })) { RCLCPP_ERROR(rclcpp::get_logger("URPositionHardwareInterface"), "Attempting to start velocity control while " "there is another control mode already " "requested."); return hardware_interface::return_type::ERROR; } - start_modes_[i] = { hardware_interface::HW_IF_VELOCITY }; + start_modes_[i].push_back(hardware_interface::HW_IF_VELOCITY); } else if (key == tf_prefix + FORCE_MODE_GPIO + "/type") { if (std::any_of(start_modes_[i].begin(), start_modes_[i].end(), [&](const std::string& item) { return item == hardware_interface::HW_IF_POSITION || item == hardware_interface::HW_IF_VELOCITY; @@ -1090,10 +1151,26 @@ hardware_interface::return_type URPositionHardwareInterface::prepare_command_mod return hardware_interface::return_type::ERROR; } start_modes_[i].push_back(FREEDRIVE_MODE_GPIO); + } else if (key == tf_prefix + TOOL_CONTACT_GPIO + "/tool_contact_set_state") { + if (std::any_of(start_modes_[i].begin(), start_modes_[i].end(), [&](const std::string& item) { + return item == FORCE_MODE_GPIO || item == FREEDRIVE_MODE_GPIO; + })) { + RCLCPP_ERROR(rclcpp::get_logger("URPositionHardwareInterface"), "Attempting to start tool contact controller " + "while either the force mode or " + "freedrive controller is running."); + return hardware_interface::return_type::ERROR; + } + start_modes_[i].push_back(TOOL_CONTACT_GPIO); } } } + if (!std::all_of(start_modes_.begin() + 1, start_modes_.end(), + [&](const std::vector& other) { return other == start_modes_[0]; })) { + RCLCPP_ERROR(rclcpp::get_logger("URPositionHardwareInterface"), "Start modes of all joints have to be the same."); + return hardware_interface::return_type::ERROR; + } + // Stopping interfaces // add stop interface per joint in tmp var for later check for (const auto& key : stop_interfaces) { @@ -1130,6 +1207,12 @@ hardware_interface::return_type URPositionHardwareInterface::prepare_command_mod [&](const std::string& item) { return item == FREEDRIVE_MODE_GPIO; }), control_modes[i].end()); } + if (key == tf_prefix + TOOL_CONTACT_GPIO + "/tool_contact_set_state") { + stop_modes_[i].push_back(StoppingInterface::STOP_TOOL_CONTACT); + control_modes[i].erase(std::remove_if(control_modes[i].begin(), control_modes[i].end(), + [&](const std::string& item) { return item == TOOL_CONTACT_GPIO; }), + control_modes[i].end()); + } } } @@ -1146,9 +1229,9 @@ hardware_interface::return_type URPositionHardwareInterface::prepare_command_mod return (item == hardware_interface::HW_IF_VELOCITY || item == hardware_interface::HW_IF_POSITION || item == FREEDRIVE_MODE_GPIO); }))) { - RCLCPP_ERROR(rclcpp::get_logger("URPosistionHardwareInterface"), "Attempting to start passthrough_trajectory " - "control while there is either position or " - "velocity or freedrive mode running."); + RCLCPP_ERROR(rclcpp::get_logger("URPositionHardwareInterface"), "Attempting to start passthrough_trajectory " + "control while there is either position or " + "velocity or freedrive mode running."); ret_val = hardware_interface::return_type::ERROR; } @@ -1158,15 +1241,15 @@ hardware_interface::return_type URPositionHardwareInterface::prepare_command_mod (std::any_of(start_modes_[0].begin(), start_modes_[0].end(), [this](auto& item) { return (item == hardware_interface::HW_IF_VELOCITY || item == hardware_interface::HW_IF_POSITION || - item == FREEDRIVE_MODE_GPIO); + item == FREEDRIVE_MODE_GPIO || item == TOOL_CONTACT_GPIO); }) || std::any_of(control_modes[0].begin(), control_modes[0].end(), [this](auto& item) { return (item == hardware_interface::HW_IF_VELOCITY || item == hardware_interface::HW_IF_POSITION || - item == FORCE_MODE_GPIO || item == FREEDRIVE_MODE_GPIO); + item == FORCE_MODE_GPIO || item == FREEDRIVE_MODE_GPIO || item == TOOL_CONTACT_GPIO); }))) { - RCLCPP_ERROR(rclcpp::get_logger("URPosistionHardwareInterface"), "Attempting to start force mode control while " - "there is either position or " - "velocity mode running."); + RCLCPP_ERROR(rclcpp::get_logger("URPositionHardwareInterface"), "Attempting to start force mode control while " + "there is either position or " + "velocity mode running."); ret_val = hardware_interface::return_type::ERROR; } @@ -1176,15 +1259,28 @@ hardware_interface::return_type URPositionHardwareInterface::prepare_command_mod (std::any_of(start_modes_[0].begin(), start_modes_[0].end(), [this](auto& item) { return (item == hardware_interface::HW_IF_VELOCITY || item == hardware_interface::HW_IF_POSITION || - item == PASSTHROUGH_GPIO || item == FORCE_MODE_GPIO); + item == PASSTHROUGH_GPIO || item == FORCE_MODE_GPIO || item == TOOL_CONTACT_GPIO); }) || std::any_of(control_modes[0].begin(), control_modes[0].end(), [this](auto& item) { return (item == hardware_interface::HW_IF_VELOCITY || item == hardware_interface::HW_IF_POSITION || - item == PASSTHROUGH_GPIO || item == FORCE_MODE_GPIO); + item == PASSTHROUGH_GPIO || item == FORCE_MODE_GPIO || item == TOOL_CONTACT_GPIO); }))) { - RCLCPP_ERROR(rclcpp::get_logger("URPosistionHardwareInterface"), "Attempting to start force mode control while " - "there is either position or " - "velocity mode running."); + RCLCPP_ERROR(rclcpp::get_logger("URPositionHardwareInterface"), "Attempting to start force mode control while " + "there is either position or " + "velocity mode running."); + ret_val = hardware_interface::return_type::ERROR; + } + + // Tool contact controller requested to start + if (std::any_of(start_modes_[0].begin(), start_modes_[0].end(), + [this](auto& item) { return (item == TOOL_CONTACT_GPIO); }) && + (std::any_of(start_modes_[0].begin(), start_modes_[0].end(), + [this](auto& item) { return (item == FORCE_MODE_GPIO || item == FREEDRIVE_MODE_GPIO); }) || + std::any_of(control_modes[0].begin(), control_modes[0].end(), + [this](auto& item) { return (item == FORCE_MODE_GPIO || item == FREEDRIVE_MODE_GPIO); }))) { + RCLCPP_ERROR(rclcpp::get_logger("URPositionHardwareInterface"), "Attempting to start tool contact controller while " + "either the force mode controller or " + "the freedrive controller is running."); ret_val = hardware_interface::return_type::ERROR; } @@ -1200,10 +1296,10 @@ hardware_interface::return_type URPositionHardwareInterface::prepare_command_mod return (item == hardware_interface::HW_IF_VELOCITY || item == hardware_interface::HW_IF_POSITION || item == PASSTHROUGH_GPIO || item == FORCE_MODE_GPIO || item == FREEDRIVE_MODE_GPIO); }))) { - RCLCPP_ERROR(rclcpp::get_logger("URPosistionHardwareInterface"), "Attempting to start position control while there " - "is either trajectory passthrough or " - "velocity mode or force_mode or freedrive mode " - "running."); + RCLCPP_ERROR(rclcpp::get_logger("URPositionHardwareInterface"), "Attempting to start position control while there " + "is either trajectory passthrough or " + "velocity mode or force_mode or freedrive mode " + "running."); ret_val = hardware_interface::return_type::ERROR; } @@ -1219,10 +1315,10 @@ hardware_interface::return_type URPositionHardwareInterface::prepare_command_mod return (item == hardware_interface::HW_IF_VELOCITY || item == hardware_interface::HW_IF_POSITION || item == PASSTHROUGH_GPIO || item == FORCE_MODE_GPIO || item == FREEDRIVE_MODE_GPIO); }))) { - RCLCPP_ERROR(rclcpp::get_logger("URPosistionHardwareInterface"), "Attempting to start velocity control while there " - "is either trajectory passthrough or " - "position mode or force_mode or freedrive mode " - "running."); + RCLCPP_ERROR(rclcpp::get_logger("URPositionHardwareInterface"), "Attempting to start velocity control while there " + "is either trajectory passthrough or " + "position mode or force_mode or freedrive mode " + "running."); ret_val = hardware_interface::return_type::ERROR; } @@ -1239,27 +1335,38 @@ hardware_interface::return_type URPositionHardwareInterface::perform_command_mod StoppingInterface::STOP_POSITION) != stop_modes_[0].end()) { position_controller_running_ = false; urcl_position_commands_ = urcl_position_commands_old_ = urcl_joint_positions_; - } else if (stop_modes_[0].size() != 0 && std::find(stop_modes_[0].begin(), stop_modes_[0].end(), - StoppingInterface::STOP_VELOCITY) != stop_modes_[0].end()) { + } + if (stop_modes_[0].size() != 0 && std::find(stop_modes_[0].begin(), stop_modes_[0].end(), + StoppingInterface::STOP_VELOCITY) != stop_modes_[0].end()) { velocity_controller_running_ = false; urcl_velocity_commands_ = { { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 } }; - } else if (stop_modes_[0].size() != 0 && std::find(stop_modes_[0].begin(), stop_modes_[0].end(), - StoppingInterface::STOP_FORCE_MODE) != stop_modes_[0].end()) { + } + if (stop_modes_[0].size() != 0 && std::find(stop_modes_[0].begin(), stop_modes_[0].end(), + StoppingInterface::STOP_FORCE_MODE) != stop_modes_[0].end()) { force_mode_controller_running_ = false; stop_force_mode(); - } else if (stop_modes_[0].size() != 0 && std::find(stop_modes_[0].begin(), stop_modes_[0].end(), - StoppingInterface::STOP_PASSTHROUGH) != stop_modes_[0].end()) { + } + if (stop_modes_[0].size() != 0 && std::find(stop_modes_[0].begin(), stop_modes_[0].end(), + StoppingInterface::STOP_PASSTHROUGH) != stop_modes_[0].end()) { + RCLCPP_WARN(rclcpp::get_logger("URPositionHardwareInterface"), "Stopping passthrough trajectory controller."); passthrough_trajectory_controller_running_ = false; passthrough_trajectory_abort_ = 1.0; trajectory_joint_positions_.clear(); trajectory_joint_accelerations_.clear(); trajectory_joint_velocities_.clear(); - } else if (stop_modes_.size() != 0 && std::find(stop_modes_[0].begin(), stop_modes_[0].end(), - StoppingInterface::STOP_FREEDRIVE) != stop_modes_[0].end()) { + } + if (stop_modes_.size() != 0 && std::find(stop_modes_[0].begin(), stop_modes_[0].end(), + StoppingInterface::STOP_FREEDRIVE) != stop_modes_[0].end()) { freedrive_mode_controller_running_ = false; freedrive_activated_ = false; freedrive_mode_abort_ = 1.0; } + if (stop_modes_.size() != 0 && std::find(stop_modes_[0].begin(), stop_modes_[0].end(), + StoppingInterface::STOP_TOOL_CONTACT) != stop_modes_[0].end()) { + tool_contact_controller_running_ = false; + tool_contact_result_ = 3.0; + ur_driver_->endToolContact(); + } if (start_modes_.size() != 0 && std::find(start_modes_[0].begin(), start_modes_[0].end(), hardware_interface::HW_IF_POSITION) != start_modes_[0].end()) { @@ -1274,23 +1381,29 @@ hardware_interface::return_type URPositionHardwareInterface::perform_command_mod passthrough_trajectory_controller_running_ = false; urcl_velocity_commands_ = { { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 } }; velocity_controller_running_ = true; - } else if (start_modes_[0].size() != 0 && - std::find(start_modes_[0].begin(), start_modes_[0].end(), FORCE_MODE_GPIO) != start_modes_[0].end()) { + } + if (start_modes_[0].size() != 0 && + std::find(start_modes_[0].begin(), start_modes_[0].end(), FORCE_MODE_GPIO) != start_modes_[0].end()) { force_mode_controller_running_ = true; - } else if (start_modes_[0].size() != 0 && - std::find(start_modes_[0].begin(), start_modes_[0].end(), PASSTHROUGH_GPIO) != start_modes_[0].end()) { + } + if (start_modes_[0].size() != 0 && + std::find(start_modes_[0].begin(), start_modes_[0].end(), PASSTHROUGH_GPIO) != start_modes_[0].end()) { velocity_controller_running_ = false; position_controller_running_ = false; passthrough_trajectory_controller_running_ = true; passthrough_trajectory_abort_ = 0.0; - } else if (start_modes_[0].size() != 0 && - std::find(start_modes_[0].begin(), start_modes_[0].end(), FREEDRIVE_MODE_GPIO) != start_modes_[0].end()) { + } + if (start_modes_[0].size() != 0 && + std::find(start_modes_[0].begin(), start_modes_[0].end(), FREEDRIVE_MODE_GPIO) != start_modes_[0].end()) { velocity_controller_running_ = false; position_controller_running_ = false; freedrive_mode_controller_running_ = true; freedrive_activated_ = false; } - + if (start_modes_[0].size() != 0 && + std::find(start_modes_[0].begin(), start_modes_[0].end(), TOOL_CONTACT_GPIO) != start_modes_[0].end()) { + tool_contact_controller_running_ = true; + } start_modes_.clear(); stop_modes_.clear(); diff --git a/ur_robot_driver/test/integration_test_controller_switch.py b/ur_robot_driver/test/integration_test_controller_switch.py index 1d2d6570e..3c8a31a26 100644 --- a/ur_robot_driver/test/integration_test_controller_switch.py +++ b/ur_robot_driver/test/integration_test_controller_switch.py @@ -405,3 +405,92 @@ def test_force_mode_and_trajectory_passthrough_controller_are_compatible(self): ], ).ok ) + + def test_tool_contact_compatibility(self): + # Deactivate all writing controllers + self.assertTrue( + self._controller_manager_interface.switch_controller( + strictness=SwitchController.Request.BEST_EFFORT, + deactivate_controllers=[ + "scaled_joint_trajectory_controller", + "joint_trajectory_controller", + "forward_position_controller", + "forward_velocity_controller", + "passthrough_trajectory_controller", + "force_mode_controller", + "tool_contact_controller", + ], + ).ok + ) + + time.sleep(3) + # Start tool contact controller and JTC + self.assertTrue( + self._controller_manager_interface.switch_controller( + strictness=SwitchController.Request.STRICT, + activate_controllers=[ + "scaled_joint_trajectory_controller", + "tool_contact_controller", + ], + ).ok + ) + self.assertTrue( + self._controller_manager_interface.switch_controller( + strictness=SwitchController.Request.STRICT, + deactivate_controllers=[ + "scaled_joint_trajectory_controller", + "tool_contact_controller", + ], + ).ok + ) + + # Start tool contact controller and passthrough trajectory + self.assertTrue( + self._controller_manager_interface.switch_controller( + strictness=SwitchController.Request.STRICT, + activate_controllers=[ + "passthrough_trajectory_controller", + "tool_contact_controller", + ], + ).ok + ) + self.assertTrue( + self._controller_manager_interface.switch_controller( + strictness=SwitchController.Request.STRICT, + deactivate_controllers=[ + "passthrough_trajectory_controller", + "tool_contact_controller", + ], + ).ok + ) + + # tool contact should not start with force_mode + self.assertFalse( + self._controller_manager_interface.switch_controller( + strictness=SwitchController.Request.STRICT, + activate_controllers=[ + "force_mode_controller", + "tool_contact_controller", + ], + ).ok + ) + self.assertFalse( + self._controller_manager_interface.switch_controller( + strictness=SwitchController.Request.STRICT, + activate_controllers=[ + "tool_contact_controller", + "force_mode_controller", + ], + ).ok + ) + + # tool contact should not start with freedrive + self.assertFalse( + self._controller_manager_interface.switch_controller( + strictness=SwitchController.Request.STRICT, + activate_controllers=[ + "tool_contact_controller", + "freedrive_mode_controller", + ], + ).ok + ) diff --git a/ur_robot_driver/test/integration_test_tool_contact.py b/ur_robot_driver/test/integration_test_tool_contact.py new file mode 100644 index 000000000..383d362b0 --- /dev/null +++ b/ur_robot_driver/test/integration_test_tool_contact.py @@ -0,0 +1,162 @@ +#!/usr/bin/env python +# 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 os +import sys +import time +import unittest + +import pytest + +import launch_testing +import rclpy +from rclpy.node import Node + +from controller_manager_msgs.srv import SwitchController +from ur_msgs.action import ToolContact +from action_msgs.msg import GoalStatus + +sys.path.append(os.path.dirname(__file__)) +from test_common import ( # noqa: E402 + ControllerManagerInterface, + DashboardInterface, + IoStatusInterface, + ActionInterface, + generate_driver_test_description, +) + + +@pytest.mark.launch_test +@launch_testing.parametrize( + "tf_prefix", + [(""), ("my_ur_")], +) +def generate_test_description(tf_prefix): + return generate_driver_test_description(tf_prefix=tf_prefix) + + +class RobotDriverTest(unittest.TestCase): + @classmethod + def setUpClass(cls): + # Initialize the ROS context + rclpy.init() + cls.node = Node("robot_driver_test") + time.sleep(1) + cls.init_robot(cls) + + @classmethod + def tearDownClass(cls): + # Shutdown the ROS context + cls.node.destroy_node() + rclpy.shutdown() + + def init_robot(self): + self._dashboard_interface = DashboardInterface(self.node) + self._controller_manager_interface = ControllerManagerInterface(self.node) + self._io_status_controller_interface = IoStatusInterface(self.node) + self._tool_contact_interface = ActionInterface( + self.node, "/tool_contact_controller/detect_tool_contact", ToolContact + ) + + def setUp(self): + self._dashboard_interface.start_robot() + time.sleep(1) + self.assertTrue(self._io_status_controller_interface.resend_robot_program().success) + + # + # Tests + # + + def test_start_tool_contact_controller(self): + self.assertTrue( + self._controller_manager_interface.switch_controller( + strictness=SwitchController.Request.BEST_EFFORT, + activate_controllers=["tool_contact_controller"], + ).ok + ) + + def test_goal_can_be_canceled(self): + self.assertTrue( + self._controller_manager_interface.switch_controller( + strictness=SwitchController.Request.BEST_EFFORT, + activate_controllers=["tool_contact_controller"], + ).ok + ) + goal_handle = self._tool_contact_interface.send_goal() + self.assertTrue(goal_handle.accepted) + + cancel_res = self._tool_contact_interface.cancel_goal(goal_handle) + self.assertEqual(cancel_res.return_code, 0) + + def test_deactivate_controller_aborts_action(self): + self.assertTrue( + self._controller_manager_interface.switch_controller( + strictness=SwitchController.Request.BEST_EFFORT, + activate_controllers=["tool_contact_controller"], + ).ok + ) + + goal_handle = self._tool_contact_interface.send_goal() + self.assertTrue(goal_handle.accepted) + + self.assertTrue( + self._controller_manager_interface.switch_controller( + strictness=SwitchController.Request.BEST_EFFORT, + deactivate_controllers=["tool_contact_controller"], + ).ok + ) + # Wait for action to finish + self._tool_contact_interface.get_result(goal_handle, 5) + # Check status of goal handle, as result does not contain information about the status of the action. Only the empty result definition. + self.assertEqual(goal_handle._status, GoalStatus.STATUS_ABORTED) + + def test_inactive_controller_rejects_actions(self): + self.assertTrue( + self._controller_manager_interface.switch_controller( + strictness=SwitchController.Request.BEST_EFFORT, + deactivate_controllers=["tool_contact_controller"], + ).ok + ) + + goal_handle = self._tool_contact_interface.send_goal() + self.assertFalse(goal_handle.accepted) + + def test_busy_controller_rejects_actions(self): + self.assertTrue( + self._controller_manager_interface.switch_controller( + strictness=SwitchController.Request.BEST_EFFORT, + activate_controllers=["tool_contact_controller"], + ).ok + ) + + goal_handle = self._tool_contact_interface.send_goal() + self.assertTrue(goal_handle.accepted) + + goal_handle = self._tool_contact_interface.send_goal() + self.assertFalse(goal_handle.accepted) diff --git a/ur_robot_driver/test/test_common.py b/ur_robot_driver/test/test_common.py index 018ec74d2..640a7afaf 100644 --- a/ur_robot_driver/test/test_common.py +++ b/ur_robot_driver/test/test_common.py @@ -191,6 +191,18 @@ def get_result(self, goal_handle, timeout): f"Exception while calling action '{self.__action_name}': {future_res.exception()}" ) + def cancel_goal(self, goal_handle, timeout=2): + future_res = goal_handle.cancel_goal_async() + logging.info("Canceling goal from '%s' with timeout %fs", self.__action_name, timeout) + rclpy.spin_until_future_complete(self.__node, future_res, timeout_sec=timeout) + if future_res.result() is not None: + logging.info(" Received result: %s", future_res.result()) + return future_res.result() + else: + raise Exception( + f"Exception while calling action '{self.__action_name}': {future_res.exception()}" + ) + class DashboardInterface( _ServiceInterface, @@ -353,7 +365,9 @@ def generate_dashboard_test_description(): def generate_driver_test_description( - tf_prefix="", controller_spawner_timeout=TIMEOUT_WAIT_SERVICE_INITIAL + tf_prefix="", + initial_joint_controller="scaled_joint_trajectory_controller", + controller_spawner_timeout=TIMEOUT_WAIT_SERVICE_INITIAL, ): ur_type = LaunchConfiguration("ur_type") @@ -362,7 +376,7 @@ def generate_driver_test_description( "ur_type": ur_type, "launch_rviz": "false", "controller_spawner_timeout": str(controller_spawner_timeout), - "initial_joint_controller": "scaled_joint_trajectory_controller", + "initial_joint_controller": initial_joint_controller, "headless_mode": "true", "launch_dashboard_client": "true", "start_joint_controller": "false", From 81b7b9c2adf884212173c70f1f962ac6e507b041 Mon Sep 17 00:00:00 2001 From: "mergify[bot]" <37929162+mergify[bot]@users.noreply.github.com> Date: Mon, 5 May 2025 09:28:53 +0200 Subject: [PATCH 064/133] tool_contact_test: Check result status directly (backport of #1345) (#1353) Re-using the goal handle seems to be flaky. --- .../test/integration_test_tool_contact.py | 19 +++++++++++++++---- 1 file changed, 15 insertions(+), 4 deletions(-) diff --git a/ur_robot_driver/test/integration_test_tool_contact.py b/ur_robot_driver/test/integration_test_tool_contact.py index 383d362b0..db6c60817 100644 --- a/ur_robot_driver/test/integration_test_tool_contact.py +++ b/ur_robot_driver/test/integration_test_tool_contact.py @@ -31,6 +31,7 @@ import sys import time import unittest +import logging import pytest @@ -131,10 +132,20 @@ def test_deactivate_controller_aborts_action(self): deactivate_controllers=["tool_contact_controller"], ).ok ) - # Wait for action to finish - self._tool_contact_interface.get_result(goal_handle, 5) - # Check status of goal handle, as result does not contain information about the status of the action. Only the empty result definition. - self.assertEqual(goal_handle._status, GoalStatus.STATUS_ABORTED) + future_res = goal_handle.get_result_async() + + timeout = 5.0 + logging.info("Waiting for action result from controller with timeout %fs", timeout) + rclpy.spin_until_future_complete(self.node, future_res, timeout_sec=timeout) + + if future_res.result() is not None: + logging.info(" Received result: %s", future_res.result().result) + # Check status of goal handle, as result does not contain information about the status of the action. Only the empty result definition. + self.assertEqual(future_res.result().status, GoalStatus.STATUS_ABORTED) + else: + raise Exception( + f"Exception while calling action '{self.__action_name}': {future_res.exception()}" + ) def test_inactive_controller_rejects_actions(self): self.assertTrue( From 47b293dba16fc6efc127331cc58b60ffbc682b72 Mon Sep 17 00:00:00 2001 From: Felix Exner Date: Thu, 15 May 2025 11:08:04 +0200 Subject: [PATCH 065/133] Add support for launching a UR15 robot (#1359) --- README.md | 2 +- ur_bringup/launch/ur15.launch.py | 102 ++++++++++++++++++ ur_bringup/launch/ur20.launch.py | 102 ++++++++++++++++++ ur_bringup/launch/ur30.launch.py | 102 ++++++++++++++++++ ur_bringup/launch/ur_control.launch.py | 1 + ur_moveit_config/launch/ur_moveit.launch.py | 1 + ur_robot_driver/config/ur15_update_rate.yaml | 3 + ur_robot_driver/doc/usage/startup.rst | 2 +- ur_robot_driver/launch/ur15.launch.py | 104 +++++++++++++++++++ ur_robot_driver/launch/ur_control.launch.py | 1 + ur_robot_driver/test/test_common.py | 1 + 11 files changed, 419 insertions(+), 2 deletions(-) create mode 100644 ur_bringup/launch/ur15.launch.py create mode 100644 ur_bringup/launch/ur20.launch.py create mode 100644 ur_bringup/launch/ur30.launch.py create mode 100644 ur_robot_driver/config/ur15_update_rate.yaml create mode 100644 ur_robot_driver/launch/ur15.launch.py diff --git a/README.md b/README.md index 29b73e43c..72f1f3ba8 100644 --- a/README.md +++ b/README.md @@ -117,7 +117,7 @@ For getting started, you'll basically need three steps: details. ```bash - # Replace ur5e with one of ur3, ur3e, ur5, ur5e, ur7e, ur10, ur10e, ur12e, ur16e, ur20, ur30 + # Replace ur5e with one of ur3, ur3e, ur5, ur5e, ur7e, ur10, ur10e, ur12e, ur16e, ur15, ur20, ur30 # Replace the IP address with the IP address of your actual robot / URSim ros2 launch ur_robot_driver ur_control.launch.py ur_type:=ur5e robot_ip:=192.168.56.101 ``` diff --git a/ur_bringup/launch/ur15.launch.py b/ur_bringup/launch/ur15.launch.py new file mode 100644 index 000000000..7d14c070b --- /dev/null +++ b/ur_bringup/launch/ur15.launch.py @@ -0,0 +1,102 @@ +# Copyright (c) 2021 PickNik, Inc. +# +# 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. + +# +# Author: Denis Stogl + +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import LaunchConfiguration, ThisLaunchFileDir + + +def generate_launch_description(): + # Declare arguments + declared_arguments = [] + declared_arguments.append( + DeclareLaunchArgument( + "robot_ip", + description="IP address by which the robot can be reached.", + ) + ) + declared_arguments.append( + DeclareLaunchArgument( + "use_fake_hardware", + default_value="false", + description="Start robot with fake hardware mirroring command to its states.", + ) + ) + declared_arguments.append( + DeclareLaunchArgument( + "fake_sensor_commands", + default_value="false", + description="Enable fake command interfaces for sensors used for simple simulations. \ + Used only if 'use_fake_hardware' parameter is true.", + ) + ) + declared_arguments.append( + DeclareLaunchArgument( + "initial_joint_controller", + default_value="scaled_joint_trajectory_controller", + description="Initially loaded robot controller.", + choices=[ + "scaled_joint_trajectory_controller", + "joint_trajectory_controller", + "forward_velocity_controller", + "forward_position_controller", + ], + ) + ) + declared_arguments.append( + DeclareLaunchArgument( + "activate_joint_controller", + default_value="true", + description="Activate loaded joint controller.", + ) + ) + + # Initialize Arguments + robot_ip = LaunchConfiguration("robot_ip") + use_fake_hardware = LaunchConfiguration("use_fake_hardware") + fake_sensor_commands = LaunchConfiguration("fake_sensor_commands") + initial_joint_controller = LaunchConfiguration("initial_joint_controller") + activate_joint_controller = LaunchConfiguration("activate_joint_controller") + + base_launch = IncludeLaunchDescription( + PythonLaunchDescriptionSource([ThisLaunchFileDir(), "/ur_control.launch.py"]), + launch_arguments={ + "ur_type": "ur15", + "robot_ip": robot_ip, + "use_fake_hardware": use_fake_hardware, + "fake_sensor_commands": fake_sensor_commands, + "initial_joint_controller": initial_joint_controller, + "activate_joint_controller": activate_joint_controller, + }.items(), + ) + + return LaunchDescription(declared_arguments + [base_launch]) diff --git a/ur_bringup/launch/ur20.launch.py b/ur_bringup/launch/ur20.launch.py new file mode 100644 index 000000000..833fd46ee --- /dev/null +++ b/ur_bringup/launch/ur20.launch.py @@ -0,0 +1,102 @@ +# Copyright (c) 2021 PickNik, Inc. +# +# 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. + +# +# Author: Denis Stogl + +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import LaunchConfiguration, ThisLaunchFileDir + + +def generate_launch_description(): + # Declare arguments + declared_arguments = [] + declared_arguments.append( + DeclareLaunchArgument( + "robot_ip", + description="IP address by which the robot can be reached.", + ) + ) + declared_arguments.append( + DeclareLaunchArgument( + "use_fake_hardware", + default_value="false", + description="Start robot with fake hardware mirroring command to its states.", + ) + ) + declared_arguments.append( + DeclareLaunchArgument( + "fake_sensor_commands", + default_value="false", + description="Enable fake command interfaces for sensors used for simple simulations. \ + Used only if 'use_fake_hardware' parameter is true.", + ) + ) + declared_arguments.append( + DeclareLaunchArgument( + "initial_joint_controller", + default_value="scaled_joint_trajectory_controller", + description="Initially loaded robot controller.", + choices=[ + "scaled_joint_trajectory_controller", + "joint_trajectory_controller", + "forward_velocity_controller", + "forward_position_controller", + ], + ) + ) + declared_arguments.append( + DeclareLaunchArgument( + "activate_joint_controller", + default_value="true", + description="Activate loaded joint controller.", + ) + ) + + # Initialize Arguments + robot_ip = LaunchConfiguration("robot_ip") + use_fake_hardware = LaunchConfiguration("use_fake_hardware") + fake_sensor_commands = LaunchConfiguration("fake_sensor_commands") + initial_joint_controller = LaunchConfiguration("initial_joint_controller") + activate_joint_controller = LaunchConfiguration("activate_joint_controller") + + base_launch = IncludeLaunchDescription( + PythonLaunchDescriptionSource([ThisLaunchFileDir(), "/ur_control.launch.py"]), + launch_arguments={ + "ur_type": "ur20", + "robot_ip": robot_ip, + "use_fake_hardware": use_fake_hardware, + "fake_sensor_commands": fake_sensor_commands, + "initial_joint_controller": initial_joint_controller, + "activate_joint_controller": activate_joint_controller, + }.items(), + ) + + return LaunchDescription(declared_arguments + [base_launch]) diff --git a/ur_bringup/launch/ur30.launch.py b/ur_bringup/launch/ur30.launch.py new file mode 100644 index 000000000..7556c85b9 --- /dev/null +++ b/ur_bringup/launch/ur30.launch.py @@ -0,0 +1,102 @@ +# Copyright (c) 2021 PickNik, Inc. +# +# 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. + +# +# Author: Denis Stogl + +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import LaunchConfiguration, ThisLaunchFileDir + + +def generate_launch_description(): + # Declare arguments + declared_arguments = [] + declared_arguments.append( + DeclareLaunchArgument( + "robot_ip", + description="IP address by which the robot can be reached.", + ) + ) + declared_arguments.append( + DeclareLaunchArgument( + "use_fake_hardware", + default_value="false", + description="Start robot with fake hardware mirroring command to its states.", + ) + ) + declared_arguments.append( + DeclareLaunchArgument( + "fake_sensor_commands", + default_value="false", + description="Enable fake command interfaces for sensors used for simple simulations. \ + Used only if 'use_fake_hardware' parameter is true.", + ) + ) + declared_arguments.append( + DeclareLaunchArgument( + "initial_joint_controller", + default_value="scaled_joint_trajectory_controller", + description="Initially loaded robot controller.", + choices=[ + "scaled_joint_trajectory_controller", + "joint_trajectory_controller", + "forward_velocity_controller", + "forward_position_controller", + ], + ) + ) + declared_arguments.append( + DeclareLaunchArgument( + "activate_joint_controller", + default_value="true", + description="Activate loaded joint controller.", + ) + ) + + # Initialize Arguments + robot_ip = LaunchConfiguration("robot_ip") + use_fake_hardware = LaunchConfiguration("use_fake_hardware") + fake_sensor_commands = LaunchConfiguration("fake_sensor_commands") + initial_joint_controller = LaunchConfiguration("initial_joint_controller") + activate_joint_controller = LaunchConfiguration("activate_joint_controller") + + base_launch = IncludeLaunchDescription( + PythonLaunchDescriptionSource([ThisLaunchFileDir(), "/ur_control.launch.py"]), + launch_arguments={ + "ur_type": "ur30", + "robot_ip": robot_ip, + "use_fake_hardware": use_fake_hardware, + "fake_sensor_commands": fake_sensor_commands, + "initial_joint_controller": initial_joint_controller, + "activate_joint_controller": activate_joint_controller, + }.items(), + ) + + return LaunchDescription(declared_arguments + [base_launch]) diff --git a/ur_bringup/launch/ur_control.launch.py b/ur_bringup/launch/ur_control.launch.py index c5139376e..9258be24a 100644 --- a/ur_bringup/launch/ur_control.launch.py +++ b/ur_bringup/launch/ur_control.launch.py @@ -60,6 +60,7 @@ def generate_launch_description(): "ur10e", "ur12e", "ur16e", + "ur15", "ur20", "ur30", ], diff --git a/ur_moveit_config/launch/ur_moveit.launch.py b/ur_moveit_config/launch/ur_moveit.launch.py index f6bd72d34..bb20be7a8 100644 --- a/ur_moveit_config/launch/ur_moveit.launch.py +++ b/ur_moveit_config/launch/ur_moveit.launch.py @@ -290,6 +290,7 @@ def generate_launch_description(): "ur10e", "ur12e", "ur16e", + "ur15", "ur20", "ur30", ], diff --git a/ur_robot_driver/config/ur15_update_rate.yaml b/ur_robot_driver/config/ur15_update_rate.yaml new file mode 100644 index 000000000..66ef3d736 --- /dev/null +++ b/ur_robot_driver/config/ur15_update_rate.yaml @@ -0,0 +1,3 @@ +controller_manager: + ros__parameters: + update_rate: 500 # Hz diff --git a/ur_robot_driver/doc/usage/startup.rst b/ur_robot_driver/doc/usage/startup.rst index a78ae6fdf..a1fa3f947 100644 --- a/ur_robot_driver/doc/usage/startup.rst +++ b/ur_robot_driver/doc/usage/startup.rst @@ -23,7 +23,7 @@ nodes for UR robots. The only required arguments are the ``ur_type`` and ``robot $ ros2 launch ur_robot_driver ur_control.launch.py ur_type:=ur5e robot_ip:=192.168.56.101 Allowed ``ur_type`` strings: ``ur3``, ``ur3e``, ``ur5``, ``ur5e``, ``ur7e``, ``ur10``, ``ur10e``, -``ur12e``, ``ur16e``, ``ur20``, ``ur30``. +``ur12e``, ``ur16e``, ``ur15``, ``ur20``, ``ur30``. Other important arguments are: diff --git a/ur_robot_driver/launch/ur15.launch.py b/ur_robot_driver/launch/ur15.launch.py new file mode 100644 index 000000000..bd0de84e1 --- /dev/null +++ b/ur_robot_driver/launch/ur15.launch.py @@ -0,0 +1,104 @@ +# Copyright (c) 2021 PickNik, Inc. +# +# 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. + +# +# Author: Denis Stogl + +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import LaunchConfiguration, ThisLaunchFileDir + + +def generate_launch_description(): + # Declare arguments + declared_arguments = [] + declared_arguments.append( + DeclareLaunchArgument( + "robot_ip", + description="IP address by which the robot can be reached.", + ) + ) + declared_arguments.append( + DeclareLaunchArgument( + "use_mock_hardware", + default_value="false", + description="Start robot with mock hardware mirroring command to its states.", + ) + ) + declared_arguments.append( + DeclareLaunchArgument( + "mock_sensor_commands", + default_value="false", + description="Enable mock command interfaces for sensors used for simple simulations. " + "Used only if 'use_mock_hardware' parameter is true.", + ) + ) + declared_arguments.append( + DeclareLaunchArgument( + "initial_joint_controller", + default_value="scaled_joint_trajectory_controller", + description="Initially loaded robot controller.", + choices=[ + "scaled_joint_trajectory_controller", + "joint_trajectory_controller", + "forward_velocity_controller", + "forward_position_controller", + "freedrive_mode_controller", + "passthrough_trajectory_controller", + ], + ) + ) + declared_arguments.append( + DeclareLaunchArgument( + "activate_joint_controller", + default_value="true", + description="Activate loaded joint controller.", + ) + ) + + # Initialize Arguments + robot_ip = LaunchConfiguration("robot_ip") + use_mock_hardware = LaunchConfiguration("use_mock_hardware") + mock_sensor_commands = LaunchConfiguration("mock_sensor_commands") + initial_joint_controller = LaunchConfiguration("initial_joint_controller") + activate_joint_controller = LaunchConfiguration("activate_joint_controller") + + base_launch = IncludeLaunchDescription( + PythonLaunchDescriptionSource([ThisLaunchFileDir(), "/ur_control.launch.py"]), + launch_arguments={ + "ur_type": "ur15", + "robot_ip": robot_ip, + "use_mock_hardware": use_mock_hardware, + "mock_sensor_commands": mock_sensor_commands, + "initial_joint_controller": initial_joint_controller, + "activate_joint_controller": activate_joint_controller, + }.items(), + ) + + return LaunchDescription(declared_arguments + [base_launch]) diff --git a/ur_robot_driver/launch/ur_control.launch.py b/ur_robot_driver/launch/ur_control.launch.py index d8f2af710..faf51adac 100644 --- a/ur_robot_driver/launch/ur_control.launch.py +++ b/ur_robot_driver/launch/ur_control.launch.py @@ -408,6 +408,7 @@ def generate_launch_description(): "ur10e", "ur12e", "ur16e", + "ur15", "ur20", "ur30", ], diff --git a/ur_robot_driver/test/test_common.py b/ur_robot_driver/test/test_common.py index 640a7afaf..9d1d79621 100644 --- a/ur_robot_driver/test/test_common.py +++ b/ur_robot_driver/test/test_common.py @@ -313,6 +313,7 @@ def _declare_launch_arguments(): "ur10e", "ur12e", "ur16e", + "ur15", "ur20", "ur30", ], From ef8b9ec65e41393f2d1ca90df5c33ab5e8d47923 Mon Sep 17 00:00:00 2001 From: Felix Exner Date: Thu, 15 May 2025 11:37:53 +0200 Subject: [PATCH 066/133] Update ci_format workflow components (#1360) --- .github/workflows/ci-format.yml | 18 +++++++++--------- 1 file changed, 9 insertions(+), 9 deletions(-) diff --git a/.github/workflows/ci-format.yml b/.github/workflows/ci-format.yml index f3527cc17..715112086 100644 --- a/.github/workflows/ci-format.yml +++ b/.github/workflows/ci-format.yml @@ -15,12 +15,12 @@ jobs: name: Format runs-on: ubuntu-22.04 steps: - - uses: actions/checkout@v2 - - uses: actions/setup-python@v2 - with: - python-version: 3.10.4 - - name: Install system hooks - run: sudo apt-get install clang-format-14 cppcheck - - uses: pre-commit/action@v2.0.3 - with: - extra_args: --all-files --hook-stage manual + - uses: actions/checkout@v4 + - uses: actions/setup-python@v5 + with: + python-version: 3.10.4 + - name: Install system hooks + run: sudo apt-get install clang-format-14 cppcheck + - uses: pre-commit/action@v3.0.1 + with: + extra_args: --all-files --hook-stage manual From 5dfed5d2814415357663cc1801e02d6464b6fdee Mon Sep 17 00:00:00 2001 From: "mergify[bot]" <37929162+mergify[bot]@users.noreply.github.com> Date: Thu, 15 May 2025 12:47:18 +0200 Subject: [PATCH 067/133] [CI] Check links using lychee instead of a custom script (backport #1355) (#1361) Co-authored-by: Felix Exner --- .github/helpers/check_urls.sh | 67 ---------------------------- .github/workflows/check_links.yml | 34 +++++++++++--- .gitignore | 1 + ur_robot_driver/doc/ROS_INTERFACE.md | 50 ++++++++++----------- 4 files changed, 54 insertions(+), 98 deletions(-) delete mode 100755 .github/helpers/check_urls.sh diff --git a/.github/helpers/check_urls.sh b/.github/helpers/check_urls.sh deleted file mode 100755 index f7222d115..000000000 --- a/.github/helpers/check_urls.sh +++ /dev/null @@ -1,67 +0,0 @@ -#!/bin/bash - -set -e - -IGNORE_FILES="" -IGNORE_PATTERNS="" - -while getopts ":f:d:p:" opt; do - case "${opt}" in - f) - IGNORE_FILES="${OPTARG}";; - d) - IGNORE_DIRS="${OPTARG}";; - p) - IGNORE_PATTERNS="${OPTARG}";; - \?) - echo "Invalid option -$OPTARG" - exit;; - esac -done - -read -r -a ignore_files <<<"$IGNORE_FILES" -read -r -a ignore_dirs <<<"$IGNORE_DIRS" -read -r -a ignore_patterns <<<"$IGNORE_PATTERNS" - -IGNORE_FILES_ARG="" -for item in "${ignore_files[@]}"; do - IGNORE_FILES_ARG="$IGNORE_FILES_ARG --exclude=$item" -done -IGNORE_DIRS_ARG="" -for item in "${ignore_dirs[@]}"; do - IGNORE_DIRS_ARG="$IGNORE_DIRS_ARG --exclude-dir=$item" -done - -#Find URLs in code: -urls=$(grep -oP '(http|ftp|https):\/\/([a-zA-Z0-9_-]+(?:(?:\.[a-zA-Z0-9_-]+)+))([a-zA-Z0-9_.,@?^=%&:\/~+#-]*[a-zA-Z0-9_@?^=%&\/~+#-])?' -rI $IGNORE_FILES_ARG $IGNORE_DIRS_ARG) - -fail_counter=0 - -FAILED_LINKS=() -for item in $urls; do -# echo $item - skip=0 - for pattern in "${ignore_patterns[@]}"; do - [[ "$item" =~ $pattern ]] && skip=1 - done - - if [[ $skip == 1 ]]; then - echo "SKIPPING $item" - continue - fi - - filename=$(echo "$item" | cut -d':' -f1) - url=$(echo "$item" | cut -d':' -f2-) - echo -n "Checking $url from file $filename" - if ! curl --head --silent --fail "$url" 2>&1 > /dev/null; then - echo -e " \033[0;31mNOT FOUND\033[32m\n" - FAILED_LINKS+=("$url from file $filename") - ((fail_counter=fail_counter+1)) - else - printf " \033[32mok\033[0m\n" - fi -done - -echo "Failed files:" -printf '%s\n' "${FAILED_LINKS[@]}" -exit $fail_counter diff --git a/.github/workflows/check_links.yml b/.github/workflows/check_links.yml index dc662f7b2..55f8bd293 100644 --- a/.github/workflows/check_links.yml +++ b/.github/workflows/check_links.yml @@ -11,9 +11,31 @@ jobs: runs-on: ubuntu-latest steps: - uses: actions/checkout@v4 - - name: Check URLs - run: | - .github/helpers/check_urls.sh \ - -d ".git build CMakeModules debian" \ - -f "package.xml ursim_docker.rst architecture_coarse.svg" \ - -p "vnc\.html opensource\.org\/licenses\/BSD-3-Clause kernel\.org\/pub\/linux\/kernel" + - name: Restore lychee cache + id: restore-cache + uses: actions/cache/restore@v4 + with: + path: .lycheecache + key: cache-lychee-${{ github.run_id }}-${{ github.run_attempt }} + restore-keys: cache-lychee- + - name: Link Checker + id: lychee + uses: lycheeverse/lychee-action@v2 + with: + fail: true + args: > + --verbose + --no-progress + --cache + --cache-exclude-status 429 + --max-cache-age 2d + --exclude '^http://192\.168\.56\.101' + --exclude 'kernel\.org\/pub\/linux\/kernel' + --exclude 'releases/download/v\$%7BURCAP_VERSION%7D/externalcontrol-\$%7BURCAP_VERSION%7D\.jar' + './**/*.md' './**/*.html' './**/*.rst' './**/*.cpp' './**/*.h' './**/*.py' + - name: Save lychee cache + uses: actions/cache/save@v4 + if: always() + with: + path: .lycheecache + key: ${{ steps.restore-cache.outputs.cache-primary-key }} diff --git a/.gitignore b/.gitignore index 141947570..729969e30 100644 --- a/.gitignore +++ b/.gitignore @@ -12,3 +12,4 @@ docs_output docs_build cross_reference ur_robot_driver/doc/generated +.lycheecache diff --git a/ur_robot_driver/doc/ROS_INTERFACE.md b/ur_robot_driver/doc/ROS_INTERFACE.md index 829302e0a..21bb3d2bc 100644 --- a/ur_robot_driver/doc/ROS_INTERFACE.md +++ b/ur_robot_driver/doc/ROS_INTERFACE.md @@ -102,103 +102,103 @@ Should the tool's RS485 interface be forwarded to the ROS machine? This is only #### Advertised Services -##### add_to_log ([ur_dashboard_msgs/AddToLog](http://docs.ros.org/api/ur_dashboard_msgs/html/srv/AddToLog.html)) +##### add_to_log ([ur_dashboard_msgs/AddToLog](https://docs.ros.org/en/humble/p/ur_dashboard_msgs/srv/AddToLog.html)) Service to add a message to the robot's log -##### brake_release ([std_srvs/Trigger](http://docs.ros.org/api/std_srvs/html/srv/Trigger.html)) +##### brake_release ([std_srvs/Trigger](https://docs.ros.org/en/humble/p/std_srvs/srv/Trigger.html)) Service to release the brakes. If the robot is currently powered off, it will get powered on on the fly. -##### clear_operational_mode ([std_srvs/Trigger](http://docs.ros.org/api/std_srvs/html/srv/Trigger.html)) +##### clear_operational_mode ([std_srvs/Trigger](https://docs.ros.org/en/humble/p/std_srvs/srv/Trigger.html)) If this service is called the operational mode can again be changed from PolyScope, and the user password is enabled. -##### close_popup ([std_srvs/Trigger](http://docs.ros.org/api/std_srvs/html/srv/Trigger.html)) +##### close_popup ([std_srvs/Trigger](https://docs.ros.org/en/humble/p/std_srvs/srv/Trigger.html)) Close a (non-safety) popup on the teach pendant. -##### close_safety_popup ([std_srvs/Trigger](http://docs.ros.org/api/std_srvs/html/srv/Trigger.html)) +##### close_safety_popup ([std_srvs/Trigger](https://docs.ros.org/en/humble/p/std_srvs/srv/Trigger.html)) Close a safety popup on the teach pendant. -##### connect ([std_srvs/Trigger](http://docs.ros.org/api/std_srvs/html/srv/Trigger.html)) +##### connect ([std_srvs/Trigger](https://docs.ros.org/en/humble/p/std_srvs/srv/Trigger.html)) Service to reconnect to the dashboard server -##### get_loaded_program ([ur_dashboard_msgs/GetLoadedProgram](http://docs.ros.org/api/ur_dashboard_msgs/html/srv/GetLoadedProgram.html)) +##### get_loaded_program ([ur_dashboard_msgs/GetLoadedProgram](https://docs.ros.org/en/humble/p/ur_dashboard_msgs/srv/GetLoadedProgram.html)) Load a robot installation from a file -##### get_robot_mode ([ur_dashboard_msgs/GetRobotMode](http://docs.ros.org/api/ur_dashboard_msgs/html/srv/GetRobotMode.html)) +##### get_robot_mode ([ur_dashboard_msgs/GetRobotMode](https://docs.ros.org/en/humble/p/ur_dashboard_msgs/srv/GetRobotMode.html)) Service to query the current robot mode -##### get_safety_mode ([ur_dashboard_msgs/GetSafetyMode](http://docs.ros.org/api/ur_dashboard_msgs/html/srv/GetSafetyMode.html)) +##### get_safety_mode ([ur_dashboard_msgs/GetSafetyMode](https://docs.ros.org/en/humble/p/ur_dashboard_msgs/srv/GetSafetyMode.html)) Service to query the current safety mode -##### load_installation ([ur_dashboard_msgs/Load](http://docs.ros.org/api/ur_dashboard_msgs/html/srv/Load.html)) +##### load_installation ([ur_dashboard_msgs/Load](https://docs.ros.org/en/humble/p/ur_dashboard_msgs/srv/Load.html)) Load a robot installation from a file -##### load_program ([ur_dashboard_msgs/Load](http://docs.ros.org/api/ur_dashboard_msgs/html/srv/Load.html)) +##### load_program ([ur_dashboard_msgs/Load](https://docs.ros.org/en/humble/p/ur_dashboard_msgs/srv/Load.html)) Load a robot program from a file -##### pause ([std_srvs/Trigger](http://docs.ros.org/api/std_srvs/html/srv/Trigger.html)) +##### pause ([std_srvs/Trigger](https://docs.ros.org/en/humble/p/std_srvs/srv/Trigger.html)) Pause a running program. -##### play ([std_srvs/Trigger](http://docs.ros.org/api/std_srvs/html/srv/Trigger.html)) +##### play ([std_srvs/Trigger](https://docs.ros.org/en/humble/p/std_srvs/srv/Trigger.html)) Start execution of a previously loaded program -##### popup ([ur_dashboard_msgs/Popup](http://docs.ros.org/api/ur_dashboard_msgs/html/srv/Popup.html)) +##### popup ([ur_dashboard_msgs/Popup](https://docs.ros.org/en/humble/p/ur_dashboard_msgs/srv/Popup.html)) Service to show a popup on the UR Teach pendant. -##### power_off ([std_srvs/Trigger](http://docs.ros.org/api/std_srvs/html/srv/Trigger.html)) +##### power_off ([std_srvs/Trigger](https://docs.ros.org/en/humble/p/std_srvs/srv/Trigger.html)) Power off the robot motors -##### power_on ([std_srvs/Trigger](http://docs.ros.org/api/std_srvs/html/srv/Trigger.html)) +##### power_on ([std_srvs/Trigger](https://docs.ros.org/en/humble/p/std_srvs/srv/Trigger.html)) Power on the robot motors. To fully start the robot, call 'brake_release' afterwards. -##### program_running ([ur_dashboard_msgs/IsProgramRunning](http://docs.ros.org/api/ur_dashboard_msgs/html/srv/IsProgramRunning.html)) +##### program_running ([ur_dashboard_msgs/IsProgramRunning](https://docs.ros.org/en/humble/p/ur_dashboard_msgs/srv/IsProgramRunning.html)) Query whether there is currently a program running -##### program_saved ([ur_dashboard_msgs/IsProgramSaved](http://docs.ros.org/api/ur_dashboard_msgs/html/srv/IsProgramSaved.html)) +##### program_saved ([ur_dashboard_msgs/IsProgramSaved](https://docs.ros.org/en/humble/p/ur_dashboard_msgs/srv/IsProgramSaved.html)) Query whether the current program is saved -##### program_state ([ur_dashboard_msgs/GetProgramState](http://docs.ros.org/api/ur_dashboard_msgs/html/srv/GetProgramState.html)) +##### program_state ([ur_dashboard_msgs/GetProgramState](https://docs.ros.org/en/humble/p/ur_dashboard_msgs/srv/GetProgramState.html)) Service to query the current program state -##### quit ([ur_dashboard_msgs/GetLoadedProgram](http://docs.ros.org/api/ur_dashboard_msgs/html/srv/GetLoadedProgram.html)) +##### quit ([ur_dashboard_msgs/GetLoadedProgram](https://docs.ros.org/en/humble/p/ur_dashboard_msgs/srv/GetLoadedProgram.html)) Disconnect from the dashboard service. -##### raw_request ([ur_dashboard_msgs/RawRequest](http://docs.ros.org/api/ur_dashboard_msgs/html/srv/RawRequest.html)) +##### raw_request ([ur_dashboard_msgs/RawRequest](https://docs.ros.org/en/humble/p/ur_dashboard_msgs/srv/RawRequest.html)) General purpose service to send arbitrary messages to the dashboard server -##### restart_safety ([std_srvs/Trigger](http://docs.ros.org/api/std_srvs/html/srv/Trigger.html)) +##### restart_safety ([std_srvs/Trigger](https://docs.ros.org/en/humble/p/std_srvs/srv/Trigger.html)) Used when robot gets a safety fault or violation to restart the safety. After safety has been rebooted the robot will be in Power Off. NOTE: You should always ensure it is okay to restart the system. It is highly recommended to check the error log before using this command (either via PolyScope or e.g. ssh connection). -##### shutdown ([std_srvs/Trigger](http://docs.ros.org/api/std_srvs/html/srv/Trigger.html)) +##### shutdown ([std_srvs/Trigger](https://docs.ros.org/en/humble/p/std_srvs/srv/Trigger.html)) Shutdown the robot controller -##### stop ([std_srvs/Trigger](http://docs.ros.org/api/std_srvs/html/srv/Trigger.html)) +##### stop ([std_srvs/Trigger](https://docs.ros.org/en/humble/p/std_srvs/srv/Trigger.html)) Stop program execution on the robot -##### unlock_protective_stop ([std_srvs/Trigger](http://docs.ros.org/api/std_srvs/html/srv/Trigger.html)) +##### unlock_protective_stop ([std_srvs/Trigger](https://docs.ros.org/en/humble/p/std_srvs/srv/Trigger.html)) Dismiss a protective stop to continue robot movements. NOTE: It is the responsibility of the user to ensure the cause of the protective stop is resolved before calling this service. From d1793554741dd87a941b0d3a75b6f3c0f8ffca89 Mon Sep 17 00:00:00 2001 From: Felix Exner Date: Tue, 20 May 2025 11:21:50 +0200 Subject: [PATCH 068/133] README: Update branches for Jazzy and Kilted (#1364) --- README.md | 31 +++++++++++++++++++------------ ci_status.md | 35 +++++++++++++++++++++++++++++------ 2 files changed, 48 insertions(+), 18 deletions(-) diff --git a/README.md b/README.md index 72f1f3ba8..1fb75aa35 100644 --- a/README.md +++ b/README.md @@ -20,19 +20,19 @@ Check also [presentations and videos](ur_robot_driver/doc/resources/README.md) a + - - - - - - - - - - - - + + + + + + + + + + + + + @@ -31,19 +33,33 @@ red pipeline there should be a corresponding issue labeled with [ci-failure](htt
+ +
HumbleIron Jazzy Rolling
humbleiron main main
- - Iron Binary Main -
- - Iron Binary Testing -
- - Iron Semi-Binary Main -
- - Iron Semi-Binary Testing - - 		-
- - - - -
From 616c8e09d4ce428134755b7bf21e1db096e387e7 Mon Sep 17 00:00:00 2001 From: Felix Exner Date: Tue, 21 Jan 2025 08:47:02 +0000 Subject: [PATCH 039/133] Update changelogs for upcoming release --- ur/CHANGELOG.rst | 3 +++ ur_bringup/CHANGELOG.rst | 5 +++++ ur_calibration/CHANGELOG.rst | 3 +++ ur_controllers/CHANGELOG.rst | 5 +++++ ur_dashboard_msgs/CHANGELOG.rst | 3 +++ ur_moveit_config/CHANGELOG.rst | 5 +++++ ur_robot_driver/CHANGELOG.rst | 10 ++++++++++ 7 files changed, 34 insertions(+) diff --git a/ur/CHANGELOG.rst b/ur/CHANGELOG.rst index d7e2eb858..342545ee6 100644 --- a/ur/CHANGELOG.rst +++ b/ur/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package ur ^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 2.5.1 (2024-12-21) ------------------ diff --git a/ur_bringup/CHANGELOG.rst b/ur_bringup/CHANGELOG.rst index 23a64b6f2..6acf9a478 100644 --- a/ur_bringup/CHANGELOG.rst +++ b/ur_bringup/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package ur_bringup ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Update pre-commit the same as on the main branch (`#1232 `_) +* Contributors: Felix Exner + 2.5.1 (2024-12-21) ------------------ diff --git a/ur_calibration/CHANGELOG.rst b/ur_calibration/CHANGELOG.rst index 31db24a59..9e2c61042 100644 --- a/ur_calibration/CHANGELOG.rst +++ b/ur_calibration/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package ur_calibration ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 2.5.1 (2024-12-21) ------------------ diff --git a/ur_controllers/CHANGELOG.rst b/ur_controllers/CHANGELOG.rst index c229959b8..91ec2b032 100644 --- a/ur_controllers/CHANGELOG.rst +++ b/ur_controllers/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package ur_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Update pre-commit the same as on the main branch (`#1232 `_) +* Contributors: Felix Exner + 2.5.1 (2024-12-21) ------------------ * Add missing test dependencies for ur_controllers (backport of `#1215 `_) diff --git a/ur_dashboard_msgs/CHANGELOG.rst b/ur_dashboard_msgs/CHANGELOG.rst index c15e7c1fa..d8a47cafc 100644 --- a/ur_dashboard_msgs/CHANGELOG.rst +++ b/ur_dashboard_msgs/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package ur_dashboard_msgs ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 2.5.1 (2024-12-21) ------------------ diff --git a/ur_moveit_config/CHANGELOG.rst b/ur_moveit_config/CHANGELOG.rst index cde6861b6..251bea9b7 100644 --- a/ur_moveit_config/CHANGELOG.rst +++ b/ur_moveit_config/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package ur_moveit_config ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* ur_moveit_config: Do not change default controller when using fake hardware (`#1237 `_) +* Contributors: Felix Exner + 2.5.1 (2024-12-21) ------------------ diff --git a/ur_robot_driver/CHANGELOG.rst b/ur_robot_driver/CHANGELOG.rst index ef79e8d8d..05870469d 100644 --- a/ur_robot_driver/CHANGELOG.rst +++ b/ur_robot_driver/CHANGELOG.rst @@ -1,3 +1,13 @@ +Forthcoming +----------- +* Check quaternions for equal dot_product instead of comparing their components individually (backport `#1238 `_) (`#1243 `_) +* fix sphinx doc link in ur_robot_driver (`#1240 `_) (`#1242 `_) +* Update pre-commit the same as on the main branch (`#1232 `_) +* Disable pose broadcaster on mock hardware (backport of `#1229 `_) (`#1230 `_) +* Remove unused include (backport of `#1220 `_) + Co-authored-by: Bence Magyar +* Contributors: Felix Exner, mergify[bot] + 2.5.1 (2024-12-21) ------------------ From ea7686607f697525f0524955c6cb3b9459f27216 Mon Sep 17 00:00:00 2001 From: Felix Exner Date: Tue, 21 Jan 2025 08:48:08 +0000 Subject: [PATCH 040/133] 2.5.2 --- ur/CHANGELOG.rst | 4 ++-- ur/package.xml | 2 +- ur_bringup/CHANGELOG.rst | 4 ++-- ur_bringup/package.xml | 2 +- ur_calibration/CHANGELOG.rst | 4 ++-- ur_calibration/package.xml | 2 +- ur_controllers/CHANGELOG.rst | 4 ++-- ur_controllers/package.xml | 2 +- ur_dashboard_msgs/CHANGELOG.rst | 4 ++-- ur_dashboard_msgs/package.xml | 2 +- ur_moveit_config/CHANGELOG.rst | 4 ++-- ur_moveit_config/package.xml | 2 +- ur_robot_driver/CHANGELOG.rst | 4 ++-- ur_robot_driver/package.xml | 2 +- 14 files changed, 21 insertions(+), 21 deletions(-) diff --git a/ur/CHANGELOG.rst b/ur/CHANGELOG.rst index 342545ee6..9cdb63792 100644 --- a/ur/CHANGELOG.rst +++ b/ur/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package ur ^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.5.2 (2025-01-21) +------------------ 2.5.1 (2024-12-21) ------------------ diff --git a/ur/package.xml b/ur/package.xml index 4c104bed2..b88fb0579 100644 --- a/ur/package.xml +++ b/ur/package.xml @@ -2,7 +2,7 @@ ur - 2.5.1 + 2.5.2 Metapackage for universal robots Felix Exner diff --git a/ur_bringup/CHANGELOG.rst b/ur_bringup/CHANGELOG.rst index 6acf9a478..28b5d359e 100644 --- a/ur_bringup/CHANGELOG.rst +++ b/ur_bringup/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package ur_bringup ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.5.2 (2025-01-21) +------------------ * Update pre-commit the same as on the main branch (`#1232 `_) * Contributors: Felix Exner diff --git a/ur_bringup/package.xml b/ur_bringup/package.xml index ede082a14..9561da9ba 100644 --- a/ur_bringup/package.xml +++ b/ur_bringup/package.xml @@ -2,7 +2,7 @@ ur_bringup - 2.5.1 + 2.5.2 Launch file and run-time configurations, e.g. controllers. Felix Exner diff --git a/ur_calibration/CHANGELOG.rst b/ur_calibration/CHANGELOG.rst index 9e2c61042..8e5b75147 100644 --- a/ur_calibration/CHANGELOG.rst +++ b/ur_calibration/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package ur_calibration ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.5.2 (2025-01-21) +------------------ 2.5.1 (2024-12-21) ------------------ diff --git a/ur_calibration/package.xml b/ur_calibration/package.xml index 5546a7a88..67de5a3ca 100644 --- a/ur_calibration/package.xml +++ b/ur_calibration/package.xml @@ -1,7 +1,7 @@ ur_calibration - 2.5.1 + 2.5.2 Package for extracting the factory calibration from a UR robot and change it such that it can be used by ur_description to gain a correct URDF Felix Exner diff --git a/ur_controllers/CHANGELOG.rst b/ur_controllers/CHANGELOG.rst index 91ec2b032..dbd46a0dd 100644 --- a/ur_controllers/CHANGELOG.rst +++ b/ur_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package ur_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.5.2 (2025-01-21) +------------------ * Update pre-commit the same as on the main branch (`#1232 `_) * Contributors: Felix Exner diff --git a/ur_controllers/package.xml b/ur_controllers/package.xml index b72920a04..5ffeea8bb 100644 --- a/ur_controllers/package.xml +++ b/ur_controllers/package.xml @@ -2,7 +2,7 @@ ur_controllers - 2.5.1 + 2.5.2 Provides controllers that use the speed scaling interface of Universal Robots. Felix Exner diff --git a/ur_dashboard_msgs/CHANGELOG.rst b/ur_dashboard_msgs/CHANGELOG.rst index d8a47cafc..8b5633009 100644 --- a/ur_dashboard_msgs/CHANGELOG.rst +++ b/ur_dashboard_msgs/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package ur_dashboard_msgs ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.5.2 (2025-01-21) +------------------ 2.5.1 (2024-12-21) ------------------ diff --git a/ur_dashboard_msgs/package.xml b/ur_dashboard_msgs/package.xml index 12f39a421..de4cb451a 100644 --- a/ur_dashboard_msgs/package.xml +++ b/ur_dashboard_msgs/package.xml @@ -2,7 +2,7 @@ ur_dashboard_msgs - 2.5.1 + 2.5.2 Messages around the UR Dashboard server. Felix Exner diff --git a/ur_moveit_config/CHANGELOG.rst b/ur_moveit_config/CHANGELOG.rst index 251bea9b7..12234f566 100644 --- a/ur_moveit_config/CHANGELOG.rst +++ b/ur_moveit_config/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package ur_moveit_config ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.5.2 (2025-01-21) +------------------ * ur_moveit_config: Do not change default controller when using fake hardware (`#1237 `_) * Contributors: Felix Exner diff --git a/ur_moveit_config/package.xml b/ur_moveit_config/package.xml index 11f07baea..fa9e18e97 100644 --- a/ur_moveit_config/package.xml +++ b/ur_moveit_config/package.xml @@ -2,7 +2,7 @@ ur_moveit_config - 2.5.1 + 2.5.2 An example package with MoveIt2 configurations for UR robots. diff --git a/ur_robot_driver/CHANGELOG.rst b/ur_robot_driver/CHANGELOG.rst index 05870469d..d312a3241 100644 --- a/ur_robot_driver/CHANGELOG.rst +++ b/ur_robot_driver/CHANGELOG.rst @@ -1,5 +1,5 @@ -Forthcoming ------------ +2.5.2 (2025-01-21) +------------------ * Check quaternions for equal dot_product instead of comparing their components individually (backport `#1238 `_) (`#1243 `_) * fix sphinx doc link in ur_robot_driver (`#1240 `_) (`#1242 `_) * Update pre-commit the same as on the main branch (`#1232 `_) diff --git a/ur_robot_driver/package.xml b/ur_robot_driver/package.xml index 142ec4166..1cb83c122 100644 --- a/ur_robot_driver/package.xml +++ b/ur_robot_driver/package.xml @@ -2,7 +2,7 @@ ur_robot_driver - 2.5.1 + 2.5.2 The new driver for Universal Robots UR3, UR5 and UR10 robots with CB3 controllers and the e-series. Felix Exner From d73b078f795aa4f8f4365ede3e8c8d9dd2927134 Mon Sep 17 00:00:00 2001 From: "mergify[bot]" <37929162+mergify[bot]@users.noreply.github.com> Date: Mon, 27 Jan 2025 14:07:05 +0100 Subject: [PATCH 041/133] Auto-update pre-commit hooks (#1255) --- .pre-commit-config.yaml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml index aa81841f3..4392673f9 100644 --- a/.pre-commit-config.yaml +++ b/.pre-commit-config.yaml @@ -135,7 +135,7 @@ repos: # Spellcheck in comments and docs # skipping of *.svg files is not working... - repo: https://github.com/codespell-project/codespell - rev: v2.3.0 + rev: v2.4.0 hooks: - id: codespell args: ['--write-changes', '-L bootup,assertIn'] From 4d344d06efc84a46898b353b8f09929d16504d04 Mon Sep 17 00:00:00 2001 From: Felix Exner Date: Thu, 30 Jan 2025 08:54:55 +0100 Subject: [PATCH 042/133] Remove urdf folder (#1257) That has been falsely added in 92b1bea47a939a266378f3095c1a87a6176f8537 --- ur_robot_driver/urdf/ur.ros2_control.xacro | 303 --------------------- 1 file changed, 303 deletions(-) delete mode 100644 ur_robot_driver/urdf/ur.ros2_control.xacro diff --git a/ur_robot_driver/urdf/ur.ros2_control.xacro b/ur_robot_driver/urdf/ur.ros2_control.xacro deleted file mode 100644 index 91e73a888..000000000 --- a/ur_robot_driver/urdf/ur.ros2_control.xacro +++ /dev/null @@ -1,303 +0,0 @@ - - - - - - - - - - - - - - - - - - - - mock_components/GenericSystem - ${mock_sensor_commands} - 0.0 - true - - - ur_robot_driver/URPositionHardwareInterface - ${robot_ip} - ${script_filename} - ${output_recipe_filename} - ${input_recipe_filename} - ${headless_mode} - ${reverse_port} - ${script_sender_port} - ${reverse_ip} - ${script_command_port} - ${trajectory_port} - ${tf_prefix} - ${non_blocking_read} - 2000 - 0.03 - ${use_tool_communication} - ${kinematics_hash} - ${tool_voltage} - ${tool_parity} - ${tool_baud_rate} - ${tool_stop_bits} - ${tool_rx_idle_chars} - ${tool_tx_idle_chars} - ${tool_device_name} - ${tool_tcp_port} - ${robot_receive_timeout} - - - - - - - - - - - 1.0 - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - From ac2aabe6441d4c83bc02e8e100347f4813761c3b Mon Sep 17 00:00:00 2001 From: Felix Exner Date: Thu, 30 Jan 2025 13:20:38 +0100 Subject: [PATCH 043/133] Fix doc links (#1247) * Add check_links workflow * Use urcl start_ursim from ur_bringup, as well * Update links in README --- .github/helpers/check_urls.sh | 67 ++++ .github/workflows/check_links.yml | 19 + README.md | 18 +- ur_bringup/scripts/start_ursim.sh | 96 +---- ur_robot_driver/README.md | 2 +- ur_robot_driver/doc/installation/real_time.md | 282 --------------- .../doc/installation/real_time.rst | 334 ++++++++++++++++++ 7 files changed, 435 insertions(+), 383 deletions(-) create mode 100755 .github/helpers/check_urls.sh create mode 100644 .github/workflows/check_links.yml delete mode 100644 ur_robot_driver/doc/installation/real_time.md create mode 100644 ur_robot_driver/doc/installation/real_time.rst diff --git a/.github/helpers/check_urls.sh b/.github/helpers/check_urls.sh new file mode 100755 index 000000000..f7222d115 --- /dev/null +++ b/.github/helpers/check_urls.sh @@ -0,0 +1,67 @@ +#!/bin/bash + +set -e + +IGNORE_FILES="" +IGNORE_PATTERNS="" + +while getopts ":f:d:p:" opt; do + case "${opt}" in + f) + IGNORE_FILES="${OPTARG}";; + d) + IGNORE_DIRS="${OPTARG}";; + p) + IGNORE_PATTERNS="${OPTARG}";; + \?) + echo "Invalid option -$OPTARG" + exit;; + esac +done + +read -r -a ignore_files <<<"$IGNORE_FILES" +read -r -a ignore_dirs <<<"$IGNORE_DIRS" +read -r -a ignore_patterns <<<"$IGNORE_PATTERNS" + +IGNORE_FILES_ARG="" +for item in "${ignore_files[@]}"; do + IGNORE_FILES_ARG="$IGNORE_FILES_ARG --exclude=$item" +done +IGNORE_DIRS_ARG="" +for item in "${ignore_dirs[@]}"; do + IGNORE_DIRS_ARG="$IGNORE_DIRS_ARG --exclude-dir=$item" +done + +#Find URLs in code: +urls=$(grep -oP '(http|ftp|https):\/\/([a-zA-Z0-9_-]+(?:(?:\.[a-zA-Z0-9_-]+)+))([a-zA-Z0-9_.,@?^=%&:\/~+#-]*[a-zA-Z0-9_@?^=%&\/~+#-])?' -rI $IGNORE_FILES_ARG $IGNORE_DIRS_ARG) + +fail_counter=0 + +FAILED_LINKS=() +for item in $urls; do +# echo $item + skip=0 + for pattern in "${ignore_patterns[@]}"; do + [[ "$item" =~ $pattern ]] && skip=1 + done + + if [[ $skip == 1 ]]; then + echo "SKIPPING $item" + continue + fi + + filename=$(echo "$item" | cut -d':' -f1) + url=$(echo "$item" | cut -d':' -f2-) + echo -n "Checking $url from file $filename" + if ! curl --head --silent --fail "$url" 2>&1 > /dev/null; then + echo -e " \033[0;31mNOT FOUND\033[32m\n" + FAILED_LINKS+=("$url from file $filename") + ((fail_counter=fail_counter+1)) + else + printf " \033[32mok\033[0m\n" + fi +done + +echo "Failed files:" +printf '%s\n' "${FAILED_LINKS[@]}" +exit $fail_counter diff --git a/.github/workflows/check_links.yml b/.github/workflows/check_links.yml new file mode 100644 index 000000000..dc662f7b2 --- /dev/null +++ b/.github/workflows/check_links.yml @@ -0,0 +1,19 @@ +name: Check Links +on: + workflow_dispatch: + pull_request: + push: + branches: + - main + +jobs: + check_links: + runs-on: ubuntu-latest + steps: + - uses: actions/checkout@v4 + - name: Check URLs + run: | + .github/helpers/check_urls.sh \ + -d ".git build CMakeModules debian" \ + -f "package.xml ursim_docker.rst architecture_coarse.svg" \ + -p "vnc\.html opensource\.org\/licenses\/BSD-3-Clause kernel\.org\/pub\/linux\/kernel" diff --git a/README.md b/README.md index 4a5f1e1e7..665854712 100644 --- a/README.md +++ b/README.md @@ -97,23 +97,23 @@ For getting started, you'll basically need three steps: ```bash sudo apt-get install ros-humble-ur ``` - See the [installation instructions](https://docs.ros.org/en/ros2_packages/humble/api/ur_robot_driver/installation/installation.html) for more details and source-build instructions. + See the [installation instructions](https://docs.ros.org/en/ros2_packages/humble/api/ur_robot_driver/doc/installation/installation.html) for more details and source-build instructions. 2. **Start & Setup the robot**. Once you've installed the driver, [setup the - robot](https://docs.ros.org/en/ros2_packages/humble/api/ur_robot_driver/installation/robot_setup.html) + robot](https://docs.ros.org/en/ros2_packages/humble/api/ur_robot_driver/doc/installation/robot_setup.html) and [create a program for external - control](https://docs.ros.org/en/ros2_packages/humble/api/ur_robot_driver/installation/install_urcap_e_series.html). + control](https://docs.ros.org/en/ros2_packages/humble/api/ur_robot_driver/doc/installation/install_urcap_e_series.html). Please do this step carefully and extract the calibration as explained - [here](https://docs.ros.org/en/ros2_packages/humble/api/ur_robot_driver/installation/robot_setup.html#extract-calibration-information). + [here](https://docs.ros.org/en/ros2_packages/humble/api/ur_robot_driver/doc/installation/robot_setup.html#extract-calibration-information). Otherwise the TCP's pose will not be correct inside the ROS ecosystem. If no real robot is required, you can [use a simulated - robot](https://docs.ros.org/en/ros2_packages/humble/api/ur_robot_driver/usage.html#usage-with-official-ur-simulator) + robot](https://docs.ros.org/en/ros2_packages/humble/api/ur_robot_driver/doc/usage.html#usage-with-official-ur-simulator) that will behave almost exactly like the real robot. 3. **Start the driver**. See the [usage - documentation](https://docs.ros.org/en/ros2_packages/humble/api/ur_robot_driver/usage.html) for + documentation](https://docs.ros.org/en/ros2_packages/humble/api/ur_robot_driver/doc/usage.html) for details. ```bash @@ -122,7 +122,7 @@ For getting started, you'll basically need three steps: ros2 launch ur_robot_driver ur_control.launch.py ur_type:=ur5e robot_ip:=192.168.56.101 ``` -4. Unless started in [headless mode](https://docs.ros.org/en/ros2_packages/humble/api/ur_robot_driver/ROS_INTERFACE.html#headless-mode): Run the external_control program by **pressing `play` on the teach pendant**. +4. Unless started in [headless mode](https://docs.ros.org/en/ros2_packages/humble/api/ur_robot_driver/doc/ROS_INTERFACE.html#headless-mode): Run the external_control program by **pressing `play` on the teach pendant**. ## MoveIt! support @@ -135,8 +135,8 @@ Watch MoveIt in action with the Universal Robots ROS2 driver: *The video shows free-space trajectory planning around a modeled collision scene object using the MoveIt2 MotionPlanning widget for Rviz2.* See the [MoveIt! -section](https://docs.ros.org/en/ros2_packages/humble/api/ur_robot_driver/usage.html#using-moveit) -of the [Usage guide](https://docs.ros.org/en/ros2_packages/humble/api/ur_robot_driver/usage.html) +section](https://docs.ros.org/en/ros2_packages/humble/api/ur_robot_driver/doc/usage.html#using-moveit) +of the [Usage guide](https://docs.ros.org/en/ros2_packages/humble/api/ur_robot_driver/doc/usage.html) for details. diff --git a/ur_bringup/scripts/start_ursim.sh b/ur_bringup/scripts/start_ursim.sh index a8dbe9bf1..3c60f1ed2 100755 --- a/ur_bringup/scripts/start_ursim.sh +++ b/ur_bringup/scripts/start_ursim.sh @@ -28,102 +28,16 @@ # ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE # POSSIBILITY OF SUCH DAMAGE. -PERSISTENT_BASE="${HOME}/.ursim" -URCAP_VERSION="1.0.5" echo -e "\033[0;31mDEPRECATION WARNING: " \ "Launch files from the ur_bringup package are deprecated and will be removed from Iron " \ "Irwini on. Please use the same launch files from the ur_robot_driver package." \ "\033[0m" -help() -{ - # Display Help - echo "Starts URSim inside a docker container" - echo - echo "Syntax: `basename "$0"` [-m|s|h]" - echo "options:" - echo " -m Robot model. One of [ur3, ur3e, ur5, ur5e, ur10, ur10e, ur16e]. Defaults to ur5e." - echo " -h Print this Help." - echo -} +ORANGE='\033[0;33m' +NC='\033[0m' # No Color -ROBOT_MODEL=UR5 -ROBOT_SERIES=e-series +URSIM_CMD="ros2 run ur_client_library start_ursim.sh" -validate_model() -{ - case $ROBOT_MODEL in - ur3|ur5|ur10) - ROBOT_MODEL=${ROBOT_MODEL^^} - ROBOT_SERIES=cb3 - ;; - ur3e|ur5e|ur10e|ur16e) - ROBOT_MODEL=${ROBOT_MODEL^^} - ROBOT_MODEL=$(echo ${ROBOT_MODEL:0:$((${#ROBOT_MODEL}-1))}) - ROBOT_SERIES=e-series - ;; - *) - echo "Not a valid robot model: $ROBOT_MODEL" - exit - ;; - esac -} - - -while getopts ":hm:s:" option; do - case $option in - h) # display Help - help - exit;; - m) # robot model - ROBOT_MODEL=${OPTARG} - validate_model - ;; - \?) # invalid option - echo "Error: Invalid option" - help - exit;; - esac -done - -URCAP_STORAGE="${PERSISTENT_BASE}/${ROBOT_SERIES}/urcaps" -PROGRAM_STORAGE="${PERSISTENT_BASE}/${ROBOT_SERIES}/programs" - -# Create local storage for programs and URCaps -mkdir -p "${URCAP_STORAGE}" -mkdir -p "${PROGRAM_STORAGE}" - -# Download external_control URCap -if [[ ! -f "${URCAP_STORAGE}/externalcontrol-${URCAP_VERSION}.jar" ]]; then - curl -L -o "${URCAP_STORAGE}/externalcontrol-${URCAP_VERSION}.jar" \ - "https://github.com/UniversalRobots/Universal_Robots_ExternalControl_URCap/releases/download/v${URCAP_VERSION}/externalcontrol-${URCAP_VERSION}.jar" -fi - -# Check whether network already exists -docker network inspect ursim_net > /dev/null -if [ $? -eq 0 ]; then - echo "ursim_net already exists" -else - echo "Creating ursim_net" - docker network create --subnet=192.168.56.0/24 ursim_net -fi - -# run docker container -docker run --rm -d --net ursim_net --ip 192.168.56.101\ - -v "${URCAP_STORAGE}":/urcaps \ - -v "${PROGRAM_STORAGE}":/ursim/programs \ - -e ROBOT_MODEL="${ROBOT_MODEL}" \ - --name ursim \ - universalrobots/ursim_${ROBOT_SERIES} || exit - -trap "echo killing; docker container kill ursim; exit" SIGINT SIGTERM - -echo "Docker URSim is running" -printf "\nTo access Polyscope, open the following URL in a web browser.\n\thttp://192.168.56.101:6080/vnc.html\n\n" -echo "To exit, press CTRL+C" - -while : -do - sleep 1 -done +echo -e "${ORANGE} DEPRECATION WARNING: The script starting URSim was moved to the ur_client_library package. This script here does still work, but will be removed with ROS Jazzy. Please use `${URSIM_CMD}` to start URSim in future." +$URSIM_CMD diff --git a/ur_robot_driver/README.md b/ur_robot_driver/README.md index 4f520eb63..69234b2ed 100644 --- a/ur_robot_driver/README.md +++ b/ur_robot_driver/README.md @@ -33,7 +33,7 @@ request. **Custom script snippets** can be sent to the robot on a topic basis. By default, they will interrupt other programs (such as the one controlling the robot). For a certain subset of functions, it is however possible to send them as secondary programs. See [UR -documentation](https://www.universal-robots.com/how-tos-and-faqs/how-to/ur-how-tos/secondary-program-17257/) +documentation](https://www.universal-robots.com/articles/ur/programming/secondary-program/) on details.
**Note to e-Series users:** diff --git a/ur_robot_driver/doc/installation/real_time.md b/ur_robot_driver/doc/installation/real_time.md deleted file mode 100644 index e55a9e638..000000000 --- a/ur_robot_driver/doc/installation/real_time.md +++ /dev/null @@ -1,282 +0,0 @@ -# Setting up Ubuntu with a PREEMPT_RT kernel -In order to run the `universal_robot_driver`, we highly recommend to setup a ubuntu system with -real-time capabilities. Especially with a robot from the e-Series the higher control frequency -might lead to non-smooth trajectory execution if not run using a real-time-enabled system. - -You might still be able to control the robot using a non-real-time system. This is, however, not recommended. - -To get real-time support into a ubuntu system, the following steps have to be performed: - 1. Get the sources of a real-time kernel - 2. Compile the real-time kernel - 3. Setup user privileges to execute real-time tasks - -This guide will help you setup your system with a real-time kernel. - -## Preparing -To build the kernel, you will need a couple of tools available on your system. You can install them -using - -``` bash -$ sudo apt-get install build-essential bc ca-certificates gnupg2 libssl-dev wget gawk flex bison -``` - -Before you download the sources of a real-time-enabled kernel, check the kernel version that is currently installed: - -```bash -$ uname -r -4.15.0-62-generic -``` - -To continue with this tutorial, please create a temporary folder and navigate into it. You should -have sufficient space (around 25GB) there, as the extracted kernel sources take much space. After -the new kernel is installed, you can delete this folder again. - -In this example we will use a temporary folder inside our home folder: - -```bash -$ mkdir -p ${HOME}/rt_kernel_build -$ cd ${HOME}/rt_kernel_build -``` - -All future commands are expected to be run inside this folder. If the folder is different, the `$` -sign will be prefixed with a path relative to the above folder. - -## Getting the sources for a real-time kernel -To build a real-time kernel, we first need to get the kernel sources and the real-time patch. - -First, we must decide on the kernel version that we want to use. Above, we -determined that our system has a 4.15 kernel installed. However, real-time -patches exist only for selected kernel versions. Those can be found on the -[linuxfoundation wiki](https://wiki.linuxfoundation.org/realtime/preempt_rt_versions). - -In this example, we will select a 4.14 kernel. Select a kernel version close to the -one installed on your system. - -Go ahead and download the kernel sources, patch sources and their signature files: - -```bash -$ wget https://cdn.kernel.org/pub/linux/kernel/projects/rt/4.14/patch-4.14.139-rt66.patch.xz -$ wget https://cdn.kernel.org/pub/linux/kernel/projects/rt/4.14/patch-4.14.139-rt66.patch.sign -$ wget https://www.kernel.org/pub/linux/kernel/v4.x/linux-4.14.139.tar.xz -$ wget https://www.kernel.org/pub/linux/kernel/v4.x/linux-4.14.139.tar.sign -``` - -To unzip the downloaded files do -```bash -$ xz -dk patch-4.14.139-rt66.patch.xz -$ xz -d linux-4.14.139.tar.xz -``` - -### Verification -Technically, you can skip this section, it is however highly recommended to verify the file -integrity of such a core component of your system! - -To verify file integrity, you must first import public keys by the kernel developers and the patch -author. For the kernel sources use (as suggested on -[kernel.org](https://www.kernel.org/signature.html)) - -```bash -$ gpg2 --locate-keys torvalds@kernel.org gregkh@kernel.org -``` - -and for the patch search for a key of the author listed on -[linuxfoundation wiki](https://wiki.linuxfoundation.org/realtime/preempt_rt_versions). - -```bash -$ gpg2 --keyserver hkp://keys.gnupg.net --search-keys zanussi -gpg: data source: http://51.38.91.189:11371 -(1) German Daniel Zanussi - 4096 bit RSA key 0x537F98A9D92CEAC8, created: 2019-07-24, expires: 2023-07-24 -(2) Michael Zanussi - 4096 bit RSA key 0x7C7F76A2C1E3D9EB, created: 2019-05-08 -(3) Tom Zanussi - Tom Zanussi - Tom Zanussi - 4096 bit RSA key 0xDE09826778A38521, created: 2017-12-15 -(4) Riccardo Zanussi - 2048 bit RSA key 0xD299A06261D919C3, created: 2014-08-27, expires: 2018-08-27 (expired) -(5) Zanussi Gianni - 1024 bit DSA key 0x78B89CB020D1836C, created: 2004-04-06 -(6) Michael Zanussi - Michael Zanussi - Michael Zanussi - Michael Zanussi - 1024 bit DSA key 0xB3E952DCAC653064, created: 2000-09-05 -(7) Michael Zanussi - 1024 bit DSA key 0xEB10BBD9BA749318, created: 1999-05-31 -(8) Michael B. Zanussi - 1024 bit DSA key 0x39EE4EAD7BBB1E43, created: 1998-07-16 -Keys 1-8 of 8 for "zanussi". Enter number(s), N)ext, or Q)uit > 3 -``` - -Now we can verify the downloaded sources: -```bash -$ gpg2 --verify linux-4.14.139.tar.sign -gpg: assuming signed data in 'linux-4.14.139.tar' -gpg: Signature made Fr 16 Aug 2019 10:15:17 CEST -gpg: using RSA key 647F28654894E3BD457199BE38DBBDC86092693E -gpg: Good signature from "Greg Kroah-Hartman " [unknown] -gpg: WARNING: This key is not certified with a trusted signature! -gpg: There is no indication that the signature belongs to the owner. -Primary key fingerprint: 647F 2865 4894 E3BD 4571 99BE 38DB BDC8 6092 693E - -$ gpg2 --verify patch-4.14.139-rt66.patch.sign -gpg: assuming signed data in 'patch-4.14.139-rt66.patch' -gpg: Signature made Fr 23 Aug 2019 21:09:20 CEST -gpg: using RSA key 0x0129F38552C38DF1 -gpg: Good signature from "Tom Zanussi " [unknown] -gpg: aka "Tom Zanussi " [unknown] -gpg: aka "Tom Zanussi " [unknown] -gpg: WARNING: This key is not certified with a trusted signature! -gpg: There is no indication that the signature belongs to the owner. -Primary key fingerprint: 5BDF C45C 2ECC 5387 D50C E5EF DE09 8267 78A3 8521 - Subkey fingerprint: ACF8 5F98 16A8 D5F0 96AE 1FD2 0129 F385 52C3 8DF1 -``` - -## Compilation -Before we can compile the sources, we have to extract the tar archive and apply the patch - -```bash -$ tar xf linux-4.14.139.tar -$ cd linux-4.14.139 -linux-4.14.139$ xzcat ../patch-4.14.139-rt66.patch.xz | patch -p1 -``` - -Now to configure your kernel, just type -```bash -linux-4.14.139$ make oldconfig -``` - -This will ask for kernel options. For everything else then the `Preemption Model` use the default -value (just press Enter) or adapt to your preferences. For the preemption model select `Fully Preemptible Kernel`: - -```bash -Preemption Model - 1. No Forced Preemption (Server) (PREEMPT_NONE) -> 2. Voluntary Kernel Preemption (Desktop) (PREEMPT_VOLUNTARY) - 3. Preemptible Kernel (Low-Latency Desktop) (PREEMPT__LL) (NEW) - 4. Preemptible Kernel (Basic RT) (PREEMPT_RTB) (NEW) - 5. Fully Preemptible Kernel (RT) (PREEMPT_RT_FULL) (NEW) -choice[1-5]: 5 -``` - -Now you can build the kernel. This will take some time... - -```bash -linux-4.14.139$ make -j `getconf _NPROCESSORS_ONLN` deb-pkg -``` - -After building, install the `linux-headers` and `linux-image` packages in the parent folder (only -the ones without the `-dbg` in the name) - -```bash -linux-4.14.139$ sudo apt install ../linux-headers-4.14.139-rt66_*.deb ../linux-image-4.14.139-rt66_*.deb -``` - -## Setup user privileges to use real-time scheduling -To be able to schedule threads with user privileges (what the driver will do) you'll have to change -the user's limits by changing `/etc/security/limits.conf` (See [the manpage](https://manpages.ubuntu.com/manpages/bionic/man5/limits.conf.5.html) for details) - -We recommend to setup a group for real-time users instead of writing a fixed username into the config -file: - -```bash -$ sudo groupadd realtime -$ sudo usermod -aG realtime $(whoami) -``` - -Then, make sure `/etc/security/limits.conf` contains -``` -@realtime soft rtprio 99 -@realtime soft priority 99 -@realtime soft memlock 102400 -@realtime hard rtprio 99 -@realtime hard priority 99 -@realtime hard memlock 102400 -``` - -Note: You will have to log out and log back in (Not only close your terminal window) for these -changes to take effect. No need to do this now, as we will reboot later on, anyway. - -## Setup GRUB to always boot the real-time kernel -To make the new kernel the default kernel that the system will boot into every time, you'll have to -change the grub config file inside `/etc/default/grub`. - -Note: This works for ubuntu, but might not be working for other linux systems. It might be necessary -to use another menuentry name there. - -But first, let's find out the name of the entry that we will want to make the default. You can list -all available kernels using - -```bash -$ awk -F\' '/menuentry |submenu / {print $1 $2}' /boot/grub/grub.cfg - -menuentry Ubuntu -submenu Advanced options for Ubuntu - menuentry Ubuntu, with Linux 4.15.0-62-generic - menuentry Ubuntu, with Linux 4.15.0-62-generic (recovery mode) - menuentry Ubuntu, with Linux 4.15.0-60-generic - menuentry Ubuntu, with Linux 4.15.0-60-generic (recovery mode) - menuentry Ubuntu, with Linux 4.15.0-58-generic - menuentry Ubuntu, with Linux 4.15.0-58-generic (recovery mode) - menuentry Ubuntu, with Linux 4.14.139-rt66 - menuentry Ubuntu, with Linux 4.14.139-rt66 (recovery mode) -menuentry Memory test (memtest86+) -menuentry Memory test (memtest86+, serial console 115200) -menuentry Windows 7 (on /dev/sdc2) -menuentry Windows 7 (on /dev/sdc3) -``` - -From the output above, we'll need to generate a string with the pattern `"submenu_name>entry_name"`. In our case this would be - -``` -"Advanced options for Ubuntu>Ubuntu, with Linux 4.14.139-rt66" -``` -**The double quotes and no spaces around the `>` are important!** - -With this, we can setup the default grub entry and then update the grub menu entries. Don't forget this last step! - -```bash -$ sudo sed -i 's/^GRUB_DEFAULT=.*/GRUB_DEFAULT="Advanced options for Ubuntu>Ubuntu, with Linux 4.14.139-rt66"/' /etc/default/grub -$ sudo update-grub -``` - -## Reboot the PC -After having performed the above mentioned steps, reboot the PC. It should boot into the correct -kernel automatically. - -## Check for preemption capabilities -Make sure that the kernel does indeed support real-time scheduling: - -```bash -$ uname -v | cut -d" " -f1-4 -#1 SMP PREEMPT RT -``` - -## Optional: Disable CPU speed scaling -Many modern CPUs support changing their clock frequency dynamically depending on the currently -requested computation resources. In some cases this can lead to small interruptions in execution. -While the real-time scheduled controller thread should be unaffected by this, any external -components such as a visual servoing system might be interrupted for a short period on scaling -changes. - -To check and modify the power saving mode, install cpufrequtils: -```bash -$ sudo apt install cpufrequtils -``` - -Run `cpufreq-info` to check available "governors" and the current CPU Frequency (`current CPU -frequency is XXX MHZ`). In the following we will set the governor to "performance". - -```bash -$ sudo systemctl disable ondemand -$ sudo systemctl enable cpufrequtils -$ sudo sh -c 'echo "GOVERNOR=performance" > /etc/default/cpufrequtils' -$ sudo systemctl daemon-reload && sudo systemctl restart cpufrequtils -``` - -This disables the `ondemand` CPU scaling daemon, creates a `cpufrequtils` config file and restarts -the `cpufrequtils` service. Check with `cpufreq-info`. - -For further information about governors, please see the [kernel -documentation](https://www.kernel.org/doc/Documentation/cpu-freq/governors.txt). diff --git a/ur_robot_driver/doc/installation/real_time.rst b/ur_robot_driver/doc/installation/real_time.rst new file mode 100644 index 000000000..deb347c05 --- /dev/null +++ b/ur_robot_driver/doc/installation/real_time.rst @@ -0,0 +1,334 @@ +.. _real time setup: + +Setup for real-time scheduling +============================== + +In order to run the ``universal_robot_driver``, we highly recommend to setup a ubuntu system with +real-time capabilities. Especially with a robot from the e-Series the higher control frequency +might lead to non-smooth trajectory execution if not run using a real-time-enabled system. + +You might still be able to control the robot using a non-real-time system. This is, however, not recommended. + +While the best-performing strategy would be to use a real-time enabled kernel, using a lowlatency +kernel has shown to be sufficient in many situations which is why this is also shown as an option +here. + +Installing a lowlatency-kernel +------------------------------ + +Installing a lowlatency kernel is pretty straightforward: + +.. code-block:: console + + $ sudo apt install linux-lowlatency + +Setting up Ubuntu with a PREEMPT_RT kernel +------------------------------------------ + +To get real-time support into a ubuntu system, the following steps have to be performed: + +#. Get the sources of a real-time kernel +#. Compile the real-time kernel +#. Setup user privileges to execute real-time tasks + +This guide will help you setup your system with a real-time kernel. + +Preparing +^^^^^^^^^ + +To build the kernel, you will need a couple of tools available on your system. You can install them +using + +.. code-block:: console + + $ sudo apt-get install build-essential bc ca-certificates gnupg2 libssl-dev wget gawk flex bison libelf-dev dwarves + + +.. note:: + + For different kernel versions the dependencies might be different than that. If you experience + problems such as ``fatal error: liXYZ.h: No such file or directory`` during compilation, try to + install the library's corresponding ``dev``-package. + +Before you download the sources of a real-time-enabled kernel, check the kernel version that is currently installed: + +.. code-block:: console + + $ uname -r + 5.15.0-107-generic + +To continue with this tutorial, please create a temporary folder and navigate into it. You should +have sufficient space (around 25GB) there, as the extracted kernel sources take much space. After +the new kernel is installed, you can delete this folder again. + +In this example we will use a temporary folder inside our home folder: + +.. code-block:: console + + $ mkdir -p ${HOME}/rt_kernel_build + $ cd ${HOME}/rt_kernel_build + +All future commands are expected to be run inside this folder. If the folder is different, the ``$`` +sign will be prefixed with a path relative to the above folder. + +Getting the sources for a real-time kernel +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +To build a real-time kernel, we first need to get the kernel sources and the real-time patch. + +First, we must decide on the kernel version that we want to use. Above, we +determined that our system has a 5.15 kernel installed. However, real-time +patches exist only for selected kernel versions. Those can be found on the +`linuxfoundation wiki `_. + +In this example, we will select a 5.15.158 kernel with RT patch version 76. Select a kernel version close to the +one installed on your system. For easier reference later on we will export version information to +our shell environment. Make sure to execute all following commands in this shell. + +.. code-block:: console + + $ export KERNEL_MAJOR_VERSION=5 + $ export KERNEL_MINOR_VERSION=15 + $ export KERNEL_PATCH_VERSION=158 + $ export RT_PATCH_VERSION=76 + $ export KERNEL_VERSION="$KERNEL_MAJOR_VERSION.$KERNEL_MINOR_VERSION.$KERNEL_PATCH_VERSION" + +Go ahead and download the kernel sources, patch sources and their signature files: + +.. code-block:: console + + $ wget https://cdn.kernel.org/pub/linux/kernel/projects/rt/$KERNEL_MAJOR_VERSION.$KERNEL_MINOR_VERSION/patch-$KERNEL_VERSION-rt$RT_PATCH_VERSION.patch.xz + $ wget https://cdn.kernel.org/pub/linux/kernel/projects/rt/$KERNEL_MAJOR_VERSION.$KERNEL_MINOR_VERSION/patch-$KERNEL_VERSION-rt$RT_PATCH_VERSION.patch.sign + $ wget https://www.kernel.org/pub/linux/kernel/v$KERNEL_MAJOR_VERSION.x/linux-$KERNEL_VERSION.tar.xz + $ wget https://www.kernel.org/pub/linux/kernel/v$KERNEL_MAJOR_VERSION.x/linux-$KERNEL_VERSION.tar.sign + +To unzip the downloaded files do + +.. code-block:: console + + $ xz -dk patch-$KERNEL_VERSION-rt$RT_PATCH_VERSION.patch.xz + $ xz -d linux-$KERNEL_VERSION.tar.xz + +Verification +~~~~~~~~~~~~ + +Technically, you can skip this section, it is however highly recommended to verify the file +integrity of such a core component of your system! + +To verify file integrity, you must first import public keys by the kernel developers and the patch +author. For the kernel sources use (as suggested on +`kernel.org `_\ ) + +.. code-block:: console + + $ gpg2 --locate-keys torvalds@kernel.org gregkh@kernel.org + +and for the patch view the gpg information + +.. code-block:: console + + $ gpg2 --verify patch-$KERNEL_VERSION-rt$RT_PATCH_VERSION.patch.sign + gpg: assuming signed data in 'patch-5.15.158-rt76.patch' + gpg: Signature made Fri May 3 17:12:45 2024 UTC + gpg: using RSA key AD85102A6BE1CDFE9BCA84F36CEF3D27CA5B141E + gpg: Can't check signature: No public key + +So, we need to import the key using + +.. code-block:: console + + gpg2 --keyserver hkp://keyserver.ubuntu.com:80 --recv-keys AD85102A6BE1CDFE9BCA84F36CEF3D27CA5B141E + + +Now we can verify the downloaded sources: + +.. code-block:: console + + $ gpg2 --verify linux-$KERNEL_VERSION.tar.sign + gpg: assuming signed data in 'linux-5.15.158.tar' + gpg: Signature made Thu May 2 14:28:07 2024 UTC + gpg: using RSA key 647F28654894E3BD457199BE38DBBDC86092693E + gpg: Good signature from "Greg Kroah-Hartman " [unknown] + gpg: WARNING: This key is not certified with a trusted signature! + gpg: There is no indication that the signature belongs to the owner. + Primary key fingerprint: 647F 2865 4894 E3BD 4571 99BE 38DB BDC8 6092 693E + +and + +.. code-block:: console + + $ gpg2 --verify patch-$KERNEL_VERSION-rt$RT_PATCH_VERSION.patch.sign + gpg: assuming signed data in 'patch-5.15.158-rt76.patch' + gpg: Signature made Fri May 3 17:12:45 2024 UTC + gpg: using RSA key AD85102A6BE1CDFE9BCA84F36CEF3D27CA5B141E + gpg: Good signature from "Joseph Salisbury " [unknown] + gpg: aka "Joseph Salisbury " [unknown] + gpg: aka "Joseph Salisbury " [unknown] + gpg: WARNING: This key is not certified with a trusted signature! + gpg: There is no indication that the signature belongs to the owner. + Primary key fingerprint: AD85 102A 6BE1 CDFE 9BCA 84F3 6CEF 3D27 CA5B 141E + + +Compilation +^^^^^^^^^^^ + +Before we can compile the sources, we have to extract the tar archive and apply the patch + +.. code-block:: console + + $ tar xf linux-$KERNEL_VERSION.tar + $ cd linux-$KERNEL_VERSION + $ xzcat ../patch-$KERNEL_VERSION-rt$RT_PATCH_VERSION.patch.xz | patch -p1 + +Now to configure your kernel, just type + +.. code-block:: console + + $ make oldconfig + +This will ask for kernel options. For everything else then the ``Preemption Model`` use the default +value (just press Enter) or adapt to your preferences. For the preemption model select ``Fully Preemptible Kernel``\ : + +.. code-block:: console + + Preemption Model + 1. No Forced Preemption (Server) (PREEMPT_NONE) + > 2. Voluntary Kernel Preemption (Desktop) (PREEMPT_VOLUNTARY) + 3. Preemptible Kernel (Low-Latency Desktop) (PREEMPT) + 4. Fully Preemptible Kernel (Real-Time) (PREEMPT_RT) (NEW) + choice[1-4?]: 4 + +On newer kernels you need to disable some key checking: + +.. code-block:: console + + $ scripts/config --disable SYSTEM_TRUSTED_KEYS + $ scripts/config --disable SYSTEM_REVOCATION_KEYS + +Now you can build the kernel. This will take some time... + +.. code-block:: console + + $ make -j $(getconf _NPROCESSORS_ONLN) deb-pkg + +After building, install the ``linux-headers`` and ``linux-image`` packages in the parent folder (only +the ones without the ``-dbg`` in the name) + +.. code-block:: console + + $ sudo apt install ../linux-headers-$KERNEL_VERSION-rt$RT_PATCH_VERSION*.deb \ + ../linux-image-$KERNEL_VERSION-rt$RT_PATCH_VERSION*.deb + +Setup user privileges to use real-time scheduling +------------------------------------------------- + +To be able to schedule threads with user privileges (what the driver will do) you'll have to change +the user's limits by changing ``/etc/security/limits.conf`` (See `the manpage `_ for details) + +We recommend to setup a group for real-time users instead of writing a fixed username into the config +file: + +.. code-block:: console + + $ sudo groupadd realtime + $ sudo usermod -aG realtime $(whoami) + +Then, make sure ``/etc/security/limits.conf`` contains + +.. code-block:: linuxconfig + + @realtime soft rtprio 99 + @realtime soft priority 99 + @realtime soft memlock 102400 + @realtime hard rtprio 99 + @realtime hard priority 99 + @realtime hard memlock 102400 + +Note: You will have to log out and log back in (Not only close your terminal window) for these +changes to take effect. No need to do this now, as we will reboot later on, anyway. + +Setup GRUB to always boot the lowlatency / real-time kernel +----------------------------------------------------------- + +To make the new kernel the default kernel that the system will boot into every time, you'll have to +change the grub config file inside ``/etc/default/grub``. + +Note: This works for ubuntu, but might not be working for other linux systems. It might be necessary +to use another menuentry name there. + +But first, let's find out the name of the entry that we will want to make the default. You can list +all available kernels using + +.. code-block:: console + + $ awk -F\' '/menuentry |submenu / {print $1 $2}' /boot/grub/grub.cfg + menuentry Ubuntu + submenu Advanced options for Ubuntu + menuentry Ubuntu, with Linux 5.15.158-rt76 + menuentry Ubuntu, with Linux 5.15.158-rt76 (recovery mode) + menuentry Ubuntu, with Linux 5.15.0-107-lowlatency + menuentry Ubuntu, with Linux 5.15.0-107-lowlatency (recovery mode) + menuentry Ubuntu, with Linux 5.15.0-107-generic + menuentry Ubuntu, with Linux 5.15.0-107-generic (recovery mode) + +From the output above, we'll need to generate a string with the pattern ``"submenu_name>entry_name"``. In our case this would be + +.. code-block:: text + + "Advanced options for Ubuntu>Ubuntu, with Linux 5.15.158-rt76" + +**The double quotes and no spaces around the** ``>`` **are important!** + +With this, we can setup the default grub entry and then update the grub menu entries. Don't forget this last step! + +.. code-block:: console + + $ sudo sed -i "s/^GRUB_DEFAULT=.*/GRUB_DEFAULT=\"Advanced options for Ubuntu>Ubuntu, with Linux ${KERNEL_VERSION}-rt${RT_PATCH_VERSION}\"/" /etc/default/grub + $ sudo update-grub + +Reboot the PC +------------- + +After having performed the above mentioned steps, reboot the PC. It should boot into the correct +kernel automatically. + +Check for preemption capabilities +--------------------------------- + +Make sure that the kernel does indeed support real-time scheduling: + +.. code-block:: console + + $ uname -v | cut -d" " -f1-4 + #1 SMP PREEMPT_RT Tue + +Optional: Disable CPU speed scaling +----------------------------------- + +Many modern CPUs support changing their clock frequency dynamically depending on the currently +requested computation resources. In some cases this can lead to small interruptions in execution. +While the real-time scheduled controller thread should be unaffected by this, any external +components such as a visual servoing system might be interrupted for a short period on scaling +changes. + +To check and modify the power saving mode, install cpufrequtils: + +.. code-block:: console + + $ sudo apt install cpufrequtils + +Run ``cpufreq-info`` to check available "governors" and the current CPU Frequency (\ ``current CPU +frequency is XXX MHZ``\ ). In the following we will set the governor to "performance". + +.. code-block:: console + + $ sudo systemctl disable ondemand + $ sudo systemctl enable cpufrequtils + $ sudo sh -c 'echo "GOVERNOR=performance" > /etc/default/cpufrequtils' + $ sudo systemctl daemon-reload && sudo systemctl restart cpufrequtils + +This disables the ``ondemand`` CPU scaling daemon, creates a ``cpufrequtils`` config file and restarts +the ``cpufrequtils`` service. Check with ``cpufreq-info``. + +For further information about governors, please see the `kernel +documentation `_. From f06092e4f32ae1d042459cfaaae96b5c0ea1b21d Mon Sep 17 00:00:00 2001 From: "mergify[bot]" <37929162+mergify[bot]@users.noreply.github.com> Date: Mon, 3 Feb 2025 09:57:19 +0100 Subject: [PATCH 044/133] Auto-update pre-commit hooks (backport #1260) (#1261) --- .pre-commit-config.yaml | 4 ++-- ur_robot_driver/test/integration_test_controller_switch.py | 2 +- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml index 4392673f9..62897b10b 100644 --- a/.pre-commit-config.yaml +++ b/.pre-commit-config.yaml @@ -39,7 +39,7 @@ repos: args: [--py36-plus] - repo: https://github.com/psf/black - rev: 24.10.0 + rev: 25.1.0 hooks: - id: black args: ["--line-length=100"] @@ -135,7 +135,7 @@ repos: # Spellcheck in comments and docs # skipping of *.svg files is not working... - repo: https://github.com/codespell-project/codespell - rev: v2.4.0 + rev: v2.4.1 hooks: - id: codespell args: ['--write-changes', '-L bootup,assertIn'] diff --git a/ur_robot_driver/test/integration_test_controller_switch.py b/ur_robot_driver/test/integration_test_controller_switch.py index a9356ffc7..1d2d6570e 100644 --- a/ur_robot_driver/test/integration_test_controller_switch.py +++ b/ur_robot_driver/test/integration_test_controller_switch.py @@ -52,7 +52,7 @@ @pytest.mark.launch_test @launch_testing.parametrize( "tf_prefix", - [("")], + [""], # [(""), ("my_ur_")], ) def generate_test_description(tf_prefix): From 4fd0b2460de2ed46594ae4629547d404f1cd6e41 Mon Sep 17 00:00:00 2001 From: Felix Exner Date: Mon, 3 Feb 2025 11:10:13 +0100 Subject: [PATCH 045/133] Remove build warnings for Humble (#1234) * Update to new realtime_tools header extension * Update pin validation to avoid compiler warning * Use new include path for generated parameter headers This got updated in generate_parameters_library. * Update more headers --- .../include/ur_controllers/force_mode_controller.hpp | 4 ++-- .../include/ur_controllers/freedrive_mode_controller.hpp | 2 +- ur_controllers/include/ur_controllers/gpio_controller.hpp | 2 +- .../ur_controllers/passthrough_trajectory_controller.hpp | 7 +++---- .../ur_controllers/scaled_joint_trajectory_controller.hpp | 2 +- .../ur_controllers/speed_scaling_state_broadcaster.hpp | 2 +- .../include/ur_controllers/ur_configuration_controller.hpp | 4 ++-- ur_controllers/package.xml | 1 + ur_controllers/src/gpio_controller.cpp | 2 +- ur_controllers/src/ur_configuration_controller.cpp | 2 +- ur_robot_driver/src/ur_ros2_control_node.cpp | 2 +- 11 files changed, 15 insertions(+), 15 deletions(-) diff --git a/ur_controllers/include/ur_controllers/force_mode_controller.hpp b/ur_controllers/include/ur_controllers/force_mode_controller.hpp index 9ae45a04b..5ce586f56 100644 --- a/ur_controllers/include/ur_controllers/force_mode_controller.hpp +++ b/ur_controllers/include/ur_controllers/force_mode_controller.hpp @@ -37,17 +37,17 @@ #pragma once #include #include -#include #include #include #include #include +#include #include #include #include -#include "force_mode_controller_parameters.hpp" +#include "ur_controllers/force_mode_controller_parameters.hpp" namespace ur_controllers { diff --git a/ur_controllers/include/ur_controllers/freedrive_mode_controller.hpp b/ur_controllers/include/ur_controllers/freedrive_mode_controller.hpp index 04e90805e..3b01e358b 100644 --- a/ur_controllers/include/ur_controllers/freedrive_mode_controller.hpp +++ b/ur_controllers/include/ur_controllers/freedrive_mode_controller.hpp @@ -53,7 +53,7 @@ #include #include "std_msgs/msg/bool.hpp" -#include "freedrive_mode_controller_parameters.hpp" +#include "ur_controllers/freedrive_mode_controller_parameters.hpp" namespace ur_controllers { diff --git a/ur_controllers/include/ur_controllers/gpio_controller.hpp b/ur_controllers/include/ur_controllers/gpio_controller.hpp index b06b5139a..502254a9a 100644 --- a/ur_controllers/include/ur_controllers/gpio_controller.hpp +++ b/ur_controllers/include/ur_controllers/gpio_controller.hpp @@ -57,7 +57,7 @@ #include "rclcpp/time.hpp" #include "rclcpp/duration.hpp" #include "std_msgs/msg/bool.hpp" -#include "gpio_controller_parameters.hpp" +#include "ur_controllers/gpio_controller_parameters.hpp" namespace ur_controllers { diff --git a/ur_controllers/include/ur_controllers/passthrough_trajectory_controller.hpp b/ur_controllers/include/ur_controllers/passthrough_trajectory_controller.hpp index a3c91d10f..3d9c785be 100644 --- a/ur_controllers/include/ur_controllers/passthrough_trajectory_controller.hpp +++ b/ur_controllers/include/ur_controllers/passthrough_trajectory_controller.hpp @@ -43,9 +43,6 @@ #include -#include -#include - #include #include #include @@ -54,6 +51,8 @@ #include #include +#include +#include #include #include #include @@ -65,7 +64,7 @@ #include #include -#include "passthrough_trajectory_controller_parameters.hpp" +#include "ur_controllers/passthrough_trajectory_controller_parameters.hpp" namespace ur_controllers { diff --git a/ur_controllers/include/ur_controllers/scaled_joint_trajectory_controller.hpp b/ur_controllers/include/ur_controllers/scaled_joint_trajectory_controller.hpp index 58bd12426..7ec1814f1 100644 --- a/ur_controllers/include/ur_controllers/scaled_joint_trajectory_controller.hpp +++ b/ur_controllers/include/ur_controllers/scaled_joint_trajectory_controller.hpp @@ -45,7 +45,7 @@ #include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" #include "rclcpp/time.hpp" #include "rclcpp/duration.hpp" -#include "scaled_joint_trajectory_controller_parameters.hpp" +#include "ur_controllers/scaled_joint_trajectory_controller_parameters.hpp" namespace ur_controllers { diff --git a/ur_controllers/include/ur_controllers/speed_scaling_state_broadcaster.hpp b/ur_controllers/include/ur_controllers/speed_scaling_state_broadcaster.hpp index 4778f7ce1..d2cf62e08 100644 --- a/ur_controllers/include/ur_controllers/speed_scaling_state_broadcaster.hpp +++ b/ur_controllers/include/ur_controllers/speed_scaling_state_broadcaster.hpp @@ -48,7 +48,7 @@ #include "rclcpp/time.hpp" #include "rclcpp/duration.hpp" #include "std_msgs/msg/float64.hpp" -#include "speed_scaling_state_broadcaster_parameters.hpp" +#include "ur_controllers/speed_scaling_state_broadcaster_parameters.hpp" namespace ur_controllers { diff --git a/ur_controllers/include/ur_controllers/ur_configuration_controller.hpp b/ur_controllers/include/ur_controllers/ur_configuration_controller.hpp index 3775ee72c..067742eb5 100644 --- a/ur_controllers/include/ur_controllers/ur_configuration_controller.hpp +++ b/ur_controllers/include/ur_controllers/ur_configuration_controller.hpp @@ -43,14 +43,14 @@ // TODO(fmauch): Currently, the realtime_box_best_effort doesn't include this #include -#include // NOLINT #include #include +#include #include "ur_msgs/srv/get_robot_software_version.hpp" -#include "ur_configuration_controller_parameters.hpp" +#include "ur_controllers/ur_configuration_controller_parameters.hpp" namespace ur_controllers { diff --git a/ur_controllers/package.xml b/ur_controllers/package.xml index 5ffeea8bb..f6f169934 100644 --- a/ur_controllers/package.xml +++ b/ur_controllers/package.xml @@ -24,6 +24,7 @@ angles controller_interface + generate_parameter_library geometry_msgs hardware_interface joint_trajectory_controller diff --git a/ur_controllers/src/gpio_controller.cpp b/ur_controllers/src/gpio_controller.cpp index 188542d0e..3fadbd3db 100644 --- a/ur_controllers/src/gpio_controller.cpp +++ b/ur_controllers/src/gpio_controller.cpp @@ -407,7 +407,7 @@ bool GPIOController::setAnalogOutput(ur_msgs::srv::SetAnalogOutput::Request::Sha return false; } - if (req->data.pin < 0 || req->data.pin > 1) { + if (!(req->data.pin == 0 || req->data.pin == 1)) { RCLCPP_ERROR(get_node()->get_logger(), "Invalid pin selected. Only pins 0 and 1 are allowed."); resp->success = false; return false; diff --git a/ur_controllers/src/ur_configuration_controller.cpp b/ur_controllers/src/ur_configuration_controller.cpp index a6ec2d24b..48f909a0f 100644 --- a/ur_controllers/src/ur_configuration_controller.cpp +++ b/ur_controllers/src/ur_configuration_controller.cpp @@ -39,7 +39,7 @@ //---------------------------------------------------------------------- #include -#include +#include namespace ur_controllers { diff --git a/ur_robot_driver/src/ur_ros2_control_node.cpp b/ur_robot_driver/src/ur_ros2_control_node.cpp index 5cce2c044..0db64afef 100644 --- a/ur_robot_driver/src/ur_ros2_control_node.cpp +++ b/ur_robot_driver/src/ur_ros2_control_node.cpp @@ -41,7 +41,7 @@ // ROS includes #include "controller_manager/controller_manager.hpp" #include "rclcpp/rclcpp.hpp" -#include "realtime_tools/thread_priority.hpp" +#include "realtime_tools/realtime_helpers.hpp" // code is inspired by // https://github.com/ros-controls/ros2_control/blob/master/controller_manager/src/ros2_control_node.cpp From b6e6bac2e57548f688e875f1e80533aeca704887 Mon Sep 17 00:00:00 2001 From: "mergify[bot]" <37929162+mergify[bot]@users.noreply.github.com> Date: Fri, 7 Feb 2025 15:34:07 +0100 Subject: [PATCH 046/133] Fix crashes on shutting down (#1270) (#1271) We did not have the on_shutdown method implemented which gets called by the resource manager when shutting down. This could potentially result in crashes or pipeline overflows when shutting down the control node. There was an intermediate fix calling on_cleanup in the destructor, which isn't correct either. That got deleted. --- .../ur_robot_driver/hardware_interface.hpp | 4 ++++ ur_robot_driver/src/hardware_interface.cpp | 16 +++++++++++++--- 2 files changed, 17 insertions(+), 3 deletions(-) diff --git a/ur_robot_driver/include/ur_robot_driver/hardware_interface.hpp b/ur_robot_driver/include/ur_robot_driver/hardware_interface.hpp index 14bdd29ea..c9f23e4f5 100644 --- a/ur_robot_driver/include/ur_robot_driver/hardware_interface.hpp +++ b/ur_robot_driver/include/ur_robot_driver/hardware_interface.hpp @@ -124,6 +124,7 @@ class URPositionHardwareInterface : public hardware_interface::SystemInterface hardware_interface::CallbackReturn on_configure(const rclcpp_lifecycle::State& previous_state) final; hardware_interface::CallbackReturn on_activate(const rclcpp_lifecycle::State& previous_state) final; hardware_interface::CallbackReturn on_cleanup(const rclcpp_lifecycle::State& previous_state) final; + hardware_interface::CallbackReturn on_shutdown(const rclcpp_lifecycle::State& previous_state) final; hardware_interface::return_type read(const rclcpp::Time& time, const rclcpp::Duration& period) final; hardware_interface::return_type write(const rclcpp::Time& time, const rclcpp::Duration& period) final; @@ -154,6 +155,9 @@ class URPositionHardwareInterface : public hardware_interface::SystemInterface void readBitsetData(const std::unique_ptr& data_pkg, const std::string& var_name, std::bitset& data); + // stop function used by on_shutdown and on_cleanup + hardware_interface::CallbackReturn stop(); + void initAsyncIO(); void checkAsyncIO(); void updateNonDoubleValues(); diff --git a/ur_robot_driver/src/hardware_interface.cpp b/ur_robot_driver/src/hardware_interface.cpp index 157310e96..4efcf0623 100644 --- a/ur_robot_driver/src/hardware_interface.cpp +++ b/ur_robot_driver/src/hardware_interface.cpp @@ -58,9 +58,6 @@ namespace ur_robot_driver URPositionHardwareInterface::~URPositionHardwareInterface() { - // If the controller manager is shutdown via Ctrl + C the on_deactivate methods won't be called. - // We therefore need to make sure to actually deactivate the communication - on_cleanup(rclcpp_lifecycle::State()); } hardware_interface::CallbackReturn @@ -592,6 +589,19 @@ URPositionHardwareInterface::on_activate(const rclcpp_lifecycle::State& previous hardware_interface::CallbackReturn URPositionHardwareInterface::on_cleanup(const rclcpp_lifecycle::State& previous_state) +{ + RCLCPP_DEBUG(rclcpp::get_logger("URPositionHardwareInterface"), "on_cleanup"); + return stop(); +} + +hardware_interface::CallbackReturn +URPositionHardwareInterface::on_shutdown(const rclcpp_lifecycle::State& previous_state) +{ + RCLCPP_DEBUG(rclcpp::get_logger("URPositionHardwareInterface"), "on_shutdown"); + return stop(); +} + +hardware_interface::CallbackReturn URPositionHardwareInterface::stop() { RCLCPP_INFO(rclcpp::get_logger("URPositionHardwareInterface"), "Stopping ...please wait..."); From 02ad82a31a95e2333faa814cbe114fe4db4d3c61 Mon Sep 17 00:00:00 2001 From: "mergify[bot]" <37929162+mergify[bot]@users.noreply.github.com> Date: Wed, 26 Feb 2025 06:29:38 +0100 Subject: [PATCH 047/133] Auto-update pre-commit hooks (#1280) (#1282) --- .pre-commit-config.yaml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml index 62897b10b..74e57b808 100644 --- a/.pre-commit-config.yaml +++ b/.pre-commit-config.yaml @@ -51,7 +51,7 @@ repos: args: ["--ignore=D100,D101,D102,D103,D104,D105,D106,D107,D203,D212,D401,D404"] - repo: https://github.com/pycqa/flake8 - rev: 7.1.1 + rev: 7.1.2 hooks: - id: flake8 args: ["--ignore=E501,W503"] From 738bd8d9003eb4e221783a09ec6ed320ec148dc5 Mon Sep 17 00:00:00 2001 From: "mergify[bot]" <37929162+mergify[bot]@users.noreply.github.com> Date: Wed, 26 Feb 2025 08:17:26 +0100 Subject: [PATCH 048/133] ur_controllers: doc -- Fix link to index page of driver (backport of #1284) (#1285) --- ur_controllers/doc/index.rst | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ur_controllers/doc/index.rst b/ur_controllers/doc/index.rst index f03632d53..b33350889 100644 --- a/ur_controllers/doc/index.rst +++ b/ur_controllers/doc/index.rst @@ -35,7 +35,7 @@ This controller publishes the current actual execution speed as reported by the floating points between 0 and 1. In the `ur_robot_driver -`_ +`_ this is calculated by multiplying the two `RTDE `_ data fields ``speed_scaling`` (which should be equal to the value shown by the speed slider position on the From 408318bcce1f0475de7c22f00968bd62202569a3 Mon Sep 17 00:00:00 2001 From: "mergify[bot]" <37929162+mergify[bot]@users.noreply.github.com> Date: Tue, 4 Mar 2025 10:16:42 +0100 Subject: [PATCH 049/133] Port robot_state_helper to ROS2 (backport of #933) (#1286) --------- Co-authored-by: Felix Durchdewald Co-authored-by: Felix Exner --- ur_dashboard_msgs/action/SetMode.action | 5 +- ur_robot_driver/CMakeLists.txt | 15 +- ur_robot_driver/doc/controller_stopper.rst | 29 ++ ur_robot_driver/doc/index.rst | 3 +- ur_robot_driver/doc/robot_state_helper.rst | 59 +++ ur_robot_driver/doc/usage/startup.rst | 142 +++++++ .../ur_robot_driver/robot_state_helper.hpp | 114 +++++ ur_robot_driver/launch/ur_control.launch.py | 11 + ur_robot_driver/src/robot_state_helper.cpp | 397 ++++++++++++++++++ .../src/robot_state_helper_node.cpp | 42 ++ 10 files changed, 813 insertions(+), 4 deletions(-) create mode 100644 ur_robot_driver/doc/controller_stopper.rst create mode 100644 ur_robot_driver/doc/robot_state_helper.rst create mode 100644 ur_robot_driver/doc/usage/startup.rst create mode 100644 ur_robot_driver/include/ur_robot_driver/robot_state_helper.hpp create mode 100644 ur_robot_driver/src/robot_state_helper.cpp create mode 100755 ur_robot_driver/src/robot_state_helper_node.cpp diff --git a/ur_dashboard_msgs/action/SetMode.action b/ur_dashboard_msgs/action/SetMode.action index d110997f1..c0ee7e27b 100644 --- a/ur_dashboard_msgs/action/SetMode.action +++ b/ur_dashboard_msgs/action/SetMode.action @@ -1,7 +1,10 @@ # This action is for setting the robot into a desired mode (e.g. RUNNING) and safety mode into a # non-critical state (e.g. NORMAL or REDUCED), for example after a safety incident happened. -# goal +# Target modes can be one of +# - 3: ROBOT_MODE_POWER_OFF +# - 5: ROBOT_MODE_IDLE +# - 7: ROBOT_MODE_RUNNING int8 target_robot_mode # Stop program execution before restoring the target mode. Can be used together with 'play_program'. diff --git a/ur_robot_driver/CMakeLists.txt b/ur_robot_driver/CMakeLists.txt index 68c082aa1..0a6f1b747 100644 --- a/ur_robot_driver/CMakeLists.txt +++ b/ur_robot_driver/CMakeLists.txt @@ -58,6 +58,7 @@ add_library(ur_robot_driver_plugin src/dashboard_client_ros.cpp src/hardware_interface.cpp src/urcl_log_handler.cpp + src/robot_state_helper.cpp ) target_link_libraries( ur_robot_driver_plugin @@ -83,7 +84,7 @@ add_executable(dashboard_client src/dashboard_client_node.cpp src/urcl_log_handler.cpp ) -target_link_libraries(dashboard_client ${catkin_LIBRARIES} ur_client_library::urcl) +target_link_libraries(dashboard_client ur_client_library::urcl) ament_target_dependencies(dashboard_client ${${PROJECT_NAME}_EXPORTED_TARGETS} ${THIS_PACKAGE_INCLUDE_DEPENDS}) # @@ -106,13 +107,23 @@ add_executable(controller_stopper_node ) ament_target_dependencies(controller_stopper_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${THIS_PACKAGE_INCLUDE_DEPENDS}) +# +# robot_state_helper +# +add_executable(robot_state_helper + src/robot_state_helper.cpp + src/robot_state_helper_node.cpp +) +target_link_libraries(robot_state_helper ur_client_library::urcl) +ament_target_dependencies(robot_state_helper ${${PROJECT_NAME}_EXPORTED_TARGETS} ${THIS_PACKAGE_INCLUDE_DEPENDS}) + add_executable(urscript_interface src/urscript_interface.cpp ) ament_target_dependencies(urscript_interface ${${PROJECT_NAME}_EXPORTED_TARGETS} ${THIS_PACKAGE_INCLUDE_DEPENDS}) install( - TARGETS dashboard_client ur_ros2_control_node controller_stopper_node urscript_interface + TARGETS dashboard_client ur_ros2_control_node controller_stopper_node urscript_interface robot_state_helper DESTINATION lib/${PROJECT_NAME} ) diff --git a/ur_robot_driver/doc/controller_stopper.rst b/ur_robot_driver/doc/controller_stopper.rst new file mode 100644 index 000000000..d02d6b012 --- /dev/null +++ b/ur_robot_driver/doc/controller_stopper.rst @@ -0,0 +1,29 @@ +.. _controller_stopper: + +Controller stopper +================== + +As explained in the section :ref:`robot_startup_program`, the robot needs to run a program in order +to receive motion commands from the ROS driver. When the program is not running, commands sent to +the robot will have no effect. + +To make that transparent, the ``controller_stopper`` node mirrors that state in the ROS +controller's state. It listens to the ``/io_and_status_controller/robot_program_running`` topic and +deactivates all motion controllers (or any controller not explicitly marked as "consistent", see +below)when the program is not running. + +Once the program is running again, any previously active motion controller will be activated again. + +This way, when sending commands to an inactive controller the caller should be transparently +informed, that the controller cannot accept commands at the moment. + +In the same way, any running action on the ROS controller will be aborted, as the controller gets +deactivated by the controller_stopper. + +Parameters +---------- + +- ``~consistent_controllers`` (list of strings, default: ``[]``) + + A list of controller names that should not be stopped when the program is not running. Any + controller that doesn't require the robot program to be running should be in that list. diff --git a/ur_robot_driver/doc/index.rst b/ur_robot_driver/doc/index.rst index bf536ad81..d2889276a 100644 --- a/ur_robot_driver/doc/index.rst +++ b/ur_robot_driver/doc/index.rst @@ -16,7 +16,8 @@ Welcome to ur_robot_driver's documentation! setup_tool_communication ROS_INTERFACE generated/index - + robot_state_helper + controller_stopper Indices and tables diff --git a/ur_robot_driver/doc/robot_state_helper.rst b/ur_robot_driver/doc/robot_state_helper.rst new file mode 100644 index 000000000..92564fe32 --- /dev/null +++ b/ur_robot_driver/doc/robot_state_helper.rst @@ -0,0 +1,59 @@ +.. _robot_state_helper: + +Robot state helper +================== +After switching on the robot, it has to be manually started, the brakes have to be released and a +program has to be started in order to make the robot ready to use. This is usually done using the +robot's teach pendant. + +Whenever the robot encounters an error, manual intervention is required to resolve the issue. For +example, if the robot goes into a protective stop, the error has to be acknowledged and the robot +program has to be unpaused. + +When the robot is in :ref:`remote_control_mode `, most interaction with the robot can be done +without using the teach pendant, many of that through the :ref:`dashboard client +`. + +The ROS driver provides a helper node that can be used to automate some of these tasks. The +``robot_state_helper`` node can be used to start the robot, release the brakes, and (re-)start the +program through an action call. It is started by default and provides a +`dashboard_msgs/action/SetMode +`_ action. + +For example, to make the robot ready to be used by the ROS driver, call + +.. code-block:: console + + $ ros2 action send_goal /ur_robot_state_helper/set_mode ur_dashboard_msgs/action/SetMode "{ target_robot_mode: 7, stop_program: true, play_program: true}" + +The ``target_robot_mode`` can be one of the following: + +.. table:: target_robot_mode + :widths: auto + + ===== ===== + index meaning + ===== ===== + 3 POWER_OFF -- Robot is powered off + 5 IDLE -- Robot is powered on, but brakes are engaged + 7 RUNNING -- Robot is powered on, brakes are released, ready to run a program + ===== ===== + +.. note:: + + When the ROBOT_STATE is in ``RUNNING``, that is equivalent to the robot showing the green dot in + the lower left corner of the teach pendant (On PolyScope 5). The program state is independent of + that and shows with the text next to that button. + +The ``stop_program`` flag is used to stop the currently running program before changing the robot +state. In combination with the :ref:`controller_stopper`, this will deactivate any motion +controller and therefore stop any ROS action being active on those controllers. + +.. warning:: + A robot's protective stop or emergency stop is only pausing the running program. If the program + is resumed after the P-Stop or EM-Stop is released, the robot will continue executing what it + has been doing. Therefore, it is advised to stop and re-start the program when recovering from a + fault. + +The ``play_program`` flag is used to start the program after the robot state has been set. This has +the same effects as explained in :ref:`continuation_after_interruptions`. diff --git a/ur_robot_driver/doc/usage/startup.rst b/ur_robot_driver/doc/usage/startup.rst new file mode 100644 index 000000000..6b0644563 --- /dev/null +++ b/ur_robot_driver/doc/usage/startup.rst @@ -0,0 +1,142 @@ +.. _ur_robot_driver_startup: + +Startup the driver +================== + +Prepare the robot +----------------- + +If you want to use a real robot, or a URSim simulator, with this driver, you need to prepare it, +first. Make sure that you complete all steps from the :ref:`setup instructions`, +installed the External Control URCap and created a program as explained +:ref:`here`. + +Launch files +------------ + +For starting the driver it is recommended to start the ``ur_control.launch.py`` launchfile from the +``ur_robot_driver`` package. It starts the driver, a set of controllers and a couple of helper +nodes for UR robots. The only required arguments are the ``ur_type`` and ``robot_ip`` parameters. + +.. code-block:: console + + $ ros2 launch ur_robot_driver ur_control.launch.py ur_type:=ur5e robot_ip:=192.168.56.101 + +Allowed ``ur_type`` strings: ``ur3``, ``ur3e``, ``ur5``, ``ur5e``, ``ur10``, ``ur10e``, ``ur16e``, +``ur20``, ``ur30``. + +Other important arguments are: + + +* ``kinematics_params_file`` (default: *None*) - Path to the calibration file extracted from the robot, as described in :ref:`calibration_extraction`. +* ``use_mock_hardware`` (default: *false* ) - Use simple hardware emulator from ros2_control. Useful for testing launch files, descriptions, etc. +* ``headless_mode`` (default: *false*) - Start driver in :ref:`headless_mode`. +* ``launch_rviz`` (default: *true*) - Start RViz together with the driver. +* ``initial_joint_controller`` (default: *scaled_joint_trajectory_controller*) - Use this if you + want to start the robot with another controller. + + .. note:: + When the driver is started, you can list all loaded controllers using the ``ros2 control + list_controllers`` command. For this, the package ``ros2controlcli`` must be installed (``sudo + apt-get install ros-${ROS_DISTRO}-ros2controlcli``). + + +For all other arguments, please see + + +.. code-block:: console + + $ ros2 launch ur_robot_driver ur_control.launch.py --show-args + +Also, there are predefined launch files for all supported types of UR robots. + +.. _robot_startup_program: + +Finish startup on the robot +--------------------------- + +Unless :ref:`headless_mode` is used, you will now have to start the *External Control URCap* program on +the robot that you have created earlier. + +Depending on the :ref:`robot control mode` do the following: + +* In *local control mode*, load the program on the robot and press the "Play" button |play_button| on the teach pendant. +* In *remote control mode* load and start the program using the following dashboard calls: + + .. code-block:: console + + $ ros2 service call /dashboard_client/load_program ur_dashboard_msgs/srv/Load "filename: my_robot_program.urp"`` + $ ros2 service call /dashboard_client/play std_srvs/srv/Trigger {} + +* When the driver is started with ``headless_mode:=true`` nothing is needed. The driver is running + already. + + +.. _verify_calibration: + +Verify calibration info is being used correctly +----------------------------------------------- + + +If you passed a path to an extracted calibration via the *kinematics_params_file* +parameter, ensure that the loaded calibration matches that of the robot by inspecting the console +output after launching the ``ur_robot_driver``. If the calibration does not match, you will see an error: + +.. code-block:: + + [INFO] [1694437690.406932381] [URPositionHardwareInterface]: Calibration checksum: 'calib_xxxxxxxxxxxxxxxxxxx' + [ERROR] [1694437690.516957265] [URPositionHardwareInterface]: The calibration parameters of the connected robot don't match the ones from the given kinematics config file. + +With the correct calibration you should see: + +.. code-block:: + + [INFO] [1694437690.406932381] [URPositionHardwareInterface]: Calibration checksum: 'calib_xxxxxxxxxxxxxxxxxxx' + [INFO] [1694437690.516957265] [URPositionHardwareInterface]: Calibration checked successfully. + +Alternatively, search for the term *checksum* in the console output after launching the driver. +Verify that the printed checksum matches that on the final line of your extracted calibration file. + + +.. _continuation_after_interruptions: + +Continuation after interruptions +-------------------------------- + + +Whenever the *External Control URCap* program gets interrupted, it has to be unpaused / restarted. + +If that happens, you will see the output ``Connection to reverse interface dropped.`` + +This can happen, e,g, when + +* The running program is actively stopped. +* The robot goes into a protective stop / EM stop. (The program will be paused, then) +* The communication is stopped, since the external source did not receive a command in time. +* There was another script sent for execution e.g. + + * Script code was sent to the robot via its primary interface + * Robot motion is performed using the Teach pendant + +Depending on the operation mode, perform one of the following steps: + +* In *local control mode*, simply press the "Play" button |play_button| on the teach pendant. +* In *remote control mode* start the program using the following dashboard call: + + .. code-block:: console + + $ ros2 service call /dashboard_client/play std_srvs/srv/Trigger {} + +* When the driver is started with ``headless_mode:=true`` perform the following service call: + + .. code-block:: console + + $ ros2 service call /io_and_status_controller/resend_robot_program std_srvs/srv/Trigger {} + + + + + +.. |play_button| image:: ../resources/play_button.svg + :height: 20px + :width: 20px diff --git a/ur_robot_driver/include/ur_robot_driver/robot_state_helper.hpp b/ur_robot_driver/include/ur_robot_driver/robot_state_helper.hpp new file mode 100644 index 000000000..d3c03189b --- /dev/null +++ b/ur_robot_driver/include/ur_robot_driver/robot_state_helper.hpp @@ -0,0 +1,114 @@ +// Copyright 2024, FZI Forschungszentrum Informatik, Created on behalf of 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. + +#ifndef UR_ROBOT_DRIVER__ROBOT_STATE_HELPER_HPP_ +#define UR_ROBOT_DRIVER__ROBOT_STATE_HELPER_HPP_ + +#include +#include + +#include "rclcpp/rclcpp.hpp" +#include "rclcpp_action/create_server.hpp" +#include "std_msgs/msg/bool.hpp" +#include "std_srvs/srv/trigger.hpp" + +#include "ur_dashboard_msgs/action/set_mode.hpp" +#include "ur_dashboard_msgs/msg/safety_mode.hpp" +#include "ur_dashboard_msgs/msg/robot_mode.hpp" +#include "ur_client_library/ur/datatypes.h" + +namespace ur_robot_driver +{ +class RobotStateHelper +{ +public: + using SetModeGoalHandle = rclcpp_action::ServerGoalHandle; + + explicit RobotStateHelper(const rclcpp::Node::SharedPtr& node); + RobotStateHelper() = delete; + virtual ~RobotStateHelper() = default; + +private: + rclcpp::Node::SharedPtr node_; + + void robotModeCallback(ur_dashboard_msgs::msg::RobotMode::SharedPtr msg); + void safetyModeCallback(ur_dashboard_msgs::msg::SafetyMode::SharedPtr msg); + + void updateRobotState(); + + bool recoverFromSafety(); + bool doTransition(const urcl::RobotMode target_mode); + bool jumpToRobotMode(const urcl::RobotMode target_mode); + + bool safeDashboardTrigger(rclcpp::Client::SharedPtr srv); + + bool stopProgram(); + + void setModeAcceptCallback(const std::shared_ptr goal_handle); + rclcpp_action::GoalResponse setModeGoalCallback(const rclcpp_action::GoalUUID& uuid, + std::shared_ptr goal); + rclcpp_action::CancelResponse setModeCancelCallback(const std::shared_ptr goal_handle); + + void setModeExecute(const std::shared_ptr goal_handle); + + bool headless_mode_; + + std::shared_ptr result_; + std::shared_ptr feedback_; + std::shared_ptr goal_; + std::shared_ptr current_goal_handle_; + + std::atomic robot_mode_; + std::atomic safety_mode_; + std::atomic error_ = false; + std::atomic in_action_; + std::atomic program_running_; + std::mutex goal_mutex_; + + rclcpp_action::Server::SharedPtr set_mode_as_; + + rclcpp::CallbackGroup::SharedPtr robot_mode_sub_cb_; + + rclcpp::Subscription::SharedPtr robot_mode_sub_; + rclcpp::Subscription::SharedPtr safety_mode_sub_; + rclcpp::Subscription::SharedPtr program_running_sub; + + rclcpp::CallbackGroup::SharedPtr service_cb_grp_; + + rclcpp::Client::SharedPtr unlock_protective_stop_srv_; + rclcpp::Client::SharedPtr restart_safety_srv_; + rclcpp::Client::SharedPtr power_on_srv_; + rclcpp::Client::SharedPtr power_off_srv_; + rclcpp::Client::SharedPtr brake_release_srv_; + rclcpp::Client::SharedPtr stop_program_srv_; + rclcpp::Client::SharedPtr play_program_srv_; + rclcpp::Client::SharedPtr resend_robot_program_srv_; +}; +} // namespace ur_robot_driver + +#endif // UR_ROBOT_DRIVER__ROBOT_STATE_HELPER_HPP_ diff --git a/ur_robot_driver/launch/ur_control.launch.py b/ur_robot_driver/launch/ur_control.launch.py index 72de393ab..4c47b07b3 100644 --- a/ur_robot_driver/launch/ur_control.launch.py +++ b/ur_robot_driver/launch/ur_control.launch.py @@ -257,6 +257,16 @@ def launch_setup(context, *args, **kwargs): parameters=[{"robot_ip": robot_ip}], ) + robot_state_helper_node = Node( + package="ur_robot_driver", + executable="robot_state_helper", + name="ur_robot_state_helper", + output="screen", + parameters=[ + {"headless_mode": headless_mode}, + ], + ) + tool_communication_node = Node( package="ur_robot_driver", condition=IfCondition(use_tool_communication), @@ -367,6 +377,7 @@ def controller_spawner(controllers, active=True): control_node, ur_control_node, dashboard_client_node, + robot_state_helper_node, tool_communication_node, controller_stopper_node, urscript_interface, diff --git a/ur_robot_driver/src/robot_state_helper.cpp b/ur_robot_driver/src/robot_state_helper.cpp new file mode 100644 index 000000000..0f35df6d7 --- /dev/null +++ b/ur_robot_driver/src/robot_state_helper.cpp @@ -0,0 +1,397 @@ +// Copyright 2024, FZI Forschungszentrum Informatik, Created on behalf of 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. + +#include +#include + +#include "rclcpp/rclcpp.hpp" +#include "rclcpp_action/create_server.hpp" +#include "std_srvs/srv/trigger.hpp" + +#include "ur_dashboard_msgs/action/set_mode.hpp" +#include "ur_dashboard_msgs/msg/safety_mode.hpp" +#include "ur_dashboard_msgs/msg/robot_mode.hpp" +#include "ur_client_library/ur/datatypes.h" + +namespace ur_robot_driver +{ +RobotStateHelper::RobotStateHelper(const rclcpp::Node::SharedPtr& node) + : node_(node) + , robot_mode_(urcl::RobotMode::UNKNOWN) + , safety_mode_(urcl::SafetyMode::UNDEFINED_SAFETY_MODE) + , in_action_(false) +{ + robot_mode_sub_cb_ = node_->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); + rclcpp::SubscriptionOptions options; + options.callback_group = robot_mode_sub_cb_; + // Topic on which the robot_mode is published by the driver + robot_mode_sub_ = node_->create_subscription( + "io_and_status_controller/robot_mode", rclcpp::SensorDataQoS(), + std::bind(&RobotStateHelper::robotModeCallback, this, std::placeholders::_1), options); + // Topic on which the safety is published by the driver + safety_mode_sub_ = node_->create_subscription( + "io_and_status_controller/safety_mode", 1, + std::bind(&RobotStateHelper::safetyModeCallback, this, std::placeholders::_1)); + program_running_sub = node_->create_subscription( + "io_and_status_controller/robot_program_running", 1, + [this](std_msgs::msg::Bool::UniquePtr msg) -> void { program_running_ = msg->data; }); + + service_cb_grp_ = node_->create_callback_group(rclcpp::CallbackGroupType::Reentrant); + + node->declare_parameter("headless_mode", false); + headless_mode_ = node->get_parameter("headless_mode").as_bool(); + + // Service to unlock protective stop + unlock_protective_stop_srv_ = node_->create_client( + "dashboard_client/unlock_protective_stop", rmw_qos_profile_services_default, service_cb_grp_); + // Service to restart safety + restart_safety_srv_ = node_->create_client("dashboard_client/restart_safety", + rmw_qos_profile_services_default, service_cb_grp_); + // Service to power on the robot + power_on_srv_ = node_->create_client("dashboard_client/power_on", + rmw_qos_profile_services_default, service_cb_grp_); + // Service to power off the robot + power_off_srv_ = node_->create_client("dashboard_client/power_off", + rmw_qos_profile_services_default, service_cb_grp_); + // Service to release the robot's brakes + brake_release_srv_ = node_->create_client("dashboard_client/brake_release", + rmw_qos_profile_services_default, service_cb_grp_); + // Service to stop UR program execution on the robot + stop_program_srv_ = node_->create_client("dashboard_client/stop", + rmw_qos_profile_services_default, service_cb_grp_); + // Service to start UR program execution on the robot + play_program_srv_ = node_->create_client("dashboard_client/play", + rmw_qos_profile_services_default, service_cb_grp_); + play_program_srv_->wait_for_service(); + + resend_robot_program_srv_ = node_->create_client( + "io_and_status_controller/resend_robot_program", rmw_qos_profile_services_default, service_cb_grp_); + resend_robot_program_srv_->wait_for_service(); + + feedback_ = std::make_shared(); + result_ = std::make_shared(); + set_mode_as_ = rclcpp_action::create_server( + node_, "~/set_mode", + std::bind(&RobotStateHelper::setModeGoalCallback, this, std::placeholders::_1, std::placeholders::_2), + std::bind(&RobotStateHelper::setModeCancelCallback, this, std::placeholders::_1), + std::bind(&RobotStateHelper::setModeAcceptCallback, this, std::placeholders::_1)); +} + +void RobotStateHelper::robotModeCallback(ur_dashboard_msgs::msg::RobotMode::SharedPtr msg) +{ + if (robot_mode_ != static_cast(msg->mode)) { + robot_mode_ = urcl::RobotMode(msg->mode); + RCLCPP_INFO_STREAM(rclcpp::get_logger("robot_state_helper"), + "The robot is currently in mode " << robotModeString(robot_mode_) << "."); + if (in_action_) { + std::scoped_lock lock(goal_mutex_); + feedback_->current_robot_mode = + static_cast(robot_mode_.load()); + current_goal_handle_->publish_feedback(feedback_); + } + } +} + +void RobotStateHelper::safetyModeCallback(ur_dashboard_msgs::msg::SafetyMode::SharedPtr msg) +{ + if (safety_mode_ != static_cast(msg->mode)) { + safety_mode_ = urcl::SafetyMode(msg->mode); + RCLCPP_INFO_STREAM(rclcpp::get_logger("robot_state_helper"), + "The robot is currently in safety mode " << safetyModeString(safety_mode_) << "."); + if (in_action_) { + std::scoped_lock lock(goal_mutex_); + feedback_->current_safety_mode = + static_cast(safety_mode_.load()); + current_goal_handle_->publish_feedback(feedback_); + } + } +} + +bool RobotStateHelper::recoverFromSafety() +{ + switch (safety_mode_) { + case urcl::SafetyMode::PROTECTIVE_STOP: + return safeDashboardTrigger(this->unlock_protective_stop_srv_); + case urcl::SafetyMode::SYSTEM_EMERGENCY_STOP:; + case urcl::SafetyMode::ROBOT_EMERGENCY_STOP: + RCLCPP_WARN_STREAM(rclcpp::get_logger("robot_state_helper"), "The robot is currently in safety mode." + << safetyModeString(safety_mode_) + << ". Please release the EM-Stop to proceed."); + return false; + case urcl::SafetyMode::VIOLATION:; + case urcl::SafetyMode::FAULT: + return safeDashboardTrigger(this->restart_safety_srv_); + default: + // nothing to do + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("robot_state_helper"), "No safety recovery needed."); + } + return true; +} + +bool RobotStateHelper::jumpToRobotMode(const urcl::RobotMode target_mode) +{ + switch (target_mode) { + case urcl::RobotMode::POWER_OFF: + return safeDashboardTrigger(this->power_off_srv_); + case urcl::RobotMode::IDLE: + return safeDashboardTrigger(this->power_on_srv_); + case urcl::RobotMode::RUNNING: + return safeDashboardTrigger(this->brake_release_srv_); + default: + RCLCPP_ERROR_STREAM(rclcpp::get_logger("robot_state_helper"), "Unreachable target robot mode."); + } + return false; +} + +bool RobotStateHelper::doTransition(const urcl::RobotMode target_mode) +{ + if (!recoverFromSafety()) { + return false; + } + switch (robot_mode_) { + case urcl::RobotMode::CONFIRM_SAFETY: + RCLCPP_WARN_STREAM(rclcpp::get_logger("robot_state_helper"), "The robot is currently in mode " + << robotModeString(robot_mode_) + << ". It is required to interact with " + "the " + "teach pendant at this point."); + break; + case urcl::RobotMode::BOOTING: + RCLCPP_INFO_STREAM(rclcpp::get_logger("robot_state_helper"), "The robot is currently in mode " + << robotModeString(robot_mode_) + << ". Please wait until the robot is " + "booted up..."); + break; + case urcl::RobotMode::POWER_OFF: + return jumpToRobotMode(target_mode); + case urcl::RobotMode::POWER_ON: + RCLCPP_INFO_STREAM(rclcpp::get_logger("robot_state_helper"), "The robot is currently in mode " + << robotModeString(robot_mode_) + << ". Please wait until the robot is in " + "mode " + << robotModeString(urcl::RobotMode::IDLE)); + break; + case urcl::RobotMode::IDLE: + return jumpToRobotMode(target_mode); + break; + case urcl::RobotMode::BACKDRIVE: + RCLCPP_INFO_STREAM(rclcpp::get_logger("robot_state_helper"), "The robot is currently in mode " + << robotModeString(robot_mode_) + << ". It will automatically return to " + "mode " + << robotModeString(urcl::RobotMode::IDLE) + << " once the teach button is " + "released."); + break; + case urcl::RobotMode::RUNNING: + if (target_mode == urcl::RobotMode::IDLE) { + // We cannot engage the brakes directly. + if (!jumpToRobotMode(urcl::RobotMode::POWER_OFF)) { + return false; + } + } + return jumpToRobotMode(target_mode); + RCLCPP_INFO_STREAM(rclcpp::get_logger("robot_state_helper"), + "The robot has reached operational mode " << robotModeString(robot_mode_)); + break; + default: + RCLCPP_WARN_STREAM(rclcpp::get_logger("robot_state_helper"), "The robot is currently in mode " + << robotModeString(robot_mode_) + << ". This won't be handled by this " + "helper. Please resolve this " + "manually."); + } + return false; +} + +bool RobotStateHelper::safeDashboardTrigger(rclcpp::Client::SharedPtr srv) +{ + assert(srv != nullptr); + auto request = std::make_shared(); + auto future = srv->async_send_request(request); + future.wait(); + auto result = future.get(); + RCLCPP_INFO_STREAM(rclcpp::get_logger("robot_state_helper"), "Service response received: " << result->message); + return result->success; +} + +void RobotStateHelper::setModeAcceptCallback(const std::shared_ptr goal_handle) +{ + std::thread{ std::bind(&RobotStateHelper::setModeExecute, this, std::placeholders::_1), goal_handle }.detach(); +} + +bool RobotStateHelper::stopProgram() +{ + if (safeDashboardTrigger(this->stop_program_srv_)) { + auto start = std::chrono::steady_clock::now(); + while (program_running_ && std::chrono::steady_clock::now() - start < std::chrono::seconds(1)) { + std::this_thread::sleep_for(std::chrono::milliseconds(2)); + return true; + } + } + return false; +} + +void RobotStateHelper::setModeExecute(const std::shared_ptr goal_handle) +{ + { + std::scoped_lock lock(goal_mutex_); + current_goal_handle_ = goal_handle; + } + in_action_ = true; + const auto goal = goal_handle->get_goal(); + this->goal_ = goal; + urcl::RobotMode target_mode; + try { + target_mode = static_cast(goal->target_robot_mode); + switch (target_mode) { + case urcl::RobotMode::POWER_OFF: + case urcl::RobotMode::IDLE: + case urcl::RobotMode::RUNNING: + if (goal_->stop_program && program_running_) { + if (!stopProgram()) { + result_->message = "Stopping the program failed."; + result_->success = false; + std::scoped_lock lock(goal_mutex_); + current_goal_handle_->abort(result_); + return; + } + } + if (robot_mode_ != target_mode || safety_mode_ > urcl::SafetyMode::REDUCED) { + RCLCPP_INFO_STREAM(rclcpp::get_logger("robot_state_helper"), + "Target mode was set to " << robotModeString(target_mode) << "."); + if (!doTransition(target_mode)) { + result_->message = "Transition to target mode failed."; + result_->success = false; + std::scoped_lock lock(goal_mutex_); + current_goal_handle_->abort(result_); + return; + } + } + break; + case urcl::RobotMode::NO_CONTROLLER: + case urcl::RobotMode::DISCONNECTED: + case urcl::RobotMode::CONFIRM_SAFETY: + case urcl::RobotMode::BOOTING: + case urcl::RobotMode::POWER_ON: + case urcl::RobotMode::BACKDRIVE: + case urcl::RobotMode::UPDATING_FIRMWARE: + result_->message = + "Requested target mode " + robotModeString(target_mode) + " which cannot be explicitly selected."; + result_->success = false; + { + std::scoped_lock lock(goal_mutex_); + current_goal_handle_->abort(result_); + } + return; + break; + default: + result_->message = "Requested illegal mode."; + result_->success = false; + { + std::scoped_lock lock(goal_mutex_); + current_goal_handle_->abort(result_); + } + return; + break; + } + } catch (const std::invalid_argument& e) { + result_->message = e.what(); + result_->success = false; + { + std::scoped_lock lock(goal_mutex_); + current_goal_handle_->abort(result_); + } + return; + } + + // Wait until the robot reached the target mode or something went wrong + while (robot_mode_ != target_mode && !error_) { + RCLCPP_INFO(rclcpp::get_logger("robot_state_helper"), "Waiting for robot to reach target mode... Current_mode: %s", + robotModeString(robot_mode_).c_str()); + std::this_thread::sleep_for(std::chrono::milliseconds(500)); + } + + if (robot_mode_ == target_mode) { + result_->success = true; + result_->message = "Reached target robot mode."; + if (robot_mode_ == urcl::RobotMode::RUNNING && goal_->play_program && !program_running_) { + if (headless_mode_) { + result_->success = safeDashboardTrigger(this->resend_robot_program_srv_); + } else { + // The dashboard denies playing immediately after switching the mode to RUNNING + sleep(1); + result_->success = safeDashboardTrigger(this->play_program_srv_); + } + } + if (result_->success) { + std::scoped_lock lock(goal_mutex_); + current_goal_handle_->succeed(result_); + } else { + std::scoped_lock lock(goal_mutex_); + current_goal_handle_->abort(result_); + } + } else { + result_->success = false; + result_->message = "Robot reached higher mode than requested during recovery. This either means that something " + "went wrong or that a higher mode was requested from somewhere else (e.g. the teach " + "pendant.)"; + { + std::scoped_lock lock(goal_mutex_); + current_goal_handle_->abort(result_); + } + } +} + +rclcpp_action::GoalResponse RobotStateHelper::setModeGoalCallback( + const rclcpp_action::GoalUUID& uuid, std::shared_ptr goal) +{ + (void)uuid; + if (robot_mode_ == urcl::RobotMode::UNKNOWN) { + RCLCPP_ERROR_STREAM(rclcpp::get_logger("robot_state_helper"), "Robot mode is unknown. Cannot accept goal, yet. Is " + "the robot switched on and connected to the driver?"); + return rclcpp_action::GoalResponse::REJECT; + } + + if (safety_mode_ == urcl::SafetyMode::UNDEFINED_SAFETY_MODE) { + RCLCPP_ERROR_STREAM(rclcpp::get_logger("robot_state_helper"), "Safety mode is unknown. Cannot accept goal, yet. Is " + "the robot switched on and connected to the driver?"); + return rclcpp_action::GoalResponse::REJECT; + } + return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; +} + +rclcpp_action::CancelResponse +RobotStateHelper::setModeCancelCallback(const std::shared_ptr goal_handle) +{ + RCLCPP_INFO(rclcpp::get_logger("robot_state_helper"), "Received request to cancel goal"); + (void)goal_handle; + return rclcpp_action::CancelResponse::REJECT; +} + +} // namespace ur_robot_driver diff --git a/ur_robot_driver/src/robot_state_helper_node.cpp b/ur_robot_driver/src/robot_state_helper_node.cpp new file mode 100755 index 000000000..37bf9a902 --- /dev/null +++ b/ur_robot_driver/src/robot_state_helper_node.cpp @@ -0,0 +1,42 @@ +// Copyright 2024, FZI Forschungszentrum Informatik, Created on behalf of 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. + +#include "ur_robot_driver/robot_state_helper.hpp" + +int main(int argc, char** argv) +{ + rclcpp::init(argc, argv); + rclcpp::Node::SharedPtr node = rclcpp::Node::make_shared("robot_state_helper"); + ur_robot_driver::RobotStateHelper state_helper(node); + + rclcpp::executors::MultiThreadedExecutor executor; + executor.add_node(node); + executor.spin(); + + return 0; +} From fc7645ba191c99a7ca07825ca0cbe470f1fbd20c Mon Sep 17 00:00:00 2001 From: "mergify[bot]" <37929162+mergify[bot]@users.noreply.github.com> Date: Mon, 17 Mar 2025 07:54:52 +0100 Subject: [PATCH 050/133] Update transformForceTorque to handle wheter it is a cb3 or an e-Series robot (backport of #1287) (#1299) The force torque is returned at the tool flange on e-series robots and at the tcp for CB3, this is now handled correctly, so that all force/torque measurements will be relative to the active TCP --- .../ur_robot_driver/hardware_interface.hpp | 6 +-- .../resources/rtde_output_recipe.txt | 2 + ur_robot_driver/src/hardware_interface.cpp | 46 ++++++++++++++----- 3 files changed, 39 insertions(+), 15 deletions(-) diff --git a/ur_robot_driver/include/ur_robot_driver/hardware_interface.hpp b/ur_robot_driver/include/ur_robot_driver/hardware_interface.hpp index c9f23e4f5..845720fec 100644 --- a/ur_robot_driver/include/ur_robot_driver/hardware_interface.hpp +++ b/ur_robot_driver/include/ur_robot_driver/hardware_interface.hpp @@ -178,6 +178,8 @@ class URPositionHardwareInterface : public hardware_interface::SystemInterface urcl::vector6d_t urcl_joint_efforts_; urcl::vector6d_t urcl_ft_sensor_measurements_; urcl::vector6d_t urcl_tcp_pose_; + urcl::vector6d_t urcl_target_tcp_pose_; + urcl::vector6d_t tcp_offset_; tf2::Quaternion tcp_rotation_quat_; Quaternion tcp_rotation_buffer; @@ -205,10 +207,6 @@ class URPositionHardwareInterface : public hardware_interface::SystemInterface std::bitset<4> robot_status_bits_; std::bitset<11> safety_status_bits_; - // transform stuff - tf2::Vector3 tcp_force_; - tf2::Vector3 tcp_torque_; - // asynchronous commands std::array standard_dig_out_bits_cmd_; std::array standard_analog_output_cmd_; diff --git a/ur_robot_driver/resources/rtde_output_recipe.txt b/ur_robot_driver/resources/rtde_output_recipe.txt index 8d10be849..c52197cb7 100644 --- a/ur_robot_driver/resources/rtde_output_recipe.txt +++ b/ur_robot_driver/resources/rtde_output_recipe.txt @@ -6,6 +6,7 @@ target_speed_fraction runtime_state actual_TCP_force actual_TCP_pose +target_TCP_pose actual_digital_input_bits actual_digital_output_bits standard_analog_input0 @@ -25,3 +26,4 @@ safety_mode robot_status_bits safety_status_bits actual_current +tcp_offset diff --git a/ur_robot_driver/src/hardware_interface.cpp b/ur_robot_driver/src/hardware_interface.cpp index 4efcf0623..20b789608 100644 --- a/ur_robot_driver/src/hardware_interface.cpp +++ b/ur_robot_driver/src/hardware_interface.cpp @@ -676,6 +676,7 @@ hardware_interface::return_type URPositionHardwareInterface::read(const rclcpp:: readData(data_pkg, "runtime_state", runtime_state_); readData(data_pkg, "actual_TCP_force", urcl_ft_sensor_measurements_); readData(data_pkg, "actual_TCP_pose", urcl_tcp_pose_); + readData(data_pkg, "target_TCP_pose", urcl_target_tcp_pose_); readData(data_pkg, "standard_analog_input0", standard_analog_input_[0]); readData(data_pkg, "standard_analog_input1", standard_analog_input_[1]); readData(data_pkg, "standard_analog_output0", standard_analog_output_[0]); @@ -694,6 +695,7 @@ hardware_interface::return_type URPositionHardwareInterface::read(const rclcpp:: readBitsetData(data_pkg, "actual_digital_output_bits", actual_dig_out_bits_); readBitsetData(data_pkg, "analog_io_types", analog_io_types_); readBitsetData(data_pkg, "tool_analog_input_types", tool_analog_input_types_); + readData(data_pkg, "tcp_offset", tcp_offset_); // required transforms extractToolPose(); @@ -934,17 +936,39 @@ void URPositionHardwareInterface::updateNonDoubleValues() void URPositionHardwareInterface::transformForceTorque() { - // imported from ROS1 driver - hardware_interface.cpp#L867-L876 - tcp_force_.setValue(urcl_ft_sensor_measurements_[0], urcl_ft_sensor_measurements_[1], - urcl_ft_sensor_measurements_[2]); - tcp_torque_.setValue(urcl_ft_sensor_measurements_[3], urcl_ft_sensor_measurements_[4], - urcl_ft_sensor_measurements_[5]); - - tcp_force_ = tf2::quatRotate(tcp_rotation_quat_.inverse(), tcp_force_); - tcp_torque_ = tf2::quatRotate(tcp_rotation_quat_.inverse(), tcp_torque_); - - urcl_ft_sensor_measurements_ = { tcp_force_.x(), tcp_force_.y(), tcp_force_.z(), - tcp_torque_.x(), tcp_torque_.y(), tcp_torque_.z() }; + KDL::Wrench ft( + KDL::Vector(urcl_ft_sensor_measurements_[0], urcl_ft_sensor_measurements_[1], urcl_ft_sensor_measurements_[2]), + KDL::Vector(urcl_ft_sensor_measurements_[3], urcl_ft_sensor_measurements_[4], urcl_ft_sensor_measurements_[5])); + if (ur_driver_->getVersion().major >= 5) // e-Series + { + // Setup necessary frames + KDL::Vector vec = KDL::Vector(tcp_offset_[3], tcp_offset_[4], tcp_offset_[5]); + double angle = vec.Normalize(); + KDL::Rotation rotation = KDL::Rotation::Rot(vec, angle); + KDL::Frame flange_to_tcp = KDL::Frame(rotation, KDL::Vector(tcp_offset_[0], tcp_offset_[1], tcp_offset_[2])); + + vec = KDL::Vector(urcl_target_tcp_pose_[3], urcl_target_tcp_pose_[4], urcl_target_tcp_pose_[5]); + angle = vec.Normalize(); + rotation = KDL::Rotation::Rot(vec, angle); + KDL::Frame base_to_tcp = + KDL::Frame(rotation, KDL::Vector(urcl_target_tcp_pose_[0], urcl_target_tcp_pose_[1], urcl_target_tcp_pose_[2])); + // Calculate transformation from base to flange, see calculation details below + // `base_to_tcp = base_to_flange*flange_to_tcp -> base_to_flange = base_to_tcp * inv(flange_to_tcp)` + KDL::Frame base_to_flange = base_to_tcp * flange_to_tcp.Inverse(); + // rotate f/t sensor output back to the flange frame + ft = base_to_flange.M.Inverse() * ft; + + // Transform the wrench to the tcp frame + ft = flange_to_tcp * ft; + } else { // CB3 + KDL::Vector vec = KDL::Vector(urcl_target_tcp_pose_[3], urcl_target_tcp_pose_[4], urcl_target_tcp_pose_[5]); + double angle = vec.Normalize(); + KDL::Rotation base_to_tcp_rot = KDL::Rotation::Rot(vec, angle); + + // rotate f/t sensor output back to the tcp frame + ft = base_to_tcp_rot.Inverse() * ft; + } + urcl_ft_sensor_measurements_ = { ft[0], ft[1], ft[2], ft[3], ft[4], ft[5] }; } void URPositionHardwareInterface::extractToolPose() From 65a8e77df9f830e780bd6af393a82af41e192a20 Mon Sep 17 00:00:00 2001 From: Felix Exner Date: Mon, 17 Mar 2025 10:54:34 +0100 Subject: [PATCH 051/133] Prepare changelogs for upcoming release --- ur/CHANGELOG.rst | 3 +++ ur_bringup/CHANGELOG.rst | 5 +++++ ur_calibration/CHANGELOG.rst | 3 +++ ur_controllers/CHANGELOG.rst | 6 ++++++ ur_dashboard_msgs/CHANGELOG.rst | 5 +++++ ur_moveit_config/CHANGELOG.rst | 3 +++ ur_robot_driver/CHANGELOG.rst | 11 +++++++++++ 7 files changed, 36 insertions(+) diff --git a/ur/CHANGELOG.rst b/ur/CHANGELOG.rst index 9cdb63792..0b5c2b35e 100644 --- a/ur/CHANGELOG.rst +++ b/ur/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package ur ^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 2.5.2 (2025-01-21) ------------------ diff --git a/ur_bringup/CHANGELOG.rst b/ur_bringup/CHANGELOG.rst index 28b5d359e..dc448b67c 100644 --- a/ur_bringup/CHANGELOG.rst +++ b/ur_bringup/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package ur_bringup ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Fix doc links (`#1247 `_) +* Contributors: Felix Exner + 2.5.2 (2025-01-21) ------------------ * Update pre-commit the same as on the main branch (`#1232 `_) diff --git a/ur_calibration/CHANGELOG.rst b/ur_calibration/CHANGELOG.rst index 8e5b75147..074f7a80b 100644 --- a/ur_calibration/CHANGELOG.rst +++ b/ur_calibration/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package ur_calibration ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 2.5.2 (2025-01-21) ------------------ diff --git a/ur_controllers/CHANGELOG.rst b/ur_controllers/CHANGELOG.rst index dbd46a0dd..46ba2a036 100644 --- a/ur_controllers/CHANGELOG.rst +++ b/ur_controllers/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package ur_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* ur_controllers: doc -- Fix link to index page of driver (backport of `#1284 `_) (`#1285 `_) +* Remove build warnings for Humble (`#1234 `_) +* Contributors: Felix Exner, mergify[bot] + 2.5.2 (2025-01-21) ------------------ * Update pre-commit the same as on the main branch (`#1232 `_) diff --git a/ur_dashboard_msgs/CHANGELOG.rst b/ur_dashboard_msgs/CHANGELOG.rst index 8b5633009..0935211c6 100644 --- a/ur_dashboard_msgs/CHANGELOG.rst +++ b/ur_dashboard_msgs/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package ur_dashboard_msgs ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Port robot_state_helper to ROS2 (backport of `#933 `_) (`#1286 `_) +* Contributors: mergify[bot] + 2.5.2 (2025-01-21) ------------------ diff --git a/ur_moveit_config/CHANGELOG.rst b/ur_moveit_config/CHANGELOG.rst index 12234f566..ffd063fb9 100644 --- a/ur_moveit_config/CHANGELOG.rst +++ b/ur_moveit_config/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package ur_moveit_config ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 2.5.2 (2025-01-21) ------------------ * ur_moveit_config: Do not change default controller when using fake hardware (`#1237 `_) diff --git a/ur_robot_driver/CHANGELOG.rst b/ur_robot_driver/CHANGELOG.rst index d312a3241..a6641a786 100644 --- a/ur_robot_driver/CHANGELOG.rst +++ b/ur_robot_driver/CHANGELOG.rst @@ -1,3 +1,14 @@ +Forthcoming +----------- +* Update transformForceTorque to handle whether it is a cb3 or an e-Series robot (backport of `#1287 `_) (`#1299 `_) +* Port robot_state_helper to ROS2 (backport of `#933 `_) (`#1286 `_) +* Fix crashes on shutting down (`#1270 `_) (`#1271 `_) +* Remove build warnings for Humble (`#1234 `_) +* Auto-update pre-commit hooks (backport `#1260 `_) (`#1261 `_) +* Fix doc links (`#1247 `_) +* Remove urdf folder (`#1257 `_) +* Contributors: Felix Exner, mergify[bot] + 2.5.2 (2025-01-21) ------------------ * Check quaternions for equal dot_product instead of comparing their components individually (backport `#1238 `_) (`#1243 `_) From 298514ac1c87ffae29585e199fd1158d7062e906 Mon Sep 17 00:00:00 2001 From: Felix Exner Date: Mon, 17 Mar 2025 10:54:49 +0100 Subject: [PATCH 052/133] 2.6.0 --- ur/CHANGELOG.rst | 4 ++-- ur/package.xml | 2 +- ur_bringup/CHANGELOG.rst | 4 ++-- ur_bringup/package.xml | 2 +- ur_calibration/CHANGELOG.rst | 4 ++-- ur_calibration/package.xml | 2 +- ur_controllers/CHANGELOG.rst | 4 ++-- ur_controllers/package.xml | 2 +- ur_dashboard_msgs/CHANGELOG.rst | 4 ++-- ur_dashboard_msgs/package.xml | 2 +- ur_moveit_config/CHANGELOG.rst | 4 ++-- ur_moveit_config/package.xml | 2 +- ur_robot_driver/CHANGELOG.rst | 4 ++-- ur_robot_driver/package.xml | 2 +- 14 files changed, 21 insertions(+), 21 deletions(-) diff --git a/ur/CHANGELOG.rst b/ur/CHANGELOG.rst index 0b5c2b35e..32970a6cd 100644 --- a/ur/CHANGELOG.rst +++ b/ur/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package ur ^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.6.0 (2025-03-17) +------------------ 2.5.2 (2025-01-21) ------------------ diff --git a/ur/package.xml b/ur/package.xml index b88fb0579..2a7a97172 100644 --- a/ur/package.xml +++ b/ur/package.xml @@ -2,7 +2,7 @@ ur - 2.5.2 + 2.6.0 Metapackage for universal robots Felix Exner diff --git a/ur_bringup/CHANGELOG.rst b/ur_bringup/CHANGELOG.rst index dc448b67c..1edeb6054 100644 --- a/ur_bringup/CHANGELOG.rst +++ b/ur_bringup/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package ur_bringup ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.6.0 (2025-03-17) +------------------ * Fix doc links (`#1247 `_) * Contributors: Felix Exner diff --git a/ur_bringup/package.xml b/ur_bringup/package.xml index 9561da9ba..f6c94f8b6 100644 --- a/ur_bringup/package.xml +++ b/ur_bringup/package.xml @@ -2,7 +2,7 @@ ur_bringup - 2.5.2 + 2.6.0 Launch file and run-time configurations, e.g. controllers. Felix Exner diff --git a/ur_calibration/CHANGELOG.rst b/ur_calibration/CHANGELOG.rst index 074f7a80b..0915682dd 100644 --- a/ur_calibration/CHANGELOG.rst +++ b/ur_calibration/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package ur_calibration ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.6.0 (2025-03-17) +------------------ 2.5.2 (2025-01-21) ------------------ diff --git a/ur_calibration/package.xml b/ur_calibration/package.xml index 67de5a3ca..01e0a643b 100644 --- a/ur_calibration/package.xml +++ b/ur_calibration/package.xml @@ -1,7 +1,7 @@ ur_calibration - 2.5.2 + 2.6.0 Package for extracting the factory calibration from a UR robot and change it such that it can be used by ur_description to gain a correct URDF Felix Exner diff --git a/ur_controllers/CHANGELOG.rst b/ur_controllers/CHANGELOG.rst index 46ba2a036..8c62ebfe4 100644 --- a/ur_controllers/CHANGELOG.rst +++ b/ur_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package ur_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.6.0 (2025-03-17) +------------------ * ur_controllers: doc -- Fix link to index page of driver (backport of `#1284 `_) (`#1285 `_) * Remove build warnings for Humble (`#1234 `_) * Contributors: Felix Exner, mergify[bot] diff --git a/ur_controllers/package.xml b/ur_controllers/package.xml index f6f169934..b0948f78b 100644 --- a/ur_controllers/package.xml +++ b/ur_controllers/package.xml @@ -2,7 +2,7 @@ ur_controllers - 2.5.2 + 2.6.0 Provides controllers that use the speed scaling interface of Universal Robots. Felix Exner diff --git a/ur_dashboard_msgs/CHANGELOG.rst b/ur_dashboard_msgs/CHANGELOG.rst index 0935211c6..75cf38339 100644 --- a/ur_dashboard_msgs/CHANGELOG.rst +++ b/ur_dashboard_msgs/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package ur_dashboard_msgs ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.6.0 (2025-03-17) +------------------ * Port robot_state_helper to ROS2 (backport of `#933 `_) (`#1286 `_) * Contributors: mergify[bot] diff --git a/ur_dashboard_msgs/package.xml b/ur_dashboard_msgs/package.xml index de4cb451a..f6effe872 100644 --- a/ur_dashboard_msgs/package.xml +++ b/ur_dashboard_msgs/package.xml @@ -2,7 +2,7 @@ ur_dashboard_msgs - 2.5.2 + 2.6.0 Messages around the UR Dashboard server. Felix Exner diff --git a/ur_moveit_config/CHANGELOG.rst b/ur_moveit_config/CHANGELOG.rst index ffd063fb9..884b01ad0 100644 --- a/ur_moveit_config/CHANGELOG.rst +++ b/ur_moveit_config/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package ur_moveit_config ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.6.0 (2025-03-17) +------------------ 2.5.2 (2025-01-21) ------------------ diff --git a/ur_moveit_config/package.xml b/ur_moveit_config/package.xml index fa9e18e97..a03e9943e 100644 --- a/ur_moveit_config/package.xml +++ b/ur_moveit_config/package.xml @@ -2,7 +2,7 @@ ur_moveit_config - 2.5.2 + 2.6.0 An example package with MoveIt2 configurations for UR robots. diff --git a/ur_robot_driver/CHANGELOG.rst b/ur_robot_driver/CHANGELOG.rst index a6641a786..f4e325129 100644 --- a/ur_robot_driver/CHANGELOG.rst +++ b/ur_robot_driver/CHANGELOG.rst @@ -1,5 +1,5 @@ -Forthcoming ------------ +2.6.0 (2025-03-17) +------------------ * Update transformForceTorque to handle whether it is a cb3 or an e-Series robot (backport of `#1287 `_) (`#1299 `_) * Port robot_state_helper to ROS2 (backport of `#933 `_) (`#1286 `_) * Fix crashes on shutting down (`#1270 `_) (`#1271 `_) diff --git a/ur_robot_driver/package.xml b/ur_robot_driver/package.xml index 1cb83c122..8953d242b 100644 --- a/ur_robot_driver/package.xml +++ b/ur_robot_driver/package.xml @@ -2,7 +2,7 @@ ur_robot_driver - 2.5.2 + 2.6.0 The new driver for Universal Robots UR3, UR5 and UR10 robots with CB3 controllers and the e-series. Felix Exner From 204e215c8a7371f6357e6a09f7e106364e566931 Mon Sep 17 00:00:00 2001 From: "mergify[bot]" <37929162+mergify[bot]@users.noreply.github.com> Date: Tue, 18 Mar 2025 12:34:04 +0100 Subject: [PATCH 053/133] Use modern CMake to link against yaml-cpp (backport of #1295) (#1304) * Use modern CMake to link against yaml-cpp * Remove direct yaml-cpp include --- ur_calibration/CMakeLists.txt | 7 ++----- ur_calibration/package.xml | 2 +- 2 files changed, 3 insertions(+), 6 deletions(-) diff --git a/ur_calibration/CMakeLists.txt b/ur_calibration/CMakeLists.txt index 0b17e4d5f..e95014a13 100644 --- a/ur_calibration/CMakeLists.txt +++ b/ur_calibration/CMakeLists.txt @@ -17,11 +17,9 @@ find_package(rclcpp REQUIRED) find_package(ur_robot_driver REQUIRED) find_package(Eigen3 REQUIRED) -find_package(yaml-cpp REQUIRED) +find_package(yaml_cpp_vendor REQUIRED) find_package(ur_client_library REQUIRED) -set(YAML_CPP_INCLUDE_DIRS ${YAML_CPP_INCLUDE_DIR}) - ########### ## Build ## ########### @@ -34,10 +32,9 @@ target_include_directories(calibration PUBLIC include ${EIGEN3_INCLUDE_DIRS} - ${YAML_CPP_INCLUDE_DIRS} ) target_link_libraries(calibration - ${YAML_CPP_LIBRARIES} + yaml-cpp ) ament_target_dependencies(calibration rclcpp diff --git a/ur_calibration/package.xml b/ur_calibration/package.xml index 01e0a643b..2bf3dbfe0 100644 --- a/ur_calibration/package.xml +++ b/ur_calibration/package.xml @@ -19,7 +19,7 @@ ur_robot_driver eigen - yaml-cpp + yaml_cpp_vendor ament_cmake_gmock ament_cmake_gtest From d410371edd1d32d595e3f365d03212e18b7ad8ce Mon Sep 17 00:00:00 2001 From: "mergify[bot]" <37929162+mergify[bot]@users.noreply.github.com> Date: Fri, 28 Mar 2025 16:02:51 +0100 Subject: [PATCH 054/133] Fix passthrough controller to not read non-existing state_interfaces (#1314) (#1316) * passthrough_trajectory: Add a test with a full quintic trajectory * Check for state interfaces being present before attempting to read from it --- .../src/passthrough_trajectory_controller.cpp | 10 ++--- ur_robot_driver/test/robot_driver.py | 37 ++++++++++++++++++- 2 files changed, 41 insertions(+), 6 deletions(-) diff --git a/ur_controllers/src/passthrough_trajectory_controller.cpp b/ur_controllers/src/passthrough_trajectory_controller.cpp index 95f726664..dc2d90dae 100644 --- a/ur_controllers/src/passthrough_trajectory_controller.cpp +++ b/ur_controllers/src/passthrough_trajectory_controller.cpp @@ -579,17 +579,17 @@ bool PassthroughTrajectoryController::check_goal_tolerance() return false; } - if (!active_joint_traj_.points.back().velocities.empty()) { + if (!active_joint_traj_.points.back().velocities.empty() && !joint_velocity_state_interface_.empty()) { const double joint_vel = joint_velocity_state_interface_[i].get().get_value(); const auto& expected_vel = active_joint_traj_.points.back().velocities[joint_mapping->at(joint_name)]; if (std::abs(joint_vel - expected_vel) > joint_tol.velocity) { return false; } } - if (!active_joint_traj_.points.back().accelerations.empty()) { - const double joint_vel = joint_acceleration_state_interface_[i].get().get_value(); - const auto& expected_vel = active_joint_traj_.points.back().accelerations[joint_mapping->at(joint_name)]; - if (std::abs(joint_vel - expected_vel) > joint_tol.acceleration) { + if (!active_joint_traj_.points.back().accelerations.empty() && !joint_acceleration_state_interface_.empty()) { + const double joint_acc = joint_acceleration_state_interface_[i].get().get_value(); + const auto& expected_acc = active_joint_traj_.points.back().accelerations[joint_mapping->at(joint_name)]; + if (std::abs(joint_acc - expected_acc) > joint_tol.acceleration) { return false; } } diff --git a/ur_robot_driver/test/robot_driver.py b/ur_robot_driver/test/robot_driver.py index b0b2a4c84..59cc3a6ec 100644 --- a/ur_robot_driver/test/robot_driver.py +++ b/ur_robot_driver/test/robot_driver.py @@ -360,7 +360,7 @@ def test_passthrough_trajectory(self, tf_prefix): ) waypts = [ [-1.58, -1.692, -1.4311, -0.0174, 1.5882, 0.0349], - [-3, -1.692, -1.4311, -0.0174, 1.5882, 0.0349], + [-2.5, -1.692, -1.4311, -0.0174, 1.5882, 0.0349], [-1.58, -1.692, -1.4311, -0.0174, 1.5882, 0.0349], ] time_vec = [ @@ -392,7 +392,42 @@ def test_passthrough_trajectory(self, tf_prefix): goal_handle, TIMEOUT_EXECUTE_TRAJECTORY ) self.assertEqual(result.error_code, FollowJointTrajectory.Result.SUCCESSFUL) + + # Full quintic trajectory + test_trajectory = zip(time_vec, waypts) + trajectory = JointTrajectory( + points=[ + JointTrajectoryPoint( + positions=pos, + time_from_start=times, + velocities=[0.0, 0.0, 0.0, 0.0, 0.0, 0.0], + accelerations=[0.0, 0.0, 0.0, 0.0, 0.0, 0.0], + ) + for (times, pos) in test_trajectory + ], + joint_names=[tf_prefix + ROBOT_JOINTS[i] for i in range(len(ROBOT_JOINTS))], + ) + goal_handle = self._passthrough_forward_joint_trajectory.send_goal( + trajectory=trajectory, + goal_time_tolerance=goal_time_tolerance, + goal_tolerance=goal_tolerance, + ) + self.assertTrue(goal_handle.accepted) + if goal_handle.accepted: + result = self._passthrough_forward_joint_trajectory.get_result( + goal_handle, TIMEOUT_EXECUTE_TRAJECTORY + ) + self.assertEqual(result.error_code, FollowJointTrajectory.Result.SUCCESSFUL) + # Test impossible goal tolerance, should fail. + test_trajectory = zip(time_vec, waypts) + trajectory = JointTrajectory( + points=[ + JointTrajectoryPoint(positions=pos, time_from_start=times) + for (times, pos) in test_trajectory + ], + joint_names=[tf_prefix + ROBOT_JOINTS[i] for i in range(len(ROBOT_JOINTS))], + ) goal_tolerance = [ JointTolerance(position=0.000000001, name=tf_prefix + ROBOT_JOINTS[i]) for i in range(len(ROBOT_JOINTS)) From 65968c44deb1c4a259e19b95cadb8a4e3ead5afb Mon Sep 17 00:00:00 2001 From: "mergify[bot]" <37929162+mergify[bot]@users.noreply.github.com> Date: Mon, 31 Mar 2025 07:33:10 +0200 Subject: [PATCH 055/133] Auto-update pre-commit hooks (backport #1323) (#1324) --- .pre-commit-config.yaml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml index 74e57b808..cb66c9f1b 100644 --- a/.pre-commit-config.yaml +++ b/.pre-commit-config.yaml @@ -51,7 +51,7 @@ repos: args: ["--ignore=D100,D101,D102,D103,D104,D105,D106,D107,D203,D212,D401,D404"] - repo: https://github.com/pycqa/flake8 - rev: 7.1.2 + rev: 7.2.0 hooks: - id: flake8 args: ["--ignore=E501,W503"] From cbd6e474061459351876518cf1221900c2d9f4bb Mon Sep 17 00:00:00 2001 From: "mergify[bot]" <37929162+mergify[bot]@users.noreply.github.com> Date: Mon, 31 Mar 2025 14:09:16 +0200 Subject: [PATCH 056/133] Remove semi-binary testing builds from ci_status (backport #1321) (#1325) The builds have been removed in 077bac55d909a1759e54d16d222ce8ece3be882e --- ci_status.md | 14 +------------- 1 file changed, 1 insertion(+), 13 deletions(-) diff --git a/ci_status.md b/ci_status.md index e8cd551e7..9850c6e27 100644 --- a/ci_status.md +++ b/ci_status.md @@ -29,10 +29,6 @@ red pipeline there should be a corresponding issue labeled with [ci-failure](htt Humble Semi-Binary Main
- - Humble Semi-Binary Testing - 		@@ -47,10 +43,6 @@ red pipeline there should be a corresponding issue labeled with [ci-failure](htt Jazzy Semi-Binary Main
- - Jazzy Semi-Binary Testing - 		@@ -65,10 +57,6 @@ red pipeline there should be a corresponding issue labeled with [ci-failure](htt Rolling Semi-Binary Main
- - Rolling Semi-Binary Testing -
ROS2 Distro Humble JazzyKilted Rolling
Branchhumblemainmain
Release status +
Branchhumblejazzymainmain
Release status
@@ -46,6 +46,13 @@ Check also [presentations and videos](ur_robot_driver/doc/resources/README.md) a 		+
+ + + + +
diff --git a/ci_status.md b/ci_status.md index 9850c6e27..7c26e67d6 100644 --- a/ci_status.md +++ b/ci_status.md @@ -8,10 +8,12 @@ red pipeline there should be a corresponding issue labeled with [ci-failure](htt
Humble JazzyKilted Rolling
humblejazzy main main
- - + Jazzy Binary Main
- - + Jazzy Binary Testing
- - + Jazzy Semi-Binary Main
+ + Kilted Binary Main +
+ + Kilted Binary Testing +
+ + Kilted Semi-Binary Main +
+ 		+
+ + + + +
From 683e969f2866be99357307569ec31bd34fb2b853 Mon Sep 17 00:00:00 2001 From: Felix Exner Date: Tue, 20 May 2025 13:26:08 +0200 Subject: [PATCH 069/133] Update changelogs for upcoming release --- ur/CHANGELOG.rst | 3 +++ ur_bringup/CHANGELOG.rst | 5 +++++ ur_calibration/CHANGELOG.rst | 3 +++ ur_controllers/CHANGELOG.rst | 5 +++++ ur_dashboard_msgs/CHANGELOG.rst | 3 +++ ur_moveit_config/CHANGELOG.rst | 5 +++++ ur_robot_driver/CHANGELOG.rst | 8 ++++++++ 7 files changed, 32 insertions(+) diff --git a/ur/CHANGELOG.rst b/ur/CHANGELOG.rst index 3ccb800fc..4b892760b 100644 --- a/ur/CHANGELOG.rst +++ b/ur/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package ur ^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 2.7.0 (2025-04-10) ------------------ diff --git a/ur_bringup/CHANGELOG.rst b/ur_bringup/CHANGELOG.rst index 6c0177c1b..8be155228 100644 --- a/ur_bringup/CHANGELOG.rst +++ b/ur_bringup/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package ur_bringup ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Add support for launching a UR15 robot (`#1359 `_) +* Contributors: Felix Exner + 2.7.0 (2025-04-10) ------------------ * Add support for UR7e and UR12e (`#1332 `_) diff --git a/ur_calibration/CHANGELOG.rst b/ur_calibration/CHANGELOG.rst index 1986b52f3..59c8f56db 100644 --- a/ur_calibration/CHANGELOG.rst +++ b/ur_calibration/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package ur_calibration ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 2.7.0 (2025-04-10) ------------------ * Use modern CMake to link against yaml-cpp (backport of `#1295 `_) (`#1304 `_) diff --git a/ur_controllers/CHANGELOG.rst b/ur_controllers/CHANGELOG.rst index 13ae58dcd..c1f527fe3 100644 --- a/ur_controllers/CHANGELOG.rst +++ b/ur_controllers/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package ur_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Added controller to enable and disable tool contact (backport of `#940 `_) (`#1337 `_) +* Contributors: mergify[bot] + 2.7.0 (2025-04-10) ------------------ * Start executing passthrough trajectories earlier than all points are transferred. (backport of `#1313 `_) (`#1335 `_) diff --git a/ur_dashboard_msgs/CHANGELOG.rst b/ur_dashboard_msgs/CHANGELOG.rst index 0b2f985f6..05c14c6b9 100644 --- a/ur_dashboard_msgs/CHANGELOG.rst +++ b/ur_dashboard_msgs/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package ur_dashboard_msgs ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 2.7.0 (2025-04-10) ------------------ diff --git a/ur_moveit_config/CHANGELOG.rst b/ur_moveit_config/CHANGELOG.rst index c8ab625ac..4a1ed9b48 100644 --- a/ur_moveit_config/CHANGELOG.rst +++ b/ur_moveit_config/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package ur_moveit_config ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Add support for launching a UR15 robot (`#1359 `_) +* Contributors: Felix Exner + 2.7.0 (2025-04-10) ------------------ * Add support for UR7e and UR12e (`#1332 `_) diff --git a/ur_robot_driver/CHANGELOG.rst b/ur_robot_driver/CHANGELOG.rst index a67312b3b..77f5a9144 100644 --- a/ur_robot_driver/CHANGELOG.rst +++ b/ur_robot_driver/CHANGELOG.rst @@ -1,3 +1,11 @@ +Forthcoming +----------- +* [CI] Check links using lychee instead of a custom script (backport `#1355 `_) (`#1361 `_) +* Add support for launching a UR15 robot (`#1359 `_) +* tool_contact_test: Check result status directly (backport of `#1345 `_) (`#1353 `_) +* Added controller to enable and disable tool contact (backport of `#940 `_) (`#1337 `_) +* Contributors: Felix Exner, mergify[bot] + 2.7.0 (2025-04-10) ------------------ * Start executing passthrough trajectories earlier than all points are transferred. (backport of `#1313 `_) (`#1335 `_) From f1b93a9b056f3353c452410467ebf3562b5ea8e0 Mon Sep 17 00:00:00 2001 From: Felix Exner Date: Tue, 20 May 2025 13:26:27 +0200 Subject: [PATCH 070/133] 2.8.0 --- ur/CHANGELOG.rst | 4 ++-- ur/package.xml | 2 +- ur_bringup/CHANGELOG.rst | 4 ++-- ur_bringup/package.xml | 2 +- ur_calibration/CHANGELOG.rst | 4 ++-- ur_calibration/package.xml | 2 +- ur_controllers/CHANGELOG.rst | 4 ++-- ur_controllers/package.xml | 2 +- ur_dashboard_msgs/CHANGELOG.rst | 4 ++-- ur_dashboard_msgs/package.xml | 2 +- ur_moveit_config/CHANGELOG.rst | 4 ++-- ur_moveit_config/package.xml | 2 +- ur_robot_driver/CHANGELOG.rst | 4 ++-- ur_robot_driver/package.xml | 2 +- 14 files changed, 21 insertions(+), 21 deletions(-) diff --git a/ur/CHANGELOG.rst b/ur/CHANGELOG.rst index 4b892760b..61737304a 100644 --- a/ur/CHANGELOG.rst +++ b/ur/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package ur ^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.8.0 (2025-05-20) +------------------ 2.7.0 (2025-04-10) ------------------ diff --git a/ur/package.xml b/ur/package.xml index 84801aae6..18b4560be 100644 --- a/ur/package.xml +++ b/ur/package.xml @@ -2,7 +2,7 @@ ur - 2.7.0 + 2.8.0 Metapackage for universal robots Felix Exner diff --git a/ur_bringup/CHANGELOG.rst b/ur_bringup/CHANGELOG.rst index 8be155228..d1819e98a 100644 --- a/ur_bringup/CHANGELOG.rst +++ b/ur_bringup/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package ur_bringup ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.8.0 (2025-05-20) +------------------ * Add support for launching a UR15 robot (`#1359 `_) * Contributors: Felix Exner diff --git a/ur_bringup/package.xml b/ur_bringup/package.xml index 6a7e9c55a..f47f895d1 100644 --- a/ur_bringup/package.xml +++ b/ur_bringup/package.xml @@ -2,7 +2,7 @@ ur_bringup - 2.7.0 + 2.8.0 Launch file and run-time configurations, e.g. controllers. Felix Exner diff --git a/ur_calibration/CHANGELOG.rst b/ur_calibration/CHANGELOG.rst index 59c8f56db..1ae1a03ae 100644 --- a/ur_calibration/CHANGELOG.rst +++ b/ur_calibration/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package ur_calibration ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.8.0 (2025-05-20) +------------------ 2.7.0 (2025-04-10) ------------------ diff --git a/ur_calibration/package.xml b/ur_calibration/package.xml index 42a9eb29d..b84089c8e 100644 --- a/ur_calibration/package.xml +++ b/ur_calibration/package.xml @@ -1,7 +1,7 @@ ur_calibration - 2.7.0 + 2.8.0 Package for extracting the factory calibration from a UR robot and change it such that it can be used by ur_description to gain a correct URDF Felix Exner diff --git a/ur_controllers/CHANGELOG.rst b/ur_controllers/CHANGELOG.rst index c1f527fe3..2cf21f08c 100644 --- a/ur_controllers/CHANGELOG.rst +++ b/ur_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package ur_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.8.0 (2025-05-20) +------------------ * Added controller to enable and disable tool contact (backport of `#940 `_) (`#1337 `_) * Contributors: mergify[bot] diff --git a/ur_controllers/package.xml b/ur_controllers/package.xml index 19eda47d5..71d1e5751 100644 --- a/ur_controllers/package.xml +++ b/ur_controllers/package.xml @@ -2,7 +2,7 @@ ur_controllers - 2.7.0 + 2.8.0 Provides controllers that use the speed scaling interface of Universal Robots. Felix Exner diff --git a/ur_dashboard_msgs/CHANGELOG.rst b/ur_dashboard_msgs/CHANGELOG.rst index 05c14c6b9..709fe3fe3 100644 --- a/ur_dashboard_msgs/CHANGELOG.rst +++ b/ur_dashboard_msgs/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package ur_dashboard_msgs ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.8.0 (2025-05-20) +------------------ 2.7.0 (2025-04-10) ------------------ diff --git a/ur_dashboard_msgs/package.xml b/ur_dashboard_msgs/package.xml index 8af37e620..5553c55ec 100644 --- a/ur_dashboard_msgs/package.xml +++ b/ur_dashboard_msgs/package.xml @@ -2,7 +2,7 @@ ur_dashboard_msgs - 2.7.0 + 2.8.0 Messages around the UR Dashboard server. Felix Exner diff --git a/ur_moveit_config/CHANGELOG.rst b/ur_moveit_config/CHANGELOG.rst index 4a1ed9b48..56522e4fd 100644 --- a/ur_moveit_config/CHANGELOG.rst +++ b/ur_moveit_config/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package ur_moveit_config ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.8.0 (2025-05-20) +------------------ * Add support for launching a UR15 robot (`#1359 `_) * Contributors: Felix Exner diff --git a/ur_moveit_config/package.xml b/ur_moveit_config/package.xml index 514449019..be9143658 100644 --- a/ur_moveit_config/package.xml +++ b/ur_moveit_config/package.xml @@ -2,7 +2,7 @@ ur_moveit_config - 2.7.0 + 2.8.0 An example package with MoveIt2 configurations for UR robots. diff --git a/ur_robot_driver/CHANGELOG.rst b/ur_robot_driver/CHANGELOG.rst index 77f5a9144..bd1b9d604 100644 --- a/ur_robot_driver/CHANGELOG.rst +++ b/ur_robot_driver/CHANGELOG.rst @@ -1,5 +1,5 @@ -Forthcoming ------------ +2.8.0 (2025-05-20) +------------------ * [CI] Check links using lychee instead of a custom script (backport `#1355 `_) (`#1361 `_) * Add support for launching a UR15 robot (`#1359 `_) * tool_contact_test: Check result status directly (backport of `#1345 `_) (`#1353 `_) diff --git a/ur_robot_driver/package.xml b/ur_robot_driver/package.xml index 0ff74f09a..73af22641 100644 --- a/ur_robot_driver/package.xml +++ b/ur_robot_driver/package.xml @@ -2,7 +2,7 @@ ur_robot_driver - 2.7.0 + 2.8.0 The new driver for Universal Robots UR3, UR5 and UR10 robots with CB3 controllers and the e-series. Felix Exner From a08d8abb946db5454ecfbbcbba1312ce4e1c7b10 Mon Sep 17 00:00:00 2001 From: "mergify[bot]" <37929162+mergify[bot]@users.noreply.github.com> Date: Mon, 26 May 2025 09:44:16 +0200 Subject: [PATCH 071/133] Auto-update pre-commit hooks (backport of #1376) --- .pre-commit-config.yaml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml index cb66c9f1b..7a478aefe 100644 --- a/.pre-commit-config.yaml +++ b/.pre-commit-config.yaml @@ -33,7 +33,7 @@ repos: # Python hooks - repo: https://github.com/asottile/pyupgrade - rev: v3.19.1 + rev: v3.20.0 hooks: - id: pyupgrade args: [--py36-plus] From 48d32b8a3017198fc99e0dfabf24f1cb63f3f4fe Mon Sep 17 00:00:00 2001 From: "mergify[bot]" <37929162+mergify[bot]@users.noreply.github.com> Date: Tue, 27 May 2025 09:57:50 +0200 Subject: [PATCH 072/133] Update feature list (backport of #1372) (#1374) --- ur_calibration/doc/index.rst | 2 ++ ur_robot_driver/doc/features.rst | 26 +++++++++++++++++--------- 2 files changed, 19 insertions(+), 9 deletions(-) diff --git a/ur_calibration/doc/index.rst b/ur_calibration/doc/index.rst index c91768eca..05f71322a 100644 --- a/ur_calibration/doc/index.rst +++ b/ur_calibration/doc/index.rst @@ -1,4 +1,6 @@ +.. _ur_calibration: + ur_calibration ============== diff --git a/ur_robot_driver/doc/features.rst b/ur_robot_driver/doc/features.rst index 535f3be37..085be27e6 100644 --- a/ur_robot_driver/doc/features.rst +++ b/ur_robot_driver/doc/features.rst @@ -13,7 +13,8 @@ Feature list and roadmap * - joint-position-based control - yes * - scaled joint-position-based control - - yes + - yes (`scaled_jtc + `_) * - joint-velocity-based control - yes\ :raw-html-m2r:`1` * - Cartesian position-based control @@ -21,7 +22,7 @@ Feature list and roadmap * - Cartesian twist-based control - no * - Trajectory forwarding for execution on robot - - no + - yes (`passthrough_trajectory_controller `_) * - reporting of tcp wrench - yes * - pausing of programs @@ -33,9 +34,9 @@ Feature list and roadmap * - panel interaction in between possible - yes * - get and set IO states - - yes + - yes (`io_and_status_controller `_) * - use `tool communication forwarder `_ on e-series - - yes + - yes (:ref:`setup-tool-communication`) * - use the driver without a teach pendant necessary - yes * - support of CB1 and CB2 robots @@ -45,13 +46,20 @@ Feature list and roadmap * - use ROS as drop-in for TP-programs - yes * - headless mode - - yes + - yes (`headless_mode + `_) * - extract calibration from robot - - yes + - yes (`ur_calibration `_) * - send custom script commands to robot - - no + - yes (`io_and_status_controller`_) * - Reconnect on a disconnected robot - yes + * - Freedrive Mode + - yes (`freedrive_mode_controller + `_) + * - Tool Contact mode + - yes (`tool_contact_controller `_) + * - Force Mode + - yes (`force_mode_controller `_) - -:raw-html-m2r:`1` Velocity-based joint control is implemented in the driver, but the current version of ros2_control do not yet support Velocity-based joint trajectory control +:raw-html-m2r:`1` Velocity-based joint control is implemented in the driver, the velocity-based joint trajectory controller would need tweaking of the gain parameters for each model. From 4db741e69bab18eaf2d50bb774d5bdbab2620fbe Mon Sep 17 00:00:00 2001 From: "mergify[bot]" <37929162+mergify[bot]@users.noreply.github.com> Date: Mon, 2 Jun 2025 17:56:23 +0200 Subject: [PATCH 073/133] [force mode controller] Fix the task frame orientation (backport #1379) (#1380) Before, we were using a RPY notation. Chaning that to an angle-axis notation should fix things. * Update force_mode example to include a pose that is not in line with one of the main axes * Make test use an arbitraty orientation This should avoid a coincitental overlap if the transformation is wrong. (cherry picked from commit 8eb4288e3099a1c5458ca208d0240c666436fba1) # Conflicts: # ur_robot_driver/test/integration_test_force_mode.py * [force mode controller] Fix the task frame orientation (#1379) Before, we were using a RPY notation. Chaning that to an angle-axis notation should fix things. * Update force_mode example to include a pose that is not in line with one of the main axes * Make test use an arbitraty orientation This should avoid a coincitental overlap if the transformation is wrong. (cherry picked from commit 8eb4288e3099a1c5458ca208d0240c666436fba1) # Conflicts: # ur_robot_driver/test/integration_test_force_mode.py --------- Co-authored-by: Felix Exner --- ur_controllers/src/force_mode_controller.cpp | 8 +- ur_robot_driver/examples/force_mode.py | 290 ++++++++++++------ .../test/integration_test_force_mode.py | 148 +++++++-- 3 files changed, 323 insertions(+), 123 deletions(-) diff --git a/ur_controllers/src/force_mode_controller.cpp b/ur_controllers/src/force_mode_controller.cpp index 3ecdd6cce..f52ec3469 100644 --- a/ur_controllers/src/force_mode_controller.cpp +++ b/ur_controllers/src/force_mode_controller.cpp @@ -260,9 +260,11 @@ bool ForceModeController::setForceMode(const ur_msgs::srv::SetForceMode::Request tf2::Quaternion quat_tf; tf2::convert(task_frame_transformed.pose.orientation, quat_tf); - tf2::Matrix3x3 rot_mat(quat_tf); - rot_mat.getRPY(force_mode_parameters.task_frame[3], force_mode_parameters.task_frame[4], - force_mode_parameters.task_frame[5]); + const double angle = quat_tf.getAngle(); + const auto axis = quat_tf.getAxis(); + force_mode_parameters.task_frame[3] = axis.x() * angle; // rx + force_mode_parameters.task_frame[4] = axis.y() * angle; // ry + force_mode_parameters.task_frame[5] = axis.z() * angle; // rz } catch (const tf2::TransformException& ex) { RCLCPP_ERROR(get_node()->get_logger(), "Could not transform %s to robot base: %s", req->task_frame.header.frame_id.c_str(), ex.what()); diff --git a/ur_robot_driver/examples/force_mode.py b/ur_robot_driver/examples/force_mode.py index 86ea05b70..f5b5c33c4 100755 --- a/ur_robot_driver/examples/force_mode.py +++ b/ur_robot_driver/examples/force_mode.py @@ -1,5 +1,5 @@ #!/usr/bin/env python3 -# Copyright 2024, Universal Robots A/S +# 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: @@ -27,116 +27,218 @@ # ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE # POSSIBILITY OF SUCH DAMAGE. +import sys import time +from tf2_ros import TransformException +from tf2_ros.buffer import Buffer +from tf2_ros.transform_listener import TransformListener +import tf2_geometry_msgs # noqa # pylint: disable=unused-import + +from builtin_interfaces.msg import Duration + import rclpy +from rclpy.callback_groups import MutuallyExclusiveCallbackGroup +from rclpy.executors import ExternalShutdownException, MultiThreadedExecutor from rclpy.node import Node from controller_manager_msgs.srv import SwitchController -from builtin_interfaces.msg import Duration from geometry_msgs.msg import Twist -from std_msgs.msg import Header from std_srvs.srv import Trigger +from visualization_msgs.msg import Marker + from geometry_msgs.msg import ( Point, - Quaternion, - Pose, PoseStamped, Wrench, Vector3, + Vector3Stamped, ) - from ur_msgs.srv import SetForceMode from examples import Robot + +class ForceModeExample(Node): + def __init__(self): + super().__init__("force_mode_example") + + self.robot = Robot(self) + # Add force mode service to service interfaces and re-init robot + self.robot.service_interfaces.update( + {"/force_mode_controller/start_force_mode": SetForceMode} + ) + self.robot.service_interfaces.update({"/force_mode_controller/stop_force_mode": Trigger}) + self.robot.init_robot() + + self.marker_publisher = self.create_publisher(Marker, "direction", 10) + self.tf_buffer = Buffer() + self.tf_listener = TransformListener(self.tf_buffer, self) + + self.direction = Vector3(x=0.0, y=0.0, z=1.0) + + self.marker_published = False + self.force_mode_start_time = None + self.startup() + + self.timer_cb_group = MutuallyExclusiveCallbackGroup() + self.timer = self.create_timer(1.0, self.on_timer, self.timer_cb_group) + + def startup(self): + # Press play on the robot + self.robot.call_service("/dashboard_client/play", Trigger.Request()) + + time.sleep(0.5) + # Start controllers + self.robot.call_service( + "/controller_manager/switch_controller", + SwitchController.Request( + deactivate_controllers=[ + "scaled_joint_trajectory_controller", + "forward_position_controller", + ], + activate_controllers=[ + "passthrough_trajectory_controller", + "force_mode_controller", + ], + strictness=SwitchController.Request.BEST_EFFORT, + ), + ) + self.move_to_starting_pose() + + def on_timer(self): + if not self.marker_published: + self.publish_direction_marker() + self.marker_published = True + elif self.force_mode_start_time is None: + self.start_force_mode() + self.force_mode_start_time = self.get_clock().now() + self.get_logger().info("Force mode started.") + elif (self.get_clock().now() - self.force_mode_start_time).nanoseconds > 3e9: + self.get_logger().info("Stopping force mode after 3 seconds.") + self.robot.call_service("/force_mode_controller/stop_force_mode", Trigger.Request()) + sys.exit(0) + + def move_to_starting_pose(self): + # Move robot in to position + self.robot.send_trajectory( + waypts=[[-1.6, -1.55, -1.7, -1.0, 2.05, 0.5]], + time_vec=[Duration(sec=5, nanosec=0)], + action_client=self.robot.passthrough_trajectory_action_client, + ) + + def start_force_mode(self): + # Create task frame for force mode + frame_stamp = PoseStamped() + frame_stamp.header.frame_id = "tool0_controller" + frame_stamp.pose.position.x = 0.0 + frame_stamp.pose.position.y = 0.0 + frame_stamp.pose.position.z = 0.0 + frame_stamp.pose.orientation.x = 0.0 + frame_stamp.pose.orientation.y = 0.0 + frame_stamp.pose.orientation.z = 0.0 + frame_stamp.pose.orientation.w = 1.0 + + wrench_vec = Wrench( + force=Vector3(x=0.0, y=0.0, z=10.0), torque=Vector3(x=0.0, y=0.0, z=0.0) + ) + type_spec = SetForceMode.Request.NO_TRANSFORM + + # Specify max speeds and deviations of force mode + speed_limits = Twist() + speed_limits.linear = Vector3(x=0.0, y=0.0, z=1.0) + speed_limits.angular = Vector3(x=0.0, y=0.0, z=0.0) + deviation_limits = [0.005, 0.005, 0.005, 0.005, 0.005, 0.005] + + # specify damping and gain scaling + damping_factor = 0.1 + gain_scale = 0.8 + + req = SetForceMode.Request() + req.task_frame = frame_stamp + req.selection_vector_x = False + req.selection_vector_y = False + req.selection_vector_z = True + req.selection_vector_rx = False + req.selection_vector_ry = False + req.selection_vector_rz = False + req.wrench = wrench_vec + req.type = type_spec + req.speed_limits = speed_limits + req.deviation_limits = deviation_limits + req.damping_factor = damping_factor + req.gain_scaling = gain_scale + + self.get_logger().info(f"Starting force mode with {req}") + self.robot.call_service("/force_mode_controller/start_force_mode", req) + + def publish_direction_marker(self): + """Publish a line strip going from the current TCP position to the desired direction.""" + to_frame_rel = "base" + from_frame_rel = "tool0_controller" + try: + t = self.tf_buffer.lookup_transform( + to_frame_rel, + from_frame_rel, + rclpy.time.Time(), + timeout=rclpy.time.Duration(seconds=10.0), + ) + self.get_logger().info( + f"[{t.transform.translation.x}, {t.transform.translation.y}, {t.transform.translation.z}]" + ) + direction_vec = Vector3Stamped() + direction_vec.header.frame_id = from_frame_rel + direction_vec.header.stamp = rclpy.time.Time() + direction_vec.vector = self.direction + transformed_direction = self.tf_buffer.transform( + direction_vec, to_frame_rel, timeout=rclpy.time.Duration(seconds=1.0) + ) + except TransformException as ex: + self.get_logger().info(f"Could not transform {to_frame_rel} to {from_frame_rel}: {ex}") + return + + marker = Marker() + marker.header.frame_id = "base" + marker.header.stamp = self.get_clock().now().to_msg() + marker.type = marker.ARROW + marker.id = 0 + marker.action = marker.ADD + marker.scale.x = 0.01 + marker.scale.y = 0.02 + marker.color.r = 1.0 + marker.color.g = 0.0 + marker.color.b = 0.0 + marker.color.a = 1.0 + marker.points.append( + Point( + x=t.transform.translation.x, + y=t.transform.translation.y, + z=t.transform.translation.z, + ) + ) + marker.points.append( + Point( + x=t.transform.translation.x + transformed_direction.vector.x, + y=t.transform.translation.y + transformed_direction.vector.y, + z=t.transform.translation.z + transformed_direction.vector.z, + ) + ) + self.marker_publisher.publish(marker) + + if __name__ == "__main__": rclpy.init() - node = Node("robot_driver_test") - robot = Robot(node) - - # Add force mode service to service interfaces and re-init robot - robot.service_interfaces.update({"/force_mode_controller/start_force_mode": SetForceMode}) - robot.service_interfaces.update({"/force_mode_controller/stop_force_mode": Trigger}) - robot.init_robot() - time.sleep(0.5) - # Press play on the robot - robot.call_service("/dashboard_client/play", Trigger.Request()) - - time.sleep(0.5) - # Start controllers - robot.call_service( - "/controller_manager/switch_controller", - SwitchController.Request( - deactivate_controllers=["scaled_joint_trajectory_controller"], - activate_controllers=["passthrough_trajectory_controller", "force_mode_controller"], - strictness=SwitchController.Request.BEST_EFFORT, - ), - ) - - # Move robot in to position - robot.send_trajectory( - waypts=[[-1.5707, -1.5707, -1.5707, -1.5707, 1.5707, 0]], - time_vec=[Duration(sec=5, nanosec=0)], - action_client=robot.passthrough_trajectory_action_client, - ) - - # Finished moving - # Create task frame for force mode - point = Point(x=0.0, y=0.0, z=0.0) - orientation = Quaternion(x=0.0, y=0.0, z=0.0, w=1.0) - task_frame_pose = Pose() - task_frame_pose.position = point - task_frame_pose.orientation = orientation - header = Header(seq=1, frame_id="world") - header.stamp.sec = int(time.time()) + 1 - header.stamp.nanosec = 0 - frame_stamp = PoseStamped() - frame_stamp.header = header - frame_stamp.pose = task_frame_pose - - # Create compliance vector (which axes should be force controlled) - compliance = [False, False, True, False, False, False] - - # Create Wrench message for force mode - wrench_vec = Wrench(force=Vector3(x=0.0, y=0.0, z=-20.0), torque=Vector3(x=0.0, y=0.0, z=0.0)) - # Specify interpretation of task frame (no transform) - type_spec = SetForceMode.Request.NO_TRANSFORM - - # Specify max speeds and deviations of force mode - speed_limits = Twist() - speed_limits.linear = Vector3(x=0.0, y=0.0, z=1.0) - speed_limits.angular = Vector3(x=0.0, y=0.0, z=1.0) - deviation_limits = [1.0, 1.0, 1.0, 1.0, 1.0, 1.0] - - # specify damping and gain scaling - damping_factor = 0.1 - gain_scale = 0.8 - - req = SetForceMode.Request() - req.task_frame = frame_stamp - req.selection_vector_x = compliance[0] - req.selection_vector_y = compliance[1] - req.selection_vector_z = compliance[2] - req.selection_vector_rx = compliance[3] - req.selection_vector_ry = compliance[4] - req.selection_vector_rz = compliance[5] - req.wrench = wrench_vec - req.type = type_spec - req.speed_limits = speed_limits - req.deviation_limits = deviation_limits - req.damping_factor = damping_factor - req.gain_scaling = gain_scale - - # Send request to controller - node.get_logger().info(f"Starting force mode with {req}") - robot.call_service("/force_mode_controller/start_force_mode", req) - robot.send_trajectory( - waypts=[[1.5707, -1.5707, -1.5707, -1.5707, 1.5707, 0]], - time_vec=[Duration(sec=5, nanosec=0)], - action_client=robot.passthrough_trajectory_action_client, - ) - - time.sleep(3) - node.get_logger().info("Deactivating force mode controller.") - robot.call_service("/force_mode_controller/stop_force_mode", Trigger.Request()) + + node = ForceModeExample() + executor = MultiThreadedExecutor() + executor.add_node(node) + try: + rclpy.spin(node) + except (KeyboardInterrupt, ExternalShutdownException): + pass + + rclpy.shutdown() + + # time.sleep(0.5) + + # # Send request to controller diff --git a/ur_robot_driver/test/integration_test_force_mode.py b/ur_robot_driver/test/integration_test_force_mode.py index a1dc89f23..8b059b8fe 100644 --- a/ur_robot_driver/test/integration_test_force_mode.py +++ b/ur_robot_driver/test/integration_test_force_mode.py @@ -41,7 +41,11 @@ from tf2_ros import TransformException from tf2_ros.buffer import Buffer from tf2_ros.transform_listener import TransformListener +import tf2_geometry_msgs # noqa: F401 # pylint: disable=unused-import +from builtin_interfaces.msg import Duration +from control_msgs.action import FollowJointTrajectory +from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint import std_msgs from controller_manager_msgs.srv import SwitchController from geometry_msgs.msg import ( @@ -52,16 +56,19 @@ Twist, Wrench, Vector3, + Vector3Stamped, ) sys.path.append(os.path.dirname(__file__)) from test_common import ( # noqa: E402 + ActionInterface, ControllerManagerInterface, DashboardInterface, ForceModeInterface, IoStatusInterface, ConfigurationInterface, generate_driver_test_description, + ROBOT_JOINTS, ) TIMEOUT_EXECUTE_TRAJECTORY = 30 @@ -101,6 +108,11 @@ def init_robot(self): self._controller_manager_interface = ControllerManagerInterface(self.node) self._io_status_controller_interface = IoStatusInterface(self.node) self._configuration_controller_interface = ConfigurationInterface(self.node) + self._passthrough_forward_joint_trajectory = ActionInterface( + self.node, + "/passthrough_trajectory_controller/follow_joint_trajectory", + FollowJointTrajectory, + ) def setUp(self): self._dashboard_interface.start_robot() @@ -121,30 +133,18 @@ def lookup_tcp_in_base(self, tf_prefix, timepoint): pass return trans - def test_force_mode_controller(self, tf_prefix): - self.assertTrue( - self._controller_manager_interface.switch_controller( - strictness=SwitchController.Request.BEST_EFFORT, - activate_controllers=[ - "force_mode_controller", - ], - deactivate_controllers=[ - "scaled_joint_trajectory_controller", - "joint_trajectory_controller", - ], - ).ok - ) + # Implementation of force mode test to be reused + # todo: If we move to pytest this could be done using parametrization + def run_force_mode(self, tf_prefix): self._force_mode_controller_interface = ForceModeInterface(self.node) # Create task frame for force mode - point = Point(x=0.8, y=0.8, z=0.8) + point = Point(x=0.0, y=0.0, z=0.0) orientation = Quaternion(x=0.7071, y=0.0, z=0.0, w=0.7071) task_frame_pose = Pose() task_frame_pose.position = point task_frame_pose.orientation = orientation - header = std_msgs.msg.Header(frame_id=tf_prefix + "base") - header.stamp.sec = int(time.time()) + 1 - header.stamp.nanosec = 0 + header = std_msgs.msg.Header(frame_id=tf_prefix + "tool0_controller") frame_stamp = PoseStamped() frame_stamp.header = header frame_stamp.pose = task_frame_pose @@ -154,7 +154,7 @@ def test_force_mode_controller(self, tf_prefix): # Create Wrench message for force mode wrench = Wrench() - wrench.force = Vector3(x=0.0, y=0.0, z=5.0) + wrench.force = Vector3(x=0.0, y=0.0, z=10.0) wrench.torque = Vector3(x=0.0, y=0.0, z=0.0) # Specify interpretation of task frame (no transform) @@ -193,27 +193,46 @@ def test_force_mode_controller(self, tf_prefix): time.sleep(5.0) trans_after = self.lookup_tcp_in_base(tf_prefix, self.node.get_clock().now()) + diff = Vector3Stamped( + vector=Vector3( + x=(trans_after.transform.translation.x - trans_before.transform.translation.x), + y=(trans_after.transform.translation.y - trans_before.transform.translation.y), + z=(trans_after.transform.translation.z - trans_before.transform.translation.z), + ), + header=trans_after.header, + ) + diff_in_tool0_controller = self.tf_buffer.transform( + diff, + tf_prefix + "tool0_controller", + timeout=rclpy.time.Duration(seconds=1.0), + ) # task frame and wrench determines the expected motion # In the example we used - # - a task frame rotated pi/2 deg around the base frame's x axis + # - a task frame rotated pi/2 deg around the tcp frame's x axis # - a wrench with a positive z component for the force - # => we should expect a motion in negative y of the base frame - self.assertTrue(trans_after.transform.translation.y < trans_before.transform.translation.y) + # => we should expect a motion in negative y of the tcp frame, since we didn't to any + # rotation + self.assertTrue( + diff_in_tool0_controller.vector.y < -0.03, + ) self.assertAlmostEqual( - trans_after.transform.translation.x, - trans_before.transform.translation.x, + diff_in_tool0_controller.vector.x, + 0.0, delta=0.001, + msg="X translation should not change", ) self.assertAlmostEqual( - trans_after.transform.translation.z, - trans_before.transform.translation.z, + diff_in_tool0_controller.vector.z, + 0.0, delta=0.001, + msg="Z translation should not change", ) self.assertTrue( are_quaternions_same( trans_after.transform.rotation, trans_before.transform.rotation, 0.001 - ) + ), + msg="Rotation should not change", ) res = self._force_mode_controller_interface.stop_force_mode() @@ -227,6 +246,83 @@ def test_force_mode_controller(self, tf_prefix): ).ok ) + def test_force_mode_controller(self, tf_prefix): + self.assertTrue( + self._controller_manager_interface.switch_controller( + strictness=SwitchController.Request.BEST_EFFORT, + activate_controllers=[ + "passthrough_trajectory_controller", + ], + deactivate_controllers=[ + "scaled_joint_trajectory_controller", + "joint_trajectory_controller", + "force_mode_controller", + ], + ).ok + ) + waypts = [[-1.6, -1.55, -1.7, -1.0, 2.05, 0.5]] + time_vec = [Duration(sec=5, nanosec=0)] + test_trajectory = zip(time_vec, waypts) + trajectory = JointTrajectory( + points=[ + JointTrajectoryPoint(positions=pos, time_from_start=times) + for (times, pos) in test_trajectory + ], + joint_names=[tf_prefix + ROBOT_JOINTS[i] for i in range(len(ROBOT_JOINTS))], + ) + goal_handle = self._passthrough_forward_joint_trajectory.send_goal( + trajectory=trajectory, + ) + self.assertTrue(goal_handle.accepted) + if goal_handle.accepted: + result = self._passthrough_forward_joint_trajectory.get_result( + goal_handle, TIMEOUT_EXECUTE_TRAJECTORY + ) + self.assertEqual(result.error_code, FollowJointTrajectory.Result.SUCCESSFUL) + + self.assertTrue( + self._controller_manager_interface.switch_controller( + strictness=SwitchController.Request.BEST_EFFORT, + activate_controllers=[ + "force_mode_controller", + ], + deactivate_controllers=[ + "scaled_joint_trajectory_controller", + "joint_trajectory_controller", + "passthrough_trajectory_controller", + ], + ).ok + ) + self.run_force_mode(tf_prefix) + + def test_force_mode_controller_with_passthrough_controller(self, tf_prefix): + self.assertTrue( + self._controller_manager_interface.switch_controller( + strictness=SwitchController.Request.BEST_EFFORT, + activate_controllers=[ + "passthrough_trajectory_controller", + ], + deactivate_controllers=[ + "scaled_joint_trajectory_controller", + "joint_trajectory_controller", + ], + ).ok + ) + time.sleep(1) + self.assertTrue( + self._controller_manager_interface.switch_controller( + strictness=SwitchController.Request.BEST_EFFORT, + activate_controllers=[ + "force_mode_controller", + ], + deactivate_controllers=[ + "scaled_joint_trajectory_controller", + "joint_trajectory_controller", + ], + ).ok + ) + self.run_force_mode(tf_prefix) + def test_illegal_force_mode_types(self, tf_prefix): self.assertTrue( self._controller_manager_interface.switch_controller( From 4aae71efd8cc4c735730afad510bbfca2b60b1b5 Mon Sep 17 00:00:00 2001 From: "mergify[bot]" <37929162+mergify[bot]@users.noreply.github.com> Date: Thu, 5 Jun 2025 21:21:07 +0200 Subject: [PATCH 074/133] Use std_atomic in SJTC (backport of #1385) (#1386) The upstream API changed, so we have to follow. --- .../scaled_joint_trajectory_controller.cpp | 19 +++++++++---------- 1 file changed, 9 insertions(+), 10 deletions(-) diff --git a/ur_controllers/src/scaled_joint_trajectory_controller.cpp b/ur_controllers/src/scaled_joint_trajectory_controller.cpp index 9a97ab38a..d0dfa1446 100644 --- a/ur_controllers/src/scaled_joint_trajectory_controller.cpp +++ b/ur_controllers/src/scaled_joint_trajectory_controller.cpp @@ -137,7 +137,7 @@ controller_interface::return_type ScaledJointTrajectoryController::update(const auto current_external_msg = traj_external_point_ptr_->get_trajectory_msg(); auto new_external_msg = traj_msg_external_point_ptr_.readFromRT(); // Discard, if a goal is pending but still not active (somewhere stuck in goal_handle_timer_) - if (current_external_msg != *new_external_msg && (*(rt_has_pending_goal_.readFromRT()) && !active_goal) == false) { + if (current_external_msg != *new_external_msg && (rt_has_pending_goal_ && !active_goal) == false) { fill_partial_goal(*new_external_msg); sort_to_local_joint_order(*new_external_msg); // TODO(denis): Add here integration of position and velocity @@ -204,8 +204,7 @@ controller_interface::return_type ScaledJointTrajectoryController::update(const // have we reached the end, are not holding position, and is a timeout configured? // Check independently of other tolerances - if (!before_last_point && *(rt_is_holding_.readFromRT()) == false && cmd_timeout_ > 0.0 && - time_difference > cmd_timeout_) { + if (!before_last_point && !rt_is_holding_ && cmd_timeout_ > 0.0 && time_difference > cmd_timeout_) { RCLCPP_WARN(logger, "Aborted due to command timeout"); traj_msg_external_point_ptr_.reset(); @@ -219,13 +218,13 @@ controller_interface::return_type ScaledJointTrajectoryController::update(const // Always check the state tolerance on the first sample in case the first sample // is the last point // print output per default, goal will be aborted afterwards - if ((before_last_point || first_sample) && *(rt_is_holding_.readFromRT()) == false && + if ((before_last_point || first_sample) && !rt_is_holding_ && !check_state_tolerance_per_joint(state_error_, index, active_tol->state_tolerance[index], true /* show_errors */)) { tolerance_violated_while_moving = true; } // past the final point, check that we end up inside goal tolerance - if (!before_last_point && *(rt_is_holding_.readFromRT()) == false && + if (!before_last_point && !rt_is_holding_ && !check_state_tolerance_per_joint(state_error_, index, active_tol->goal_state_tolerance[index], false /* show_errors */)) { outside_goal_tolerance = true; @@ -295,7 +294,7 @@ controller_interface::return_type ScaledJointTrajectoryController::update(const // TODO(matthew-reynolds): Need a lock-free write here // See https://github.com/ros-controls/ros2_controllers/issues/168 rt_active_goal_.writeFromNonRT(RealtimeGoalHandlePtr()); - rt_has_pending_goal_.writeFromNonRT(false); + rt_has_pending_goal_ = false; RCLCPP_WARN(logger, "Aborted due to state tolerance violation"); @@ -311,7 +310,7 @@ controller_interface::return_type ScaledJointTrajectoryController::update(const // TODO(matthew-reynolds): Need a lock-free write here // See https://github.com/ros-controls/ros2_controllers/issues/168 rt_active_goal_.writeFromNonRT(RealtimeGoalHandlePtr()); - rt_has_pending_goal_.writeFromNonRT(false); + rt_has_pending_goal_ = false; RCLCPP_INFO(logger, "Goal reached, success!"); @@ -328,7 +327,7 @@ controller_interface::return_type ScaledJointTrajectoryController::update(const // TODO(matthew-reynolds): Need a lock-free write here // See https://github.com/ros-controls/ros2_controllers/issues/168 rt_active_goal_.writeFromNonRT(RealtimeGoalHandlePtr()); - rt_has_pending_goal_.writeFromNonRT(false); + rt_has_pending_goal_ = false; RCLCPP_WARN(logger, error_string.c_str()); @@ -336,13 +335,13 @@ controller_interface::return_type ScaledJointTrajectoryController::update(const traj_msg_external_point_ptr_.initRT(set_hold_position()); } } - } else if (tolerance_violated_while_moving && *(rt_has_pending_goal_.readFromRT()) == false) { + } else if (tolerance_violated_while_moving && !rt_has_pending_goal_) { // we need to ensure that there is no pending goal -> we get a race condition otherwise RCLCPP_ERROR(logger, "Holding position due to state tolerance violation"); traj_msg_external_point_ptr_.reset(); traj_msg_external_point_ptr_.initRT(set_hold_position()); - } else if (!before_last_point && !within_goal_time && *(rt_has_pending_goal_.readFromRT()) == false) { + } else if (!before_last_point && !within_goal_time && !rt_has_pending_goal_) { RCLCPP_ERROR(logger, "Exceeded goal_time_tolerance: holding position..."); traj_msg_external_point_ptr_.reset(); From e9b0017185849adefc2ef43ee71929437d0fcda9 Mon Sep 17 00:00:00 2001 From: Felix Exner Date: Wed, 11 Jun 2025 08:30:34 +0200 Subject: [PATCH 075/133] Update changelogs for upcoming release --- ur/CHANGELOG.rst | 3 +++ ur_bringup/CHANGELOG.rst | 3 +++ ur_calibration/CHANGELOG.rst | 5 +++++ ur_controllers/CHANGELOG.rst | 6 ++++++ ur_dashboard_msgs/CHANGELOG.rst | 3 +++ ur_moveit_config/CHANGELOG.rst | 3 +++ ur_robot_driver/CHANGELOG.rst | 6 ++++++ 7 files changed, 29 insertions(+) diff --git a/ur/CHANGELOG.rst b/ur/CHANGELOG.rst index 61737304a..fa8f456d9 100644 --- a/ur/CHANGELOG.rst +++ b/ur/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package ur ^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 2.8.0 (2025-05-20) ------------------ diff --git a/ur_bringup/CHANGELOG.rst b/ur_bringup/CHANGELOG.rst index d1819e98a..36eb8629d 100644 --- a/ur_bringup/CHANGELOG.rst +++ b/ur_bringup/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package ur_bringup ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 2.8.0 (2025-05-20) ------------------ * Add support for launching a UR15 robot (`#1359 `_) diff --git a/ur_calibration/CHANGELOG.rst b/ur_calibration/CHANGELOG.rst index 1ae1a03ae..22efdb817 100644 --- a/ur_calibration/CHANGELOG.rst +++ b/ur_calibration/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package ur_calibration ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Update feature list (backport of `#1372 `_) (`#1374 `_) +* Contributors: mergify[bot] + 2.8.0 (2025-05-20) ------------------ diff --git a/ur_controllers/CHANGELOG.rst b/ur_controllers/CHANGELOG.rst index 2cf21f08c..6632e5023 100644 --- a/ur_controllers/CHANGELOG.rst +++ b/ur_controllers/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package ur_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Use std_atomic in SJTC (backport of `#1385 `_) (`#1386 `_) +* [force mode controller] Fix the task frame orientation (backport `#1379 `_) (`#1380 `_) +* Contributors: mergify[bot] + 2.8.0 (2025-05-20) ------------------ * Added controller to enable and disable tool contact (backport of `#940 `_) (`#1337 `_) diff --git a/ur_dashboard_msgs/CHANGELOG.rst b/ur_dashboard_msgs/CHANGELOG.rst index 709fe3fe3..f10c0b856 100644 --- a/ur_dashboard_msgs/CHANGELOG.rst +++ b/ur_dashboard_msgs/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package ur_dashboard_msgs ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 2.8.0 (2025-05-20) ------------------ diff --git a/ur_moveit_config/CHANGELOG.rst b/ur_moveit_config/CHANGELOG.rst index 56522e4fd..aa910d7ab 100644 --- a/ur_moveit_config/CHANGELOG.rst +++ b/ur_moveit_config/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package ur_moveit_config ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 2.8.0 (2025-05-20) ------------------ * Add support for launching a UR15 robot (`#1359 `_) diff --git a/ur_robot_driver/CHANGELOG.rst b/ur_robot_driver/CHANGELOG.rst index bd1b9d604..da8b2f0af 100644 --- a/ur_robot_driver/CHANGELOG.rst +++ b/ur_robot_driver/CHANGELOG.rst @@ -1,3 +1,9 @@ +Forthcoming +----------- +* [force mode controller] Fix the task frame orientation (backport `#1379 `_) (`#1380 `_) +* Update feature list (backport of `#1372 `_) (`#1374 `_) +* Contributors: mergify[bot] + 2.8.0 (2025-05-20) ------------------ * [CI] Check links using lychee instead of a custom script (backport `#1355 `_) (`#1361 `_) From 6f414083d2725250a9d3c10dacd4663cefa94b34 Mon Sep 17 00:00:00 2001 From: Felix Exner Date: Wed, 11 Jun 2025 08:30:59 +0200 Subject: [PATCH 076/133] 2.8.1 --- ur/CHANGELOG.rst | 4 ++-- ur/package.xml | 2 +- ur_bringup/CHANGELOG.rst | 4 ++-- ur_bringup/package.xml | 2 +- ur_calibration/CHANGELOG.rst | 4 ++-- ur_calibration/package.xml | 2 +- ur_controllers/CHANGELOG.rst | 4 ++-- ur_controllers/package.xml | 2 +- ur_dashboard_msgs/CHANGELOG.rst | 4 ++-- ur_dashboard_msgs/package.xml | 2 +- ur_moveit_config/CHANGELOG.rst | 4 ++-- ur_moveit_config/package.xml | 2 +- ur_robot_driver/CHANGELOG.rst | 4 ++-- ur_robot_driver/package.xml | 2 +- 14 files changed, 21 insertions(+), 21 deletions(-) diff --git a/ur/CHANGELOG.rst b/ur/CHANGELOG.rst index fa8f456d9..c8949579c 100644 --- a/ur/CHANGELOG.rst +++ b/ur/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package ur ^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.8.1 (2025-06-11) +------------------ 2.8.0 (2025-05-20) ------------------ diff --git a/ur/package.xml b/ur/package.xml index 18b4560be..ee55f8342 100644 --- a/ur/package.xml +++ b/ur/package.xml @@ -2,7 +2,7 @@ ur - 2.8.0 + 2.8.1 Metapackage for universal robots Felix Exner diff --git a/ur_bringup/CHANGELOG.rst b/ur_bringup/CHANGELOG.rst index 36eb8629d..29a107cab 100644 --- a/ur_bringup/CHANGELOG.rst +++ b/ur_bringup/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package ur_bringup ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.8.1 (2025-06-11) +------------------ 2.8.0 (2025-05-20) ------------------ diff --git a/ur_bringup/package.xml b/ur_bringup/package.xml index f47f895d1..0d577b6d1 100644 --- a/ur_bringup/package.xml +++ b/ur_bringup/package.xml @@ -2,7 +2,7 @@ ur_bringup - 2.8.0 + 2.8.1 Launch file and run-time configurations, e.g. controllers. Felix Exner diff --git a/ur_calibration/CHANGELOG.rst b/ur_calibration/CHANGELOG.rst index 22efdb817..dfbca8b3b 100644 --- a/ur_calibration/CHANGELOG.rst +++ b/ur_calibration/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package ur_calibration ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.8.1 (2025-06-11) +------------------ * Update feature list (backport of `#1372 `_) (`#1374 `_) * Contributors: mergify[bot] diff --git a/ur_calibration/package.xml b/ur_calibration/package.xml index b84089c8e..f9215eee6 100644 --- a/ur_calibration/package.xml +++ b/ur_calibration/package.xml @@ -1,7 +1,7 @@ ur_calibration - 2.8.0 + 2.8.1 Package for extracting the factory calibration from a UR robot and change it such that it can be used by ur_description to gain a correct URDF Felix Exner diff --git a/ur_controllers/CHANGELOG.rst b/ur_controllers/CHANGELOG.rst index 6632e5023..aa248f264 100644 --- a/ur_controllers/CHANGELOG.rst +++ b/ur_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package ur_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.8.1 (2025-06-11) +------------------ * Use std_atomic in SJTC (backport of `#1385 `_) (`#1386 `_) * [force mode controller] Fix the task frame orientation (backport `#1379 `_) (`#1380 `_) * Contributors: mergify[bot] diff --git a/ur_controllers/package.xml b/ur_controllers/package.xml index 71d1e5751..2585ddb8b 100644 --- a/ur_controllers/package.xml +++ b/ur_controllers/package.xml @@ -2,7 +2,7 @@ ur_controllers - 2.8.0 + 2.8.1 Provides controllers that use the speed scaling interface of Universal Robots. Felix Exner diff --git a/ur_dashboard_msgs/CHANGELOG.rst b/ur_dashboard_msgs/CHANGELOG.rst index f10c0b856..a6cb1055c 100644 --- a/ur_dashboard_msgs/CHANGELOG.rst +++ b/ur_dashboard_msgs/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package ur_dashboard_msgs ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.8.1 (2025-06-11) +------------------ 2.8.0 (2025-05-20) ------------------ diff --git a/ur_dashboard_msgs/package.xml b/ur_dashboard_msgs/package.xml index 5553c55ec..7db4e6163 100644 --- a/ur_dashboard_msgs/package.xml +++ b/ur_dashboard_msgs/package.xml @@ -2,7 +2,7 @@ ur_dashboard_msgs - 2.8.0 + 2.8.1 Messages around the UR Dashboard server. Felix Exner diff --git a/ur_moveit_config/CHANGELOG.rst b/ur_moveit_config/CHANGELOG.rst index aa910d7ab..3fbaabb36 100644 --- a/ur_moveit_config/CHANGELOG.rst +++ b/ur_moveit_config/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package ur_moveit_config ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.8.1 (2025-06-11) +------------------ 2.8.0 (2025-05-20) ------------------ diff --git a/ur_moveit_config/package.xml b/ur_moveit_config/package.xml index be9143658..c51ee4416 100644 --- a/ur_moveit_config/package.xml +++ b/ur_moveit_config/package.xml @@ -2,7 +2,7 @@ ur_moveit_config - 2.8.0 + 2.8.1 An example package with MoveIt2 configurations for UR robots. diff --git a/ur_robot_driver/CHANGELOG.rst b/ur_robot_driver/CHANGELOG.rst index da8b2f0af..a731f6e9b 100644 --- a/ur_robot_driver/CHANGELOG.rst +++ b/ur_robot_driver/CHANGELOG.rst @@ -1,5 +1,5 @@ -Forthcoming ------------ +2.8.1 (2025-06-11) +------------------ * [force mode controller] Fix the task frame orientation (backport `#1379 `_) (`#1380 `_) * Update feature list (backport of `#1372 `_) (`#1374 `_) * Contributors: mergify[bot] diff --git a/ur_robot_driver/package.xml b/ur_robot_driver/package.xml index 73af22641..c53b8df09 100644 --- a/ur_robot_driver/package.xml +++ b/ur_robot_driver/package.xml @@ -2,7 +2,7 @@ ur_robot_driver - 2.8.0 + 2.8.1 The new driver for Universal Robots UR3, UR5 and UR10 robots with CB3 controllers and the e-series. Felix Exner From 3f51316340914c01f4bce573d908e1313efd2ba6 Mon Sep 17 00:00:00 2001 From: "mergify[bot]" <37929162+mergify[bot]@users.noreply.github.com> Date: Wed, 11 Jun 2025 09:08:34 +0200 Subject: [PATCH 077/133] Use the Python mirror for clang-format in pre-commit config (backport of #1391) (#1397) This will not require users to have a specific clang-format version installed. Also, this will be updated automatically, so we are not stuck with an old clang-format version. If formatting changes come in from a newer clang-format version, we can adapt our config whenever that happens. --- .pre-commit-config.yaml | 9 ++------- 1 file changed, 2 insertions(+), 7 deletions(-) diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml index 7a478aefe..27091dc92 100644 --- a/.pre-commit-config.yaml +++ b/.pre-commit-config.yaml @@ -86,15 +86,10 @@ repos: files: \.(h\+\+|h|hh|hxx|hpp|cuh|c|cc|cpp|cu|c\+\+|cxx|tpp|txx)$ args: ["--linelength=120"] - - repo: local + - repo: https://github.com/pre-commit/mirrors-clang-format + rev: 'v20.1.5' hooks: - id: clang-format - name: clang-format - description: Format files with ClangFormat. - entry: clang-format-14 - language: system - files: \.(c|cc|cxx|cpp|frag|glsl|h|hpp|hxx|ih|ispc|ipp|java|js|m|proto|vert)$ - args: ['-fallback-style=none', '-i'] # Cmake hooks - repo: local From b397c9be3b04a8a76c27eb3a770efba02c2acc3a Mon Sep 17 00:00:00 2001 From: "mergify[bot]" <37929162+mergify[bot]@users.noreply.github.com> Date: Tue, 17 Jun 2025 09:04:54 +0200 Subject: [PATCH 078/133] Auto-update pre-commit hooks (backport of #1404) (#1406) --- .pre-commit-config.yaml | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml index 27091dc92..6cb02d10d 100644 --- a/.pre-commit-config.yaml +++ b/.pre-commit-config.yaml @@ -87,7 +87,7 @@ repos: args: ["--linelength=120"] - repo: https://github.com/pre-commit/mirrors-clang-format - rev: 'v20.1.5' + rev: 'v20.1.6' hooks: - id: clang-format @@ -115,7 +115,7 @@ repos: # Docs - RestructuredText hooks - repo: https://github.com/PyCQA/doc8 - rev: v1.1.2 + rev: v2.0.0 hooks: - id: doc8 args: ['--max-line-length=100', '--ignore=D001'] From 2e19549f3dee552df715ceea7974e6e11275980b Mon Sep 17 00:00:00 2001 From: "mergify[bot]" <37929162+mergify[bot]@users.noreply.github.com> Date: Tue, 17 Jun 2025 09:06:08 +0200 Subject: [PATCH 079/133] Fix ur_calibration compilation on Windows (backport of #1400) (#1408) --- ur_calibration/CMakeLists.txt | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/ur_calibration/CMakeLists.txt b/ur_calibration/CMakeLists.txt index e95014a13..3f3e9a5e3 100644 --- a/ur_calibration/CMakeLists.txt +++ b/ur_calibration/CMakeLists.txt @@ -2,10 +2,10 @@ cmake_minimum_required(VERSION 3.5) project(ur_calibration) if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") - add_compile_options(-Wall -Wextra -Wpedantic) + add_compile_options(-Wall -Wextra -Wpedantic -Wno-unused-parameter) endif() -add_compile_options(-Wno-unused-parameter) +set(CMAKE_WINDOWS_EXPORT_ALL_SYMBOLS ON) if(NOT CMAKE_CONFIGURATION_TYPES AND NOT CMAKE_BUILD_TYPE) message("${PROJECT_NAME}: You did not request a specific build type: selecting 'RelWithDebInfo'.") From daba62664bc00c9be6537c5df4a2364d6d67b0ea Mon Sep 17 00:00:00 2001 From: "mergify[bot]" <37929162+mergify[bot]@users.noreply.github.com> Date: Tue, 17 Jun 2025 10:18:59 +0200 Subject: [PATCH 080/133] ur_controllers: Fix compilation on Windows (backport #1402) (#1412) Co-authored-by: Silvio Traversaro --- ur_controllers/CMakeLists.txt | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/ur_controllers/CMakeLists.txt b/ur_controllers/CMakeLists.txt index 4d172d22b..70076d3d9 100644 --- a/ur_controllers/CMakeLists.txt +++ b/ur_controllers/CMakeLists.txt @@ -4,6 +4,7 @@ project(ur_controllers) if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") add_compile_options(-Wall -Wextra) endif() +set(CMAKE_WINDOWS_EXPORT_ALL_SYMBOLS ON) find_package(ament_cmake REQUIRED) find_package(angles REQUIRED) @@ -119,7 +120,9 @@ ament_target_dependencies(${PROJECT_NAME} ${THIS_PACKAGE_INCLUDE_DEPENDS} ) -target_compile_options(${PROJECT_NAME} PRIVATE -Wpedantic -Werror=return-type) +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + target_compile_options(${PROJECT_NAME} PRIVATE -Wpedantic -Werror=return-type) +endif() # prevent pluginlib from using boost target_compile_definitions(${PROJECT_NAME} PUBLIC "PLUGINLIB__DISABLE_BOOST_FUNCTIONS") From 817e5655ca04df5422c53437e39d5a35ce424933 Mon Sep 17 00:00:00 2001 From: "mergify[bot]" <37929162+mergify[bot]@users.noreply.github.com> Date: Mon, 23 Jun 2025 09:44:56 +0200 Subject: [PATCH 081/133] Auto-update pre-commit hooks (#1424) --- .pre-commit-config.yaml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml index 6cb02d10d..7031fb191 100644 --- a/.pre-commit-config.yaml +++ b/.pre-commit-config.yaml @@ -51,7 +51,7 @@ repos: args: ["--ignore=D100,D101,D102,D103,D104,D105,D106,D107,D203,D212,D401,D404"] - repo: https://github.com/pycqa/flake8 - rev: 7.2.0 + rev: 7.3.0 hooks: - id: flake8 args: ["--ignore=E501,W503"] From 9ab47a11f852cdb096a160a9c855e7cb78d50ad5 Mon Sep 17 00:00:00 2001 From: "mergify[bot]" <37929162+mergify[bot]@users.noreply.github.com> Date: Mon, 23 Jun 2025 15:01:29 +0200 Subject: [PATCH 082/133] Refactor prepare_switch method (backport #1417) (#1427) Refactored prepare_switch mechanism. This way it should be easier to understand and to extend. --- .../ur_robot_driver/hardware_interface.hpp | 6 +- ur_robot_driver/src/hardware_interface.cpp | 294 ++++++------------ 2 files changed, 98 insertions(+), 202 deletions(-) diff --git a/ur_robot_driver/include/ur_robot_driver/hardware_interface.hpp b/ur_robot_driver/include/ur_robot_driver/hardware_interface.hpp index 3ff25ebeb..e9ea08129 100644 --- a/ur_robot_driver/include/ur_robot_driver/hardware_interface.hpp +++ b/ur_robot_driver/include/ur_robot_driver/hardware_interface.hpp @@ -40,10 +40,11 @@ #define UR_ROBOT_DRIVER__HARDWARE_INTERFACE_HPP_ // System +#include #include #include +#include #include -#include // ros2_control hardware_interface #include "hardware_interface/hardware_info.hpp" @@ -114,6 +115,7 @@ class URPositionHardwareInterface : public hardware_interface::SystemInterface { public: RCLCPP_SHARED_PTR_DEFINITIONS(URPositionHardwareInterface); + URPositionHardwareInterface(); virtual ~URPositionHardwareInterface(); hardware_interface::CallbackReturn on_init(const hardware_interface::HardwareInfo& system_info) final; @@ -316,6 +318,8 @@ class URPositionHardwareInterface : public hardware_interface::SystemInterface const std::string FORCE_MODE_GPIO = "force_mode"; const std::string FREEDRIVE_MODE_GPIO = "freedrive_mode"; const std::string TOOL_CONTACT_GPIO = "tool_contact"; + + std::unordered_map> mode_compatibility_; }; } // namespace ur_robot_driver diff --git a/ur_robot_driver/src/hardware_interface.cpp b/ur_robot_driver/src/hardware_interface.cpp index 1719016cc..dcb930304 100644 --- a/ur_robot_driver/src/hardware_interface.cpp +++ b/ur_robot_driver/src/hardware_interface.cpp @@ -57,6 +57,45 @@ namespace rtde = urcl::rtde_interface; namespace ur_robot_driver { +URPositionHardwareInterface::URPositionHardwareInterface() +{ + mode_compatibility_[hardware_interface::HW_IF_POSITION][hardware_interface::HW_IF_VELOCITY] = false; + mode_compatibility_[hardware_interface::HW_IF_POSITION][FORCE_MODE_GPIO] = false; + mode_compatibility_[hardware_interface::HW_IF_POSITION][PASSTHROUGH_GPIO] = false; + mode_compatibility_[hardware_interface::HW_IF_POSITION][FREEDRIVE_MODE_GPIO] = false; + mode_compatibility_[hardware_interface::HW_IF_POSITION][TOOL_CONTACT_GPIO] = true; + + mode_compatibility_[hardware_interface::HW_IF_VELOCITY][hardware_interface::HW_IF_POSITION] = false; + mode_compatibility_[hardware_interface::HW_IF_VELOCITY][FORCE_MODE_GPIO] = false; + mode_compatibility_[hardware_interface::HW_IF_VELOCITY][PASSTHROUGH_GPIO] = false; + mode_compatibility_[hardware_interface::HW_IF_VELOCITY][FREEDRIVE_MODE_GPIO] = false; + mode_compatibility_[hardware_interface::HW_IF_VELOCITY][TOOL_CONTACT_GPIO] = true; + + mode_compatibility_[FORCE_MODE_GPIO][hardware_interface::HW_IF_POSITION] = false; + mode_compatibility_[FORCE_MODE_GPIO][hardware_interface::HW_IF_VELOCITY] = false; + mode_compatibility_[FORCE_MODE_GPIO][PASSTHROUGH_GPIO] = true; + mode_compatibility_[FORCE_MODE_GPIO][FREEDRIVE_MODE_GPIO] = false; + mode_compatibility_[FORCE_MODE_GPIO][TOOL_CONTACT_GPIO] = false; + + mode_compatibility_[PASSTHROUGH_GPIO][hardware_interface::HW_IF_POSITION] = false; + mode_compatibility_[PASSTHROUGH_GPIO][hardware_interface::HW_IF_VELOCITY] = false; + mode_compatibility_[PASSTHROUGH_GPIO][FORCE_MODE_GPIO] = true; + mode_compatibility_[PASSTHROUGH_GPIO][FREEDRIVE_MODE_GPIO] = false; + mode_compatibility_[PASSTHROUGH_GPIO][TOOL_CONTACT_GPIO] = true; + + mode_compatibility_[FREEDRIVE_MODE_GPIO][hardware_interface::HW_IF_POSITION] = false; + mode_compatibility_[FREEDRIVE_MODE_GPIO][hardware_interface::HW_IF_VELOCITY] = false; + mode_compatibility_[FREEDRIVE_MODE_GPIO][FORCE_MODE_GPIO] = false; + mode_compatibility_[FREEDRIVE_MODE_GPIO][PASSTHROUGH_GPIO] = false; + mode_compatibility_[FREEDRIVE_MODE_GPIO][TOOL_CONTACT_GPIO] = false; + + mode_compatibility_[TOOL_CONTACT_GPIO][hardware_interface::HW_IF_POSITION] = true; + mode_compatibility_[TOOL_CONTACT_GPIO][hardware_interface::HW_IF_VELOCITY] = true; + mode_compatibility_[TOOL_CONTACT_GPIO][FORCE_MODE_GPIO] = false; + mode_compatibility_[TOOL_CONTACT_GPIO][PASSTHROUGH_GPIO] = true; + mode_compatibility_[TOOL_CONTACT_GPIO][FREEDRIVE_MODE_GPIO] = false; +} + URPositionHardwareInterface::~URPositionHardwareInterface() { } @@ -1094,73 +1133,42 @@ hardware_interface::return_type URPositionHardwareInterface::prepare_command_mod } } + auto is_mode_compatible = [this](const std::string& mode, const std::vector& other_modes) { + for (auto& other : other_modes) { + if (mode == other) + continue; + + if (mode_compatibility_[mode][other] == false) { + RCLCPP_ERROR(rclcpp::get_logger("URPositionHardwareInterface"), "Starting %s together with %s is not allowed. ", + mode.c_str(), other.c_str()); + return false; + } + } + return true; + }; + // Starting interfaces // If a joint has been reserved already, raise an error. // Modes that are not directly mapped to a single joint such as force_mode reserve all joints. for (const auto& key : start_interfaces) { for (auto i = 0u; i < info_.joints.size(); i++) { - if (key == info_.joints[i].name + "/" + hardware_interface::HW_IF_POSITION) { - if (std::any_of(start_modes_[i].begin(), start_modes_[i].end(), [&](const std::string& item) { - return item == hardware_interface::HW_IF_VELOCITY || item == PASSTHROUGH_GPIO || - item == FORCE_MODE_GPIO || item == FREEDRIVE_MODE_GPIO; - })) { - RCLCPP_ERROR(rclcpp::get_logger("URPositionHardwareInterface"), "Attempting to start position control while " - "there is another control mode already " - "requested."); - return hardware_interface::return_type::ERROR; - } - start_modes_[i].push_back(hardware_interface::HW_IF_POSITION); - } else if (key == info_.joints[i].name + "/" + hardware_interface::HW_IF_VELOCITY) { - if (std::any_of(start_modes_[i].begin(), start_modes_[i].end(), [&](const std::string& item) { - return item == hardware_interface::HW_IF_POSITION || item == PASSTHROUGH_GPIO || - item == FORCE_MODE_GPIO || item == FREEDRIVE_MODE_GPIO; - })) { - RCLCPP_ERROR(rclcpp::get_logger("URPositionHardwareInterface"), "Attempting to start velocity control while " - "there is another control mode already " - "requested."); - return hardware_interface::return_type::ERROR; - } - start_modes_[i].push_back(hardware_interface::HW_IF_VELOCITY); - } else if (key == tf_prefix + FORCE_MODE_GPIO + "/type") { - if (std::any_of(start_modes_[i].begin(), start_modes_[i].end(), [&](const std::string& item) { - return item == hardware_interface::HW_IF_POSITION || item == hardware_interface::HW_IF_VELOCITY; - })) { - RCLCPP_ERROR(rclcpp::get_logger("URPositionHardwareInterface"), "Attempting to start force_mode control " - "while there is either position or " - "velocity mode already requested by another " - "controller."); - return hardware_interface::return_type::ERROR; - } - start_modes_[i].push_back(FORCE_MODE_GPIO); - } else if (key == tf_prefix + PASSTHROUGH_GPIO + "/setpoint_positions_" + std::to_string(i)) { - if (std::any_of(start_modes_[i].begin(), start_modes_[i].end(), [&](const std::string& item) { - return item == hardware_interface::HW_IF_POSITION || item == hardware_interface::HW_IF_VELOCITY; - })) { - RCLCPP_ERROR(rclcpp::get_logger("URPositionHardwareInterface"), "Attempting to start trajectory passthrough " - "control while there is either " - "position or velocity mode already requested " - "by another controller."); - return hardware_interface::return_type::ERROR; - } - start_modes_[i].push_back(PASSTHROUGH_GPIO); - } else if (key == tf_prefix + FREEDRIVE_MODE_GPIO + "/async_success") { - if (std::any_of(start_modes_[i].begin(), start_modes_[i].end(), [&](const std::string& item) { - return item == hardware_interface::HW_IF_POSITION || item == hardware_interface::HW_IF_VELOCITY || - item == PASSTHROUGH_GPIO || item == FORCE_MODE_GPIO; - })) { - return hardware_interface::return_type::ERROR; - } - start_modes_[i].push_back(FREEDRIVE_MODE_GPIO); - } else if (key == tf_prefix + TOOL_CONTACT_GPIO + "/tool_contact_set_state") { - if (std::any_of(start_modes_[i].begin(), start_modes_[i].end(), [&](const std::string& item) { - return item == FORCE_MODE_GPIO || item == FREEDRIVE_MODE_GPIO; - })) { - RCLCPP_ERROR(rclcpp::get_logger("URPositionHardwareInterface"), "Attempting to start tool contact controller " - "while either the force mode or " - "freedrive controller is running."); - return hardware_interface::return_type::ERROR; + const std::vector> start_modes_to_check{ + { info_.joints[i].name + "/" + hardware_interface::HW_IF_POSITION, hardware_interface::HW_IF_POSITION }, + { info_.joints[i].name + "/" + hardware_interface::HW_IF_VELOCITY, hardware_interface::HW_IF_VELOCITY }, + { tf_prefix + FORCE_MODE_GPIO + "/type", FORCE_MODE_GPIO }, + { tf_prefix + PASSTHROUGH_GPIO + "/setpoint_positions_" + std::to_string(i), PASSTHROUGH_GPIO }, + { tf_prefix + FREEDRIVE_MODE_GPIO + "/async_success", FREEDRIVE_MODE_GPIO }, + { tf_prefix + TOOL_CONTACT_GPIO + "/tool_contact_set_state", TOOL_CONTACT_GPIO } + }; + + for (auto& item : start_modes_to_check) { + if (key == item.first) { + if (!is_mode_compatible(item.second, start_modes_[i])) { + return hardware_interface::return_type::ERROR; + } + start_modes_[i].push_back(item.second); + continue; } - start_modes_[i].push_back(TOOL_CONTACT_GPIO); } } } @@ -1175,151 +1183,35 @@ hardware_interface::return_type URPositionHardwareInterface::prepare_command_mod // add stop interface per joint in tmp var for later check for (const auto& key : stop_interfaces) { for (auto i = 0u; i < info_.joints.size(); i++) { - if (key == info_.joints[i].name + "/" + hardware_interface::HW_IF_POSITION) { - stop_modes_[i].push_back(StoppingInterface::STOP_POSITION); - control_modes[i].erase( - std::remove_if(control_modes[i].begin(), control_modes[i].end(), - [](const std::string& item) { return item == hardware_interface::HW_IF_POSITION; }), - control_modes[i].end()); - } - if (key == info_.joints[i].name + "/" + hardware_interface::HW_IF_VELOCITY) { - stop_modes_[i].push_back(StoppingInterface::STOP_VELOCITY); - control_modes[i].erase( - std::remove_if(control_modes[i].begin(), control_modes[i].end(), - [](const std::string& item) { return item == hardware_interface::HW_IF_VELOCITY; }), - control_modes[i].end()); - } - if (key == tf_prefix + FORCE_MODE_GPIO + "/disable_cmd") { - stop_modes_[i].push_back(StoppingInterface::STOP_FORCE_MODE); - control_modes[i].erase(std::remove_if(control_modes[i].begin(), control_modes[i].end(), - [&](const std::string& item) { return item == FORCE_MODE_GPIO; }), - control_modes[i].end()); - } - if (key == tf_prefix + PASSTHROUGH_GPIO + "/setpoint_positions_" + std::to_string(i)) { - stop_modes_[i].push_back(StoppingInterface::STOP_PASSTHROUGH); - control_modes[i].erase(std::remove_if(control_modes[i].begin(), control_modes[i].end(), - [&](const std::string& item) { return item == PASSTHROUGH_GPIO; }), - control_modes[i].end()); - } - if (key == tf_prefix + FREEDRIVE_MODE_GPIO + "/async_success") { - stop_modes_[i].push_back(StoppingInterface::STOP_FREEDRIVE); - control_modes[i].erase(std::remove_if(control_modes[i].begin(), control_modes[i].end(), - [&](const std::string& item) { return item == FREEDRIVE_MODE_GPIO; }), - control_modes[i].end()); - } - if (key == tf_prefix + TOOL_CONTACT_GPIO + "/tool_contact_set_state") { - stop_modes_[i].push_back(StoppingInterface::STOP_TOOL_CONTACT); - control_modes[i].erase(std::remove_if(control_modes[i].begin(), control_modes[i].end(), - [&](const std::string& item) { return item == TOOL_CONTACT_GPIO; }), - control_modes[i].end()); + const std::vector> stop_modes_to_check{ + { info_.joints[i].name + "/" + hardware_interface::HW_IF_POSITION, hardware_interface::HW_IF_POSITION, + StoppingInterface::STOP_POSITION }, + { info_.joints[i].name + "/" + hardware_interface::HW_IF_VELOCITY, hardware_interface::HW_IF_VELOCITY, + StoppingInterface::STOP_VELOCITY }, + { tf_prefix + FORCE_MODE_GPIO + "/disable_cmd", FORCE_MODE_GPIO, StoppingInterface::STOP_FORCE_MODE }, + { tf_prefix + PASSTHROUGH_GPIO + "/setpoint_positions_" + std::to_string(i), PASSTHROUGH_GPIO, + StoppingInterface::STOP_PASSTHROUGH }, + { tf_prefix + FREEDRIVE_MODE_GPIO + "/async_success", FREEDRIVE_MODE_GPIO, StoppingInterface::STOP_FREEDRIVE }, + { tf_prefix + TOOL_CONTACT_GPIO + "/tool_contact_set_state", TOOL_CONTACT_GPIO, + StoppingInterface::STOP_TOOL_CONTACT } + }; + for (auto& item : stop_modes_to_check) { + if (key == std::get<0>(item)) { + stop_modes_[i].push_back(std::get<2>(item)); + control_modes[i].erase( + std::remove_if(control_modes[i].begin(), control_modes[i].end(), + [&item](const std::string& entry) { return entry == std::get<1>(item); }), + control_modes[i].end()); + } } } } // Do not start conflicting controllers - // Passthrough controller requested to start - if (std::any_of(start_modes_[0].begin(), start_modes_[0].end(), - [this](auto& item) { return (item == PASSTHROUGH_GPIO); }) && - (std::any_of(start_modes_[0].begin(), start_modes_[0].end(), - [this](auto& item) { - return (item == hardware_interface::HW_IF_VELOCITY || item == hardware_interface::HW_IF_POSITION || - item == FREEDRIVE_MODE_GPIO); - }) || - std::any_of(control_modes[0].begin(), control_modes[0].end(), [this](auto& item) { - return (item == hardware_interface::HW_IF_VELOCITY || item == hardware_interface::HW_IF_POSITION || - item == FREEDRIVE_MODE_GPIO); - }))) { - RCLCPP_ERROR(rclcpp::get_logger("URPositionHardwareInterface"), "Attempting to start passthrough_trajectory " - "control while there is either position or " - "velocity or freedrive mode running."); - ret_val = hardware_interface::return_type::ERROR; - } - - // Force mode requested to start - if (std::any_of(start_modes_[0].begin(), start_modes_[0].end(), - [this](auto& item) { return (item == FORCE_MODE_GPIO); }) && - (std::any_of(start_modes_[0].begin(), start_modes_[0].end(), - [this](auto& item) { - return (item == hardware_interface::HW_IF_VELOCITY || item == hardware_interface::HW_IF_POSITION || - item == FREEDRIVE_MODE_GPIO || item == TOOL_CONTACT_GPIO); - }) || - std::any_of(control_modes[0].begin(), control_modes[0].end(), [this](auto& item) { - return (item == hardware_interface::HW_IF_VELOCITY || item == hardware_interface::HW_IF_POSITION || - item == FORCE_MODE_GPIO || item == FREEDRIVE_MODE_GPIO || item == TOOL_CONTACT_GPIO); - }))) { - RCLCPP_ERROR(rclcpp::get_logger("URPositionHardwareInterface"), "Attempting to start force mode control while " - "there is either position or " - "velocity mode running."); - ret_val = hardware_interface::return_type::ERROR; - } - - // Freedrive mode requested to start - if (std::any_of(start_modes_[0].begin(), start_modes_[0].end(), - [this](auto& item) { return (item == FREEDRIVE_MODE_GPIO); }) && - (std::any_of(start_modes_[0].begin(), start_modes_[0].end(), - [this](auto& item) { - return (item == hardware_interface::HW_IF_VELOCITY || item == hardware_interface::HW_IF_POSITION || - item == PASSTHROUGH_GPIO || item == FORCE_MODE_GPIO || item == TOOL_CONTACT_GPIO); - }) || - std::any_of(control_modes[0].begin(), control_modes[0].end(), [this](auto& item) { - return (item == hardware_interface::HW_IF_VELOCITY || item == hardware_interface::HW_IF_POSITION || - item == PASSTHROUGH_GPIO || item == FORCE_MODE_GPIO || item == TOOL_CONTACT_GPIO); - }))) { - RCLCPP_ERROR(rclcpp::get_logger("URPositionHardwareInterface"), "Attempting to start force mode control while " - "there is either position or " - "velocity mode running."); - ret_val = hardware_interface::return_type::ERROR; - } - - // Tool contact controller requested to start - if (std::any_of(start_modes_[0].begin(), start_modes_[0].end(), - [this](auto& item) { return (item == TOOL_CONTACT_GPIO); }) && - (std::any_of(start_modes_[0].begin(), start_modes_[0].end(), - [this](auto& item) { return (item == FORCE_MODE_GPIO || item == FREEDRIVE_MODE_GPIO); }) || - std::any_of(control_modes[0].begin(), control_modes[0].end(), - [this](auto& item) { return (item == FORCE_MODE_GPIO || item == FREEDRIVE_MODE_GPIO); }))) { - RCLCPP_ERROR(rclcpp::get_logger("URPositionHardwareInterface"), "Attempting to start tool contact controller while " - "either the force mode controller or " - "the freedrive controller is running."); - ret_val = hardware_interface::return_type::ERROR; - } - - // Position mode requested to start - if (std::any_of(start_modes_[0].begin(), start_modes_[0].end(), - [](auto& item) { return (item == hardware_interface::HW_IF_POSITION); }) && - (std::any_of(start_modes_[0].begin(), start_modes_[0].end(), - [this](auto& item) { - return (item == hardware_interface::HW_IF_VELOCITY || item == PASSTHROUGH_GPIO || - item == FORCE_MODE_GPIO || item == FREEDRIVE_MODE_GPIO); - }) || - std::any_of(control_modes[0].begin(), control_modes[0].end(), [this](auto& item) { - return (item == hardware_interface::HW_IF_VELOCITY || item == hardware_interface::HW_IF_POSITION || - item == PASSTHROUGH_GPIO || item == FORCE_MODE_GPIO || item == FREEDRIVE_MODE_GPIO); - }))) { - RCLCPP_ERROR(rclcpp::get_logger("URPositionHardwareInterface"), "Attempting to start position control while there " - "is either trajectory passthrough or " - "velocity mode or force_mode or freedrive mode " - "running."); - ret_val = hardware_interface::return_type::ERROR; - } - - // Velocity mode requested to start - if (std::any_of(start_modes_[0].begin(), start_modes_[0].end(), - [](auto& item) { return (item == hardware_interface::HW_IF_VELOCITY); }) && - (std::any_of(start_modes_[0].begin(), start_modes_[0].end(), - [this](auto& item) { - return (item == hardware_interface::HW_IF_POSITION || item == PASSTHROUGH_GPIO || - item == FORCE_MODE_GPIO || item == FREEDRIVE_MODE_GPIO); - }) || - std::any_of(control_modes[0].begin(), control_modes[0].end(), [this](auto& item) { - return (item == hardware_interface::HW_IF_VELOCITY || item == hardware_interface::HW_IF_POSITION || - item == PASSTHROUGH_GPIO || item == FORCE_MODE_GPIO || item == FREEDRIVE_MODE_GPIO); - }))) { - RCLCPP_ERROR(rclcpp::get_logger("URPositionHardwareInterface"), "Attempting to start velocity control while there " - "is either trajectory passthrough or " - "position mode or force_mode or freedrive mode " - "running."); - ret_val = hardware_interface::return_type::ERROR; + for (auto& start_mode : start_modes_[0]) { + if (!is_mode_compatible(start_mode, control_modes[0])) { + return hardware_interface::return_type::ERROR; + } } controllers_initialized_ = true; From 9cbdacd6e9d4ad145713247c232af2740e93f610 Mon Sep 17 00:00:00 2001 From: "mergify[bot]" <37929162+mergify[bot]@users.noreply.github.com> Date: Wed, 25 Jun 2025 09:09:17 +0200 Subject: [PATCH 083/133] ur_robot_driver: Fix compilation on Windows (backport of #1421) (#1431) Co-authored-by: Silvio Traversaro --- ur_robot_driver/CMakeLists.txt | 9 ++++++--- .../include/ur_robot_driver/hardware_interface.hpp | 2 +- ur_robot_driver/src/hardware_interface.cpp | 4 ++-- ur_robot_driver/src/robot_state_helper.cpp | 2 +- 4 files changed, 10 insertions(+), 7 deletions(-) diff --git a/ur_robot_driver/CMakeLists.txt b/ur_robot_driver/CMakeLists.txt index adb8f7119..43a653cc9 100644 --- a/ur_robot_driver/CMakeLists.txt +++ b/ur_robot_driver/CMakeLists.txt @@ -9,9 +9,12 @@ option( OFF ) -add_compile_options(-Wall) -add_compile_options(-Wextra) -add_compile_options(-Wno-unused-parameter) +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall) + add_compile_options(-Wextra) + add_compile_options(-Wno-unused-parameter) +endif() +set(CMAKE_WINDOWS_EXPORT_ALL_SYMBOLS ON) if(NOT CMAKE_CONFIGURATION_TYPES AND NOT CMAKE_BUILD_TYPE) message("${PROJECT_NAME}: You did not request a specific build type: selecting 'RelWithDebInfo'.") diff --git a/ur_robot_driver/include/ur_robot_driver/hardware_interface.hpp b/ur_robot_driver/include/ur_robot_driver/hardware_interface.hpp index e9ea08129..e6cc00c86 100644 --- a/ur_robot_driver/include/ur_robot_driver/hardware_interface.hpp +++ b/ur_robot_driver/include/ur_robot_driver/hardware_interface.hpp @@ -301,7 +301,7 @@ class URPositionHardwareInterface : public hardware_interface::SystemInterface double pausing_ramp_up_increment_; // resources switching aux vars - std::vector> stop_modes_; + std::vector> stop_modes_; std::vector> start_modes_; bool position_controller_running_; bool velocity_controller_running_; diff --git a/ur_robot_driver/src/hardware_interface.cpp b/ur_robot_driver/src/hardware_interface.cpp index dcb930304..a051148b8 100644 --- a/ur_robot_driver/src/hardware_interface.cpp +++ b/ur_robot_driver/src/hardware_interface.cpp @@ -220,7 +220,7 @@ std::vector URPositionHardwareInterface::exp const std::vector fts_names = { "force.x", "force.y", "force.z", "torque.x", "torque.y", "torque.z" }; - for (uint j = 0; j < 6; ++j) { + for (uint32_t j = 0; j < 6; ++j) { state_interfaces.emplace_back( hardware_interface::StateInterface(sensor.name, fts_names[j], &urcl_ft_sensor_measurements_[j])); } @@ -1107,7 +1107,7 @@ hardware_interface::return_type URPositionHardwareInterface::prepare_command_mod hardware_interface::return_type ret_val = hardware_interface::return_type::OK; start_modes_ = std::vector>(info_.joints.size()); - stop_modes_ = std::vector>(info_.joints.size()); + stop_modes_ = std::vector>(info_.joints.size()); std::vector> control_modes(info_.joints.size()); const std::string tf_prefix = info_.hardware_parameters.at("tf_prefix"); diff --git a/ur_robot_driver/src/robot_state_helper.cpp b/ur_robot_driver/src/robot_state_helper.cpp index 5adefe2a1..4caad6f0e 100644 --- a/ur_robot_driver/src/robot_state_helper.cpp +++ b/ur_robot_driver/src/robot_state_helper.cpp @@ -361,7 +361,7 @@ void RobotStateHelper::setModeExecute(const std::shared_ptrmessage = "Play program service not available on this robot."; } else { // The dashboard denies playing immediately after switching the mode to RUNNING - sleep(1); + std::this_thread::sleep_for(std::chrono::milliseconds(1000)); result_->success = safeDashboardTrigger(this->play_program_srv_); } } From 9dee1bc14878f4bf4f1af412e0e0cbeda6e81e86 Mon Sep 17 00:00:00 2001 From: "mergify[bot]" <37929162+mergify[bot]@users.noreply.github.com> Date: Thu, 26 Jun 2025 12:32:31 +0200 Subject: [PATCH 084/133] Add a release config file for GitHub release note auto-generation (#1434) Github allows customizing the auto-generated changelogs. This is a first attempt to configure that. --- .github/release.yml | 23 +++++++++++++++++++++++ 1 file changed, 23 insertions(+) create mode 100644 .github/release.yml diff --git a/.github/release.yml b/.github/release.yml new file mode 100644 index 000000000..cf114c938 --- /dev/null +++ b/.github/release.yml @@ -0,0 +1,23 @@ +--- +changelog: + exclude: + labels: + - release + categories: + - title: Bugfixes + labels: + - bugfix + - title: New features + labels: + - enhancement + - title: Documentation + labels: + - documentation + - title: CI/ Repo / Packages + labels: + - CI + - package + - dependencies + - title: Other + labels: + - "*" From 61fbe82e5b413495920d98cf0ccb2b2cb06f88a1 Mon Sep 17 00:00:00 2001 From: "mergify[bot]" <37929162+mergify[bot]@users.noreply.github.com> Date: Thu, 26 Jun 2025 20:23:27 +0200 Subject: [PATCH 085/133] Added 'is in remote control' call as a dashboard service (backport of #1433) (#1436) Co-authored-by: Mads Holm Peters <79145214+urmahp@users.noreply.github.com> --- ur_dashboard_msgs/CMakeLists.txt | 1 + ur_dashboard_msgs/srv/IsInRemoteControl.srv | 4 ++++ ur_robot_driver/doc/ROS_INTERFACE.md | 4 ++++ .../ur_robot_driver/dashboard_client_ros.hpp | 5 +++++ ur_robot_driver/src/dashboard_client_ros.cpp | 21 +++++++++++++++++++ 5 files changed, 35 insertions(+) create mode 100644 ur_dashboard_msgs/srv/IsInRemoteControl.srv diff --git a/ur_dashboard_msgs/CMakeLists.txt b/ur_dashboard_msgs/CMakeLists.txt index b04e37fd4..db943ea71 100644 --- a/ur_dashboard_msgs/CMakeLists.txt +++ b/ur_dashboard_msgs/CMakeLists.txt @@ -23,6 +23,7 @@ set(srv_files srv/Load.srv srv/Popup.srv srv/RawRequest.srv + srv/IsInRemoteControl.srv ) set(action_files diff --git a/ur_dashboard_msgs/srv/IsInRemoteControl.srv b/ur_dashboard_msgs/srv/IsInRemoteControl.srv new file mode 100644 index 000000000..622bbd4af --- /dev/null +++ b/ur_dashboard_msgs/srv/IsInRemoteControl.srv @@ -0,0 +1,4 @@ +--- +string answer +bool remote_control # is the robot in remote control? +bool success # Did the dashboard server call succeed? diff --git a/ur_robot_driver/doc/ROS_INTERFACE.md b/ur_robot_driver/doc/ROS_INTERFACE.md index 21bb3d2bc..00c7082a4 100644 --- a/ur_robot_driver/doc/ROS_INTERFACE.md +++ b/ur_robot_driver/doc/ROS_INTERFACE.md @@ -202,6 +202,10 @@ Stop program execution on the robot Dismiss a protective stop to continue robot movements. NOTE: It is the responsibility of the user to ensure the cause of the protective stop is resolved before calling this service. +##### is_in_remote_control [ur_dashboard_msgs/IsInRemoteControl](http://docs.ros.org/en/rolling/p/ur_dashboard_msgs/srv/IsInRemoteControl.html) + +Service to query whether the robot is in remote control + #### Parameters ##### receive_timeout (Required) diff --git a/ur_robot_driver/include/ur_robot_driver/dashboard_client_ros.hpp b/ur_robot_driver/include/ur_robot_driver/dashboard_client_ros.hpp index 6c493246f..7fb4c913f 100644 --- a/ur_robot_driver/include/ur_robot_driver/dashboard_client_ros.hpp +++ b/ur_robot_driver/include/ur_robot_driver/dashboard_client_ros.hpp @@ -62,6 +62,7 @@ #include "ur_dashboard_msgs/srv/load.hpp" #include "ur_dashboard_msgs/srv/popup.hpp" #include "ur_dashboard_msgs/srv/raw_request.hpp" +#include "ur_dashboard_msgs/srv/is_in_remote_control.hpp" namespace ur_robot_driver { @@ -115,6 +116,9 @@ class DashboardClientROS bool handleRobotModeQuery(ur_dashboard_msgs::srv::GetRobotMode::Request::SharedPtr req, ur_dashboard_msgs::srv::GetRobotMode::Response::SharedPtr resp); + bool handleRemoteControlQuery(ur_dashboard_msgs::srv::IsInRemoteControl::Request::SharedPtr req, + ur_dashboard_msgs::srv::IsInRemoteControl::Response::SharedPtr resp); + bool connect(); std::shared_ptr node_; @@ -151,6 +155,7 @@ class DashboardClientROS rclcpp::Service::SharedPtr program_state_service_; rclcpp::Service::SharedPtr safety_mode_service_; rclcpp::Service::SharedPtr robot_mode_service_; + rclcpp::Service::SharedPtr is_in_remote_control_service_; }; } // namespace ur_robot_driver diff --git a/ur_robot_driver/src/dashboard_client_ros.cpp b/ur_robot_driver/src/dashboard_client_ros.cpp index 77068fdc1..3a729c8c0 100644 --- a/ur_robot_driver/src/dashboard_client_ros.cpp +++ b/ur_robot_driver/src/dashboard_client_ros.cpp @@ -269,6 +269,11 @@ DashboardClientROS::DashboardClientROS(const rclcpp::Node::SharedPtr& node, cons } return true; }); + + // Service to query whether the robot is in remote control. + is_in_remote_control_service_ = node_->create_service( + "~/is_in_remote_control", + std::bind(&DashboardClientROS::handleRemoteControlQuery, this, std::placeholders::_1, std::placeholders::_2)); } bool DashboardClientROS::connect() @@ -410,4 +415,20 @@ bool DashboardClientROS::handleRobotModeQuery(const ur_dashboard_msgs::srv::GetR } return true; } + +bool DashboardClientROS::handleRemoteControlQuery( + const ur_dashboard_msgs::srv::IsInRemoteControl::Request::SharedPtr req, + ur_dashboard_msgs::srv::IsInRemoteControl::Response::SharedPtr resp) +{ + try { + resp->remote_control = this->client_.commandIsInRemoteControl(); + resp->success = true; + } catch (const urcl::UrException& e) { + RCLCPP_ERROR(rclcpp::get_logger("Dashboard_Client"), "Service Call failed: '%s'", e.what()); + resp->answer = e.what(); + resp->success = false; + } + return true; +} + } // namespace ur_robot_driver From c768778c70fb4d94b9de274a9b74ca4994fee02f Mon Sep 17 00:00:00 2001 From: "mergify[bot]" <37929162+mergify[bot]@users.noreply.github.com> Date: Tue, 1 Jul 2025 09:56:25 +0200 Subject: [PATCH 086/133] Auto-update pre-commit hooks (backport #1440) (#1441) --- .pre-commit-config.yaml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml index 7031fb191..a5945be29 100644 --- a/.pre-commit-config.yaml +++ b/.pre-commit-config.yaml @@ -87,7 +87,7 @@ repos: args: ["--linelength=120"] - repo: https://github.com/pre-commit/mirrors-clang-format - rev: 'v20.1.6' + rev: 'v20.1.7' hooks: - id: clang-format From 58dcd06a15f9cf089fa497d94e7752269562861e Mon Sep 17 00:00:00 2001 From: "mergify[bot]" <37929162+mergify[bot]@users.noreply.github.com> Date: Wed, 2 Jul 2025 10:45:42 +0200 Subject: [PATCH 087/133] Reduce flakiness of trajectory controller tests (backport of #1443) (#1444) * Set minimal velocity tolerance in passthrough trajectory test When running a quintic spline with the passthrough trajectory test there can be a minimal velocity deviation at the end of the trajectory due to how the robot interpolates motions. With a velocity tolerance of 0.0 there is a chance that we hit this tolerance. * Explicitly switch on controllers in test where they are needed (cherry picked from commit 1cd3ac5b84ced30a996ce6dde456606e57c0c4e9) Co-authored-by: Felix Exner --- ur_robot_driver/test/robot_driver.py | 23 ++++++++++++++++++++++- 1 file changed, 22 insertions(+), 1 deletion(-) diff --git a/ur_robot_driver/test/robot_driver.py b/ur_robot_driver/test/robot_driver.py index 59cc3a6ec..e07462772 100644 --- a/ur_robot_driver/test/robot_driver.py +++ b/ur_robot_driver/test/robot_driver.py @@ -187,6 +187,13 @@ def io_msg_cb(msg): def test_trajectory(self, tf_prefix): """Test robot movement.""" + self.assertTrue( + self._controller_manager_interface.switch_controller( + strictness=SwitchController.Request.BEST_EFFORT, + deactivate_controllers=["passthrough_trajectory_controller"], + activate_controllers=["scaled_joint_trajectory_controller"], + ).ok + ) # Construct test trajectory test_trajectory = [ (Duration(sec=6, nanosec=0), [0.0 for j in ROBOT_JOINTS]), @@ -219,6 +226,13 @@ def test_illegal_trajectory(self, tf_prefix): This is more of a validation test that the testing suite does the right thing """ + self.assertTrue( + self._controller_manager_interface.switch_controller( + strictness=SwitchController.Request.BEST_EFFORT, + deactivate_controllers=["passthrough_trajectory_controller"], + activate_controllers=["scaled_joint_trajectory_controller"], + ).ok + ) # Construct test trajectory, the second point wrongly starts before the first test_trajectory = [ (Duration(sec=6, nanosec=0), [0.0 for j in ROBOT_JOINTS]), @@ -244,6 +258,13 @@ def test_illegal_trajectory(self, tf_prefix): def test_trajectory_scaled(self, tf_prefix): """Test robot movement.""" + self.assertTrue( + self._controller_manager_interface.switch_controller( + strictness=SwitchController.Request.BEST_EFFORT, + deactivate_controllers=["passthrough_trajectory_controller"], + activate_controllers=["scaled_joint_trajectory_controller"], + ).ok + ) # Construct test trajectory test_trajectory = [ (Duration(sec=6, nanosec=0), [0.0 for j in ROBOT_JOINTS]), @@ -369,7 +390,7 @@ def test_passthrough_trajectory(self, tf_prefix): Duration(sec=12, nanosec=0), ] goal_tolerance = [ - JointTolerance(position=0.01, name=tf_prefix + ROBOT_JOINTS[i]) + JointTolerance(position=0.01, velocity=5e-5, name=tf_prefix + ROBOT_JOINTS[i]) for i in range(len(ROBOT_JOINTS)) ] goal_time_tolerance = Duration(sec=1, nanosec=0) From efd2e35e8b89972f91fe16a06ef5e942d74f0f57 Mon Sep 17 00:00:00 2001 From: "mergify[bot]" <37929162+mergify[bot]@users.noreply.github.com> Date: Tue, 8 Jul 2025 10:58:04 +0200 Subject: [PATCH 088/133] fix_flaky_force_mode_test (backport of #1429) (#1448) --- .../test/integration_test_force_mode.py | 41 +++++++++++++------ 1 file changed, 29 insertions(+), 12 deletions(-) diff --git a/ur_robot_driver/test/integration_test_force_mode.py b/ur_robot_driver/test/integration_test_force_mode.py index 8b059b8fe..40ad582a6 100644 --- a/ur_robot_driver/test/integration_test_force_mode.py +++ b/ur_robot_driver/test/integration_test_force_mode.py @@ -120,18 +120,30 @@ def setUp(self): self.assertTrue(self._io_status_controller_interface.resend_robot_program().success) self.tf_buffer = Buffer() self.tf_listener = TransformListener(self.tf_buffer, self.node) - - def lookup_tcp_in_base(self, tf_prefix, timepoint): - trans = None - while not trans: - rclpy.spin_once(self.node) + time.sleep(1) # Wait whether the controller stopper resets controllers + + def wait_for_lookup(self, source, target, timepoint, timeout=5.0): + """ + Wait until the transform between source and target is available. + + :param source: The source frame + :param target: The target frame + :param timeout: The point in time at which to make the lookup + :param timeout: Timeout in seconds + :return: transform between source and target at the given timepoint + :raises TimeoutError: If the transform is not available within the timeout + """ + end_time = time.time() + timeout + while time.time() < end_time: try: - trans = self.tf_buffer.lookup_transform( - tf_prefix + "base", tf_prefix + "tool0", timepoint - ) + trans = self.tf_buffer.lookup_transform(source, target, timepoint) + return trans except TransformException: - pass - return trans + rclpy.spin_once(self.node) + raise TimeoutError() + + def lookup_tcp_in_base(self, tf_prefix, timepoint): + return self.wait_for_lookup(tf_prefix + "base", tf_prefix + "tool0_controller", timepoint) # Implementation of force mode test to be reused # todo: If we move to pytest this could be done using parametrization @@ -201,6 +213,9 @@ def run_force_mode(self, tf_prefix): ), header=trans_after.header, ) + self.wait_for_lookup( + diff.header.frame_id, tf_prefix + "tool0_controller", diff.header.stamp + ) diff_in_tool0_controller = self.tf_buffer.transform( diff, tf_prefix + "tool0_controller", @@ -519,7 +534,6 @@ def test_deactivating_controller_stops_force_mode(self, tf_prefix): ], ).ok ) - self._force_mode_controller_interface = ForceModeInterface(self.node) time.sleep(0.5) trans_before_wait = self.lookup_tcp_in_base(tf_prefix, self.node.get_clock().now()) @@ -529,7 +543,10 @@ def test_deactivating_controller_stops_force_mode(self, tf_prefix): trans_after_wait = self.lookup_tcp_in_base(tf_prefix, self.node.get_clock().now()) self.assertAlmostEqual( - trans_before_wait.transform.translation.z, trans_after_wait.transform.translation.z + trans_before_wait.transform.translation.z, + trans_after_wait.transform.translation.z, + delta=1e-7, + msg="Robot should not move after force mode is stopped", ) def test_params_out_of_range_fails(self, tf_prefix): From a45190f7c23c0ebf468f3237676d64176cccf15b Mon Sep 17 00:00:00 2001 From: "mergify[bot]" <37929162+mergify[bot]@users.noreply.github.com> Date: Tue, 8 Jul 2025 10:58:54 +0200 Subject: [PATCH 089/133] Fix flaky controller switch test (backport of #1447) (#1450) We were starting the tests right after we started the driver. However, the test requires that all controllers are actually available. This commit adds a check whether all required controllers are actually loaded. If not all required controllers are found in a specific time, the test will throw an exception and therefore fail. --- .../integration_test_controller_switch.py | 55 +++++++------------ ur_robot_driver/test/test_common.py | 15 +++-- ur_robot_driver/test/urscript_interface.py | 4 +- 3 files changed, 32 insertions(+), 42 deletions(-) diff --git a/ur_robot_driver/test/integration_test_controller_switch.py b/ur_robot_driver/test/integration_test_controller_switch.py index 3c8a31a26..58e11a1e4 100644 --- a/ur_robot_driver/test/integration_test_controller_switch.py +++ b/ur_robot_driver/test/integration_test_controller_switch.py @@ -48,6 +48,16 @@ generate_driver_test_description, ) +ALL_CONTROLLERS = [ + "scaled_joint_trajectory_controller", + "joint_trajectory_controller", + "forward_position_controller", + "forward_velocity_controller", + "passthrough_trajectory_controller", + "force_mode_controller", + "freedrive_mode_controller", +] + @pytest.mark.launch_test @launch_testing.parametrize( @@ -78,6 +88,8 @@ def init_robot(self): self._dashboard_interface = DashboardInterface(self.node) self._controller_manager_interface = ControllerManagerInterface(self.node) self._io_status_controller_interface = IoStatusInterface(self.node) + for controller in ALL_CONTROLLERS: + self._controller_manager_interface.wait_for_controller(controller) def setUp(self): self._dashboard_interface.start_robot() @@ -89,15 +101,7 @@ def test_activating_multiple_controllers_same_interface_fails(self): self.assertTrue( self._controller_manager_interface.switch_controller( strictness=SwitchController.Request.BEST_EFFORT, - deactivate_controllers=[ - "scaled_joint_trajectory_controller", - "joint_trajectory_controller", - "forward_position_controller", - "forward_velocity_controller", - "passthrough_trajectory_controller", - "force_mode_controller", - "freedrive_mode_controller", - ], + deactivate_controllers=ALL_CONTROLLERS, ).ok ) @@ -126,15 +130,7 @@ def test_activating_multiple_controllers_different_interface_fails(self): self.assertTrue( self._controller_manager_interface.switch_controller( strictness=SwitchController.Request.BEST_EFFORT, - deactivate_controllers=[ - "scaled_joint_trajectory_controller", - "joint_trajectory_controller", - "forward_position_controller", - "forward_velocity_controller", - "force_mode_controller", - "passthrough_trajectory_controller", - "freedrive_mode_controller", - ], + deactivate_controllers=ALL_CONTROLLERS, ).ok ) self.assertFalse( @@ -261,7 +257,9 @@ def test_activating_controller_with_running_position_controller_fails(self): ).ok ) - def test_activating_controller_with_running_passthrough_trajectory_controller_fails(self): + def test_activating_controller_with_running_passthrough_trajectory_controller_fails( + self, + ): # Having a position-based controller active, no other controller should be able to # activate. self.assertTrue( @@ -332,14 +330,7 @@ def test_force_mode_and_trajectory_passthrough_controller_are_compatible(self): self.assertTrue( self._controller_manager_interface.switch_controller( strictness=SwitchController.Request.BEST_EFFORT, - deactivate_controllers=[ - "scaled_joint_trajectory_controller", - "joint_trajectory_controller", - "forward_position_controller", - "forward_velocity_controller", - "passthrough_trajectory_controller", - "force_mode_controller", - ], + deactivate_controllers=ALL_CONTROLLERS, ).ok ) @@ -411,15 +402,7 @@ def test_tool_contact_compatibility(self): self.assertTrue( self._controller_manager_interface.switch_controller( strictness=SwitchController.Request.BEST_EFFORT, - deactivate_controllers=[ - "scaled_joint_trajectory_controller", - "joint_trajectory_controller", - "forward_position_controller", - "forward_velocity_controller", - "passthrough_trajectory_controller", - "force_mode_controller", - "tool_contact_controller", - ], + deactivate_controllers=ALL_CONTROLLERS, ).ok ) diff --git a/ur_robot_driver/test/test_common.py b/ur_robot_driver/test/test_common.py index 9d1d79621..215f8ff55 100644 --- a/ur_robot_driver/test/test_common.py +++ b/ur_robot_driver/test/test_common.py @@ -256,14 +256,19 @@ class ControllerManagerInterface( }, services={"list_controllers": ListControllers}, ): - def wait_for_controller(self, controller_name, target_state="active"): - while True: + def wait_for_controller(self, controller_name, target_state=None, timeout=TIMEOUT_WAIT_SERVICE): + start_time = time.time() + while time.time() - start_time < timeout: controllers = self.list_controllers().controller for controller in controllers: - if (controller.name == controller_name) and (controller.state == target_state): - return - + if controller.name == controller_name: + if (target_state is None) or (controller.state == target_state): + return time.sleep(1) + raise Exception( + "Controller '%s' not found or not in state '%s' within %fs" + % (controller_name, target_state, timeout) + ) class IoStatusInterface( diff --git a/ur_robot_driver/test/urscript_interface.py b/ur_robot_driver/test/urscript_interface.py index 724ad3abe..e7892a397 100755 --- a/ur_robot_driver/test/urscript_interface.py +++ b/ur_robot_driver/test/urscript_interface.py @@ -81,7 +81,9 @@ def setUp(self): time.sleep(1) self.assertTrue(self._io_status_controller_interface.resend_robot_program().success) - self._controller_manager_interface.wait_for_controller("io_and_status_controller") + self._controller_manager_interface.wait_for_controller( + "io_and_status_controller", target_state="active" + ) def test_set_io(self): """Test setting an IO using a direct program call.""" From 8c8e3f7adc0ba8c955485e76bc4c7ab7a852d7a2 Mon Sep 17 00:00:00 2001 From: "mergify[bot]" <37929162+mergify[bot]@users.noreply.github.com> Date: Mon, 14 Jul 2025 10:30:54 +0200 Subject: [PATCH 090/133] Auto-update pre-commit hooks (backport of #1453) (#1455) --- .pre-commit-config.yaml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml index a5945be29..1e80e8805 100644 --- a/.pre-commit-config.yaml +++ b/.pre-commit-config.yaml @@ -87,7 +87,7 @@ repos: args: ["--linelength=120"] - repo: https://github.com/pre-commit/mirrors-clang-format - rev: 'v20.1.7' + rev: 'v20.1.8' hooks: - id: clang-format From f13886cc1a94ab29981b99b77f3e0d1f5154d4a7 Mon Sep 17 00:00:00 2001 From: Felix Exner Date: Thu, 17 Jul 2025 13:29:43 +0200 Subject: [PATCH 091/133] ur_moveit_config: Assure the description is loaded as string (#1452) Otherwise the description might get loaded as yaml under certain situations. --- ur_moveit_config/launch/ur_moveit.launch.py | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/ur_moveit_config/launch/ur_moveit.launch.py b/ur_moveit_config/launch/ur_moveit.launch.py index bb20be7a8..41de1f4cd 100644 --- a/ur_moveit_config/launch/ur_moveit.launch.py +++ b/ur_moveit_config/launch/ur_moveit.launch.py @@ -34,6 +34,7 @@ from launch_ros.actions import Node from launch_ros.substitutions import FindPackageShare from ur_moveit_config.launch_common import load_yaml +from launch_ros.parameter_descriptions import ParameterValue from launch import LaunchDescription from launch.actions import DeclareLaunchArgument, OpaqueFunction @@ -125,7 +126,9 @@ def launch_setup(context, *args, **kwargs): " ", ] ) - robot_description = {"robot_description": robot_description_content} + robot_description = { + "robot_description": ParameterValue(robot_description_content, value_type=str) + } # MoveIt Configuration robot_description_semantic_content = Command( From 8e92d46bf6021d3418e981ca099545d286619692 Mon Sep 17 00:00:00 2001 From: "mergify[bot]" <37929162+mergify[bot]@users.noreply.github.com> Date: Wed, 10 Sep 2025 17:23:18 +0200 Subject: [PATCH 092/133] Bump actions/setup-python from 5 to 6 (backport #1487) (#1492) --- .github/workflows/ci-format.yml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/workflows/ci-format.yml b/.github/workflows/ci-format.yml index 715112086..e7d085b02 100644 --- a/.github/workflows/ci-format.yml +++ b/.github/workflows/ci-format.yml @@ -16,7 +16,7 @@ jobs: runs-on: ubuntu-22.04 steps: - uses: actions/checkout@v4 - - uses: actions/setup-python@v5 + - uses: actions/setup-python@v6 with: python-version: 3.10.4 - name: Install system hooks From 234c0faefc5aa12b2b73541f6184f5ccda9ea188 Mon Sep 17 00:00:00 2001 From: "mergify[bot]" <37929162+mergify[bot]@users.noreply.github.com> Date: Wed, 10 Sep 2025 17:28:54 +0200 Subject: [PATCH 093/133] Auto-update pre-commit hooks (backport of #1475) (#1494) --- .pre-commit-config.yaml | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml index 1e80e8805..b7a2834bb 100644 --- a/.pre-commit-config.yaml +++ b/.pre-commit-config.yaml @@ -15,7 +15,7 @@ repos: # Standard hooks - repo: https://github.com/pre-commit/pre-commit-hooks - rev: v5.0.0 + rev: v6.0.0 hooks: - id: check-added-large-files - id: check-ast @@ -29,7 +29,7 @@ repos: - id: end-of-file-fixer - id: mixed-line-ending - id: trailing-whitespace - - id: check-byte-order-marker # Forbid UTF-8 byte-order markers + - id: fix-byte-order-marker # Forbid UTF-8 byte-order markers # Python hooks - repo: https://github.com/asottile/pyupgrade @@ -87,7 +87,7 @@ repos: args: ["--linelength=120"] - repo: https://github.com/pre-commit/mirrors-clang-format - rev: 'v20.1.8' + rev: 'v21.1.0' hooks: - id: clang-format From 26c87b4b55c5369d3689efabd8dec26ad1486f7b Mon Sep 17 00:00:00 2001 From: Felix Exner Date: Mon, 15 Sep 2025 11:45:39 +0200 Subject: [PATCH 094/133] Add support for UR8 Long (#1491) * Add support for launching UR8Long * Reorder model listings by generation, payload The list of supported robot models has been growing over time. This commit sorts them by generation (CB3, e-Series and UR series) and each generation by payload. --- README.md | 2 +- ur_bringup/config/ur8long_update_rate.yaml | 3 + ur_bringup/launch/ur8long.launch.py | 102 ++++++++++++++++++ ur_bringup/launch/ur_control.launch.py | 5 +- ur_moveit_config/launch/ur_moveit.launch.py | 5 +- .../config/ur8long_update_rate.yaml | 3 + ur_robot_driver/doc/usage/startup.rst | 4 +- ur_robot_driver/launch/ur8long.launch.py | 101 +++++++++++++++++ ur_robot_driver/launch/ur_control.launch.py | 5 +- ur_robot_driver/test/test_common.py | 5 +- 10 files changed, 224 insertions(+), 11 deletions(-) create mode 100644 ur_bringup/config/ur8long_update_rate.yaml create mode 100644 ur_bringup/launch/ur8long.launch.py create mode 100644 ur_robot_driver/config/ur8long_update_rate.yaml create mode 100644 ur_robot_driver/launch/ur8long.launch.py diff --git a/README.md b/README.md index 1fb75aa35..9ce977bcf 100644 --- a/README.md +++ b/README.md @@ -124,7 +124,7 @@ For getting started, you'll basically need three steps: details. ```bash - # Replace ur5e with one of ur3, ur3e, ur5, ur5e, ur7e, ur10, ur10e, ur12e, ur16e, ur15, ur20, ur30 + # Replace ur5e with one of ur3, ur5, ur10, ur3e, ur5e, ur7e, ur10e, ur12e, ur16e, ur8long, ur15, ur20, ur30 # Replace the IP address with the IP address of your actual robot / URSim ros2 launch ur_robot_driver ur_control.launch.py ur_type:=ur5e robot_ip:=192.168.56.101 ``` diff --git a/ur_bringup/config/ur8long_update_rate.yaml b/ur_bringup/config/ur8long_update_rate.yaml new file mode 100644 index 000000000..66ef3d736 --- /dev/null +++ b/ur_bringup/config/ur8long_update_rate.yaml @@ -0,0 +1,3 @@ +controller_manager: + ros__parameters: + update_rate: 500 # Hz diff --git a/ur_bringup/launch/ur8long.launch.py b/ur_bringup/launch/ur8long.launch.py new file mode 100644 index 000000000..08bc49f36 --- /dev/null +++ b/ur_bringup/launch/ur8long.launch.py @@ -0,0 +1,102 @@ +# Copyright (c) 2021 PickNik, Inc. +# +# 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. + +# +# Author: Denis Stogl + +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import LaunchConfiguration, ThisLaunchFileDir + + +def generate_launch_description(): + # Declare arguments + declared_arguments = [] + declared_arguments.append( + DeclareLaunchArgument( + "robot_ip", + description="IP address by which the robot can be reached.", + ) + ) + declared_arguments.append( + DeclareLaunchArgument( + "use_fake_hardware", + default_value="false", + description="Start robot with fake hardware mirroring command to its states.", + ) + ) + declared_arguments.append( + DeclareLaunchArgument( + "fake_sensor_commands", + default_value="false", + description="Enable fake command interfaces for sensors used for simple simulations. \ + Used only if 'use_fake_hardware' parameter is true.", + ) + ) + declared_arguments.append( + DeclareLaunchArgument( + "initial_joint_controller", + default_value="scaled_joint_trajectory_controller", + description="Initially loaded robot controller.", + choices=[ + "scaled_joint_trajectory_controller", + "joint_trajectory_controller", + "forward_velocity_controller", + "forward_position_controller", + ], + ) + ) + declared_arguments.append( + DeclareLaunchArgument( + "activate_joint_controller", + default_value="true", + description="Activate loaded joint controller.", + ) + ) + + # Initialize Arguments + robot_ip = LaunchConfiguration("robot_ip") + use_fake_hardware = LaunchConfiguration("use_fake_hardware") + fake_sensor_commands = LaunchConfiguration("fake_sensor_commands") + initial_joint_controller = LaunchConfiguration("initial_joint_controller") + activate_joint_controller = LaunchConfiguration("activate_joint_controller") + + base_launch = IncludeLaunchDescription( + PythonLaunchDescriptionSource([ThisLaunchFileDir(), "/ur_control.launch.py"]), + launch_arguments={ + "ur_type": "ur8long", + "robot_ip": robot_ip, + "use_fake_hardware": use_fake_hardware, + "fake_sensor_commands": fake_sensor_commands, + "initial_joint_controller": initial_joint_controller, + "activate_joint_controller": activate_joint_controller, + }.items(), + ) + + return LaunchDescription(declared_arguments + [base_launch]) diff --git a/ur_bringup/launch/ur_control.launch.py b/ur_bringup/launch/ur_control.launch.py index 9258be24a..90a5f57a3 100644 --- a/ur_bringup/launch/ur_control.launch.py +++ b/ur_bringup/launch/ur_control.launch.py @@ -52,14 +52,15 @@ def generate_launch_description(): description="Type/series of used UR robot.", choices=[ "ur3", - "ur3e", "ur5", + "ur10", + "ur3e", "ur5e", "ur7e", - "ur10", "ur10e", "ur12e", "ur16e", + "ur8long", "ur15", "ur20", "ur30", diff --git a/ur_moveit_config/launch/ur_moveit.launch.py b/ur_moveit_config/launch/ur_moveit.launch.py index 41de1f4cd..fe4447a77 100644 --- a/ur_moveit_config/launch/ur_moveit.launch.py +++ b/ur_moveit_config/launch/ur_moveit.launch.py @@ -285,14 +285,15 @@ def generate_launch_description(): description="Type/series of used UR robot.", choices=[ "ur3", - "ur3e", "ur5", + "ur10", + "ur3e", "ur5e", "ur7e", - "ur10", "ur10e", "ur12e", "ur16e", + "ur8long", "ur15", "ur20", "ur30", diff --git a/ur_robot_driver/config/ur8long_update_rate.yaml b/ur_robot_driver/config/ur8long_update_rate.yaml new file mode 100644 index 000000000..66ef3d736 --- /dev/null +++ b/ur_robot_driver/config/ur8long_update_rate.yaml @@ -0,0 +1,3 @@ +controller_manager: + ros__parameters: + update_rate: 500 # Hz diff --git a/ur_robot_driver/doc/usage/startup.rst b/ur_robot_driver/doc/usage/startup.rst index a1fa3f947..f8c2024ab 100644 --- a/ur_robot_driver/doc/usage/startup.rst +++ b/ur_robot_driver/doc/usage/startup.rst @@ -22,8 +22,8 @@ nodes for UR robots. The only required arguments are the ``ur_type`` and ``robot $ ros2 launch ur_robot_driver ur_control.launch.py ur_type:=ur5e robot_ip:=192.168.56.101 -Allowed ``ur_type`` strings: ``ur3``, ``ur3e``, ``ur5``, ``ur5e``, ``ur7e``, ``ur10``, ``ur10e``, -``ur12e``, ``ur16e``, ``ur15``, ``ur20``, ``ur30``. +Allowed ``ur_type`` strings: ``ur3``, ``ur5``, ``ur10``, ``ur3e``, ``ur5e``, ``ur7e``, ``ur10e``, +``ur12e``, ``ur16e``, ``ur8long``, ``ur15``, ``ur20``, ``ur30``. Other important arguments are: diff --git a/ur_robot_driver/launch/ur8long.launch.py b/ur_robot_driver/launch/ur8long.launch.py new file mode 100644 index 000000000..5eb25f91e --- /dev/null +++ b/ur_robot_driver/launch/ur8long.launch.py @@ -0,0 +1,101 @@ +# Copyright (c) 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. + +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import LaunchConfiguration, ThisLaunchFileDir + + +def generate_launch_description(): + # Declare arguments + declared_arguments = [] + declared_arguments.append( + DeclareLaunchArgument( + "robot_ip", + description="IP address by which the robot can be reached.", + ) + ) + declared_arguments.append( + DeclareLaunchArgument( + "use_mock_hardware", + default_value="false", + description="Start robot with mock hardware mirroring command to its states.", + ) + ) + declared_arguments.append( + DeclareLaunchArgument( + "mock_sensor_commands", + default_value="false", + description="Enable mock command interfaces for sensors used for simple simulations. " + "Used only if 'use_mock_hardware' parameter is true.", + ) + ) + declared_arguments.append( + DeclareLaunchArgument( + "initial_joint_controller", + default_value="scaled_joint_trajectory_controller", + description="Initially loaded robot controller.", + choices=[ + "scaled_joint_trajectory_controller", + "joint_trajectory_controller", + "forward_velocity_controller", + "forward_position_controller", + "freedrive_mode_controller", + "passthrough_trajectory_controller", + ], + ) + ) + declared_arguments.append( + DeclareLaunchArgument( + "activate_joint_controller", + default_value="true", + description="Activate loaded joint controller.", + ) + ) + + # Initialize Arguments + robot_ip = LaunchConfiguration("robot_ip") + use_mock_hardware = LaunchConfiguration("use_mock_hardware") + mock_sensor_commands = LaunchConfiguration("mock_sensor_commands") + initial_joint_controller = LaunchConfiguration("initial_joint_controller") + activate_joint_controller = LaunchConfiguration("activate_joint_controller") + + base_launch = IncludeLaunchDescription( + PythonLaunchDescriptionSource([ThisLaunchFileDir(), "/ur_control.launch.py"]), + launch_arguments={ + "ur_type": "ur8long", + "robot_ip": robot_ip, + "use_mock_hardware": use_mock_hardware, + "mock_sensor_commands": mock_sensor_commands, + "initial_joint_controller": initial_joint_controller, + "activate_joint_controller": activate_joint_controller, + }.items(), + ) + + return LaunchDescription(declared_arguments + [base_launch]) diff --git a/ur_robot_driver/launch/ur_control.launch.py b/ur_robot_driver/launch/ur_control.launch.py index faf51adac..ef2210894 100644 --- a/ur_robot_driver/launch/ur_control.launch.py +++ b/ur_robot_driver/launch/ur_control.launch.py @@ -400,14 +400,15 @@ def generate_launch_description(): description="Type/series of used UR robot.", choices=[ "ur3", - "ur3e", "ur5", + "ur10", + "ur3e", "ur5e", "ur7e", - "ur10", "ur10e", "ur12e", "ur16e", + "ur8long", "ur15", "ur20", "ur30", diff --git a/ur_robot_driver/test/test_common.py b/ur_robot_driver/test/test_common.py index 215f8ff55..e43f3760e 100644 --- a/ur_robot_driver/test/test_common.py +++ b/ur_robot_driver/test/test_common.py @@ -310,14 +310,15 @@ def _declare_launch_arguments(): description="Type/series of used UR robot.", choices=[ "ur3", - "ur3e", "ur5", + "ur10", + "ur3e", "ur5e", "ur7e", - "ur10", "ur10e", "ur12e", "ur16e", + "ur8long", "ur15", "ur20", "ur30", From 9058756345273219a13e7fecff2e629f72ad445a Mon Sep 17 00:00:00 2001 From: "mergify[bot]" <37929162+mergify[bot]@users.noreply.github.com> Date: Mon, 15 Sep 2025 13:37:43 +0200 Subject: [PATCH 095/133] Bump actions/checkout from 4 to 5 (backport #1479) (#1498) --- .github/workflows/check_links.yml | 2 +- .github/workflows/ci-format.yml | 2 +- .github/workflows/coverage-build.yml | 5 +++-- 3 files changed, 5 insertions(+), 4 deletions(-) diff --git a/.github/workflows/check_links.yml b/.github/workflows/check_links.yml index 55f8bd293..672bfa264 100644 --- a/.github/workflows/check_links.yml +++ b/.github/workflows/check_links.yml @@ -10,7 +10,7 @@ jobs: check_links: runs-on: ubuntu-latest steps: - - uses: actions/checkout@v4 + - uses: actions/checkout@v5 - name: Restore lychee cache id: restore-cache uses: actions/cache/restore@v4 diff --git a/.github/workflows/ci-format.yml b/.github/workflows/ci-format.yml index e7d085b02..6dae95d28 100644 --- a/.github/workflows/ci-format.yml +++ b/.github/workflows/ci-format.yml @@ -15,7 +15,7 @@ jobs: name: Format runs-on: ubuntu-22.04 steps: - - uses: actions/checkout@v4 + - uses: actions/checkout@v5 - uses: actions/setup-python@v6 with: python-version: 3.10.4 diff --git a/.github/workflows/coverage-build.yml b/.github/workflows/coverage-build.yml index e5d63b937..2b1fe5d71 100644 --- a/.github/workflows/coverage-build.yml +++ b/.github/workflows/coverage-build.yml @@ -19,8 +19,9 @@ jobs: - uses: ros-tooling/setup-ros@v0.3 with: required-ros-distributions: ${{ env.ROS_DISTRO }} - - uses: actions/checkout@v2 - - uses: ros-tooling/action-ros-ci@v0.2 + use-ros2-testing: true + - uses: actions/checkout@v5 + - uses: ros-tooling/action-ros-ci@v0.4 with: target-ros2-distro: ${{ env.ROS_DISTRO }} # build all packages listed in the meta package From 7bedd6acc970eb3c00a3b12c5a798b5f782aeac6 Mon Sep 17 00:00:00 2001 From: Felix Exner Date: Tue, 16 Sep 2025 10:54:35 +0200 Subject: [PATCH 096/133] Prepare changelogs for upcoming release --- ur/CHANGELOG.rst | 3 +++ ur_bringup/CHANGELOG.rst | 5 +++++ ur_calibration/CHANGELOG.rst | 5 +++++ ur_controllers/CHANGELOG.rst | 6 ++++++ ur_dashboard_msgs/CHANGELOG.rst | 5 +++++ ur_moveit_config/CHANGELOG.rst | 6 ++++++ ur_robot_driver/CHANGELOG.rst | 12 ++++++++++++ 7 files changed, 42 insertions(+) diff --git a/ur/CHANGELOG.rst b/ur/CHANGELOG.rst index c8949579c..1a5465d83 100644 --- a/ur/CHANGELOG.rst +++ b/ur/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package ur ^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 2.8.1 (2025-06-11) ------------------ diff --git a/ur_bringup/CHANGELOG.rst b/ur_bringup/CHANGELOG.rst index 29a107cab..a2f3128ad 100644 --- a/ur_bringup/CHANGELOG.rst +++ b/ur_bringup/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package ur_bringup ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Add support for UR8 Long (`#1491 `_) +* Contributors: Felix Exner + 2.8.1 (2025-06-11) ------------------ diff --git a/ur_calibration/CHANGELOG.rst b/ur_calibration/CHANGELOG.rst index dfbca8b3b..33b8ee2ee 100644 --- a/ur_calibration/CHANGELOG.rst +++ b/ur_calibration/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package ur_calibration ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Fix ur_calibration compilation on Windows (backport of `#1400 `_) (`#1408 `_) +* Contributors: mergify[bot] + 2.8.1 (2025-06-11) ------------------ * Update feature list (backport of `#1372 `_) (`#1374 `_) diff --git a/ur_controllers/CHANGELOG.rst b/ur_controllers/CHANGELOG.rst index aa248f264..31213a2e5 100644 --- a/ur_controllers/CHANGELOG.rst +++ b/ur_controllers/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package ur_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* ur_controllers: Fix compilation on Windows (backport `#1402 `_) (`#1412 `_) + Co-authored-by: Silvio Traversaro +* Contributors: mergify[bot] + 2.8.1 (2025-06-11) ------------------ * Use std_atomic in SJTC (backport of `#1385 `_) (`#1386 `_) diff --git a/ur_dashboard_msgs/CHANGELOG.rst b/ur_dashboard_msgs/CHANGELOG.rst index a6cb1055c..735642bdf 100644 --- a/ur_dashboard_msgs/CHANGELOG.rst +++ b/ur_dashboard_msgs/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package ur_dashboard_msgs ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Added 'is in remote control' call as a dashboard service (backport of `#1433 `_) (`#1436 `_) +* Contributors: mergify[bot] + 2.8.1 (2025-06-11) ------------------ diff --git a/ur_moveit_config/CHANGELOG.rst b/ur_moveit_config/CHANGELOG.rst index 3fbaabb36..37aa81d0b 100644 --- a/ur_moveit_config/CHANGELOG.rst +++ b/ur_moveit_config/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package ur_moveit_config ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Add support for UR8 Long (`#1491 `_) +* ur_moveit_config: Assure the description is loaded as string (`#1452 `_) +* Contributors: Felix Exner + 2.8.1 (2025-06-11) ------------------ diff --git a/ur_robot_driver/CHANGELOG.rst b/ur_robot_driver/CHANGELOG.rst index a731f6e9b..2e2f02d2e 100644 --- a/ur_robot_driver/CHANGELOG.rst +++ b/ur_robot_driver/CHANGELOG.rst @@ -1,3 +1,15 @@ +Forthcoming +----------- +* Add support for UR8 Long (`#1491 `_) +* Fix flaky controller switch test (backport of `#1447 `_) (`#1450 `_) +* fix_flaky_force_mode_test (backport of `#1429 `_) (`#1448 `_) +* Reduce flakiness of trajectory controller tests (backport of `#1443 `_) (`#1444 `_) +* Added 'is in remote control' call as a dashboard service (backport of `#1433 `_) (`#1436 `_) +* ur_robot_driver: Fix compilation on Windows (backport of `#1421 `_) (`#1431 `_) + Co-authored-by: Silvio Traversaro +* Refactor prepare_switch method (backport `#1417 `_) (`#1427 `_) +* Contributors: Felix Exner, mergify[bot] + 2.8.1 (2025-06-11) ------------------ * [force mode controller] Fix the task frame orientation (backport `#1379 `_) (`#1380 `_) From ef09036e37c533b9a0c468c4f854e2e8e56e7afc Mon Sep 17 00:00:00 2001 From: Felix Exner Date: Tue, 16 Sep 2025 10:54:58 +0200 Subject: [PATCH 097/133] 2.9.0 --- ur/CHANGELOG.rst | 4 ++-- ur/package.xml | 2 +- ur_bringup/CHANGELOG.rst | 4 ++-- ur_bringup/package.xml | 2 +- ur_calibration/CHANGELOG.rst | 4 ++-- ur_calibration/package.xml | 2 +- ur_controllers/CHANGELOG.rst | 4 ++-- ur_controllers/package.xml | 2 +- ur_dashboard_msgs/CHANGELOG.rst | 4 ++-- ur_dashboard_msgs/package.xml | 2 +- ur_moveit_config/CHANGELOG.rst | 4 ++-- ur_moveit_config/package.xml | 2 +- ur_robot_driver/CHANGELOG.rst | 4 ++-- ur_robot_driver/package.xml | 2 +- 14 files changed, 21 insertions(+), 21 deletions(-) diff --git a/ur/CHANGELOG.rst b/ur/CHANGELOG.rst index 1a5465d83..2894bc31d 100644 --- a/ur/CHANGELOG.rst +++ b/ur/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package ur ^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.9.0 (2025-09-16) +------------------ 2.8.1 (2025-06-11) ------------------ diff --git a/ur/package.xml b/ur/package.xml index ee55f8342..a567618ca 100644 --- a/ur/package.xml +++ b/ur/package.xml @@ -2,7 +2,7 @@ ur - 2.8.1 + 2.9.0 Metapackage for universal robots Felix Exner diff --git a/ur_bringup/CHANGELOG.rst b/ur_bringup/CHANGELOG.rst index a2f3128ad..a1d90d7bd 100644 --- a/ur_bringup/CHANGELOG.rst +++ b/ur_bringup/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package ur_bringup ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.9.0 (2025-09-16) +------------------ * Add support for UR8 Long (`#1491 `_) * Contributors: Felix Exner diff --git a/ur_bringup/package.xml b/ur_bringup/package.xml index 0d577b6d1..2384decd7 100644 --- a/ur_bringup/package.xml +++ b/ur_bringup/package.xml @@ -2,7 +2,7 @@ ur_bringup - 2.8.1 + 2.9.0 Launch file and run-time configurations, e.g. controllers. Felix Exner diff --git a/ur_calibration/CHANGELOG.rst b/ur_calibration/CHANGELOG.rst index 33b8ee2ee..00108108f 100644 --- a/ur_calibration/CHANGELOG.rst +++ b/ur_calibration/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package ur_calibration ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.9.0 (2025-09-16) +------------------ * Fix ur_calibration compilation on Windows (backport of `#1400 `_) (`#1408 `_) * Contributors: mergify[bot] diff --git a/ur_calibration/package.xml b/ur_calibration/package.xml index f9215eee6..2a7b4abdf 100644 --- a/ur_calibration/package.xml +++ b/ur_calibration/package.xml @@ -1,7 +1,7 @@ ur_calibration - 2.8.1 + 2.9.0 Package for extracting the factory calibration from a UR robot and change it such that it can be used by ur_description to gain a correct URDF Felix Exner diff --git a/ur_controllers/CHANGELOG.rst b/ur_controllers/CHANGELOG.rst index 31213a2e5..8b06f8d50 100644 --- a/ur_controllers/CHANGELOG.rst +++ b/ur_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package ur_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.9.0 (2025-09-16) +------------------ * ur_controllers: Fix compilation on Windows (backport `#1402 `_) (`#1412 `_) Co-authored-by: Silvio Traversaro * Contributors: mergify[bot] diff --git a/ur_controllers/package.xml b/ur_controllers/package.xml index 2585ddb8b..75027f855 100644 --- a/ur_controllers/package.xml +++ b/ur_controllers/package.xml @@ -2,7 +2,7 @@ ur_controllers - 2.8.1 + 2.9.0 Provides controllers that use the speed scaling interface of Universal Robots. Felix Exner diff --git a/ur_dashboard_msgs/CHANGELOG.rst b/ur_dashboard_msgs/CHANGELOG.rst index 735642bdf..8b1de132e 100644 --- a/ur_dashboard_msgs/CHANGELOG.rst +++ b/ur_dashboard_msgs/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package ur_dashboard_msgs ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.9.0 (2025-09-16) +------------------ * Added 'is in remote control' call as a dashboard service (backport of `#1433 `_) (`#1436 `_) * Contributors: mergify[bot] diff --git a/ur_dashboard_msgs/package.xml b/ur_dashboard_msgs/package.xml index 7db4e6163..b07ab7f7f 100644 --- a/ur_dashboard_msgs/package.xml +++ b/ur_dashboard_msgs/package.xml @@ -2,7 +2,7 @@ ur_dashboard_msgs - 2.8.1 + 2.9.0 Messages around the UR Dashboard server. Felix Exner diff --git a/ur_moveit_config/CHANGELOG.rst b/ur_moveit_config/CHANGELOG.rst index 37aa81d0b..db4dbeff0 100644 --- a/ur_moveit_config/CHANGELOG.rst +++ b/ur_moveit_config/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package ur_moveit_config ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.9.0 (2025-09-16) +------------------ * Add support for UR8 Long (`#1491 `_) * ur_moveit_config: Assure the description is loaded as string (`#1452 `_) * Contributors: Felix Exner diff --git a/ur_moveit_config/package.xml b/ur_moveit_config/package.xml index c51ee4416..9724ca9b4 100644 --- a/ur_moveit_config/package.xml +++ b/ur_moveit_config/package.xml @@ -2,7 +2,7 @@ ur_moveit_config - 2.8.1 + 2.9.0 An example package with MoveIt2 configurations for UR robots. diff --git a/ur_robot_driver/CHANGELOG.rst b/ur_robot_driver/CHANGELOG.rst index 2e2f02d2e..0a0c61614 100644 --- a/ur_robot_driver/CHANGELOG.rst +++ b/ur_robot_driver/CHANGELOG.rst @@ -1,5 +1,5 @@ -Forthcoming ------------ +2.9.0 (2025-09-16) +------------------ * Add support for UR8 Long (`#1491 `_) * Fix flaky controller switch test (backport of `#1447 `_) (`#1450 `_) * fix_flaky_force_mode_test (backport of `#1429 `_) (`#1448 `_) diff --git a/ur_robot_driver/package.xml b/ur_robot_driver/package.xml index c53b8df09..620b57775 100644 --- a/ur_robot_driver/package.xml +++ b/ur_robot_driver/package.xml @@ -2,7 +2,7 @@ ur_robot_driver - 2.8.1 + 2.9.0 The new driver for Universal Robots UR3, UR5 and UR10 robots with CB3 controllers and the e-series. Felix Exner From b152635e5cd645324af71168a131216a4f16ae84 Mon Sep 17 00:00:00 2001 From: "mergify[bot]" <37929162+mergify[bot]@users.noreply.github.com> Date: Wed, 17 Sep 2025 12:04:55 +0200 Subject: [PATCH 098/133] Add test for hardware component lifecycle (backport #1476) (#1506) --- .../integration_test_component_lifecycle.py | 174 ++++++++++++++++++ ur_robot_driver/test/test_common.py | 8 +- 2 files changed, 181 insertions(+), 1 deletion(-) create mode 100644 ur_robot_driver/test/integration_test_component_lifecycle.py diff --git a/ur_robot_driver/test/integration_test_component_lifecycle.py b/ur_robot_driver/test/integration_test_component_lifecycle.py new file mode 100644 index 000000000..2da4b1ae7 --- /dev/null +++ b/ur_robot_driver/test/integration_test_component_lifecycle.py @@ -0,0 +1,174 @@ +#!/usr/bin/env python +# 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 os +import sys +import time +import unittest + +import pytest + +import launch_testing +import rclpy +from rclpy.node import Node + +from lifecycle_msgs.msg import State + +sys.path.append(os.path.dirname(__file__)) +from test_common import ( # noqa: E402 + ControllerManagerInterface, + DashboardInterface, + IoStatusInterface, + generate_driver_test_description, +) + + +@pytest.mark.launch_test +@launch_testing.parametrize( + "tf_prefix", + [""], +) +def generate_test_description(tf_prefix): + return generate_driver_test_description(tf_prefix=tf_prefix) + + +class ComponentLifecycleTest(unittest.TestCase): + @classmethod + def setUpClass(cls): + # Initialize the ROS context + rclpy.init() + cls.node = Node("component_lifecycle_test") + time.sleep(1) + cls.init_robot(cls) + + @classmethod + def tearDownClass(cls): + # Shutdown the ROS context + cls.node.destroy_node() + rclpy.shutdown() + + def init_robot(self): + self._dashboard_interface = DashboardInterface(self.node) + self._controller_manager_interface = ControllerManagerInterface(self.node) + self._io_status_controller_interface = IoStatusInterface(self.node) + + def setUp(self): + self._dashboard_interface.start_robot() + time.sleep(1) + self.assertTrue(self._io_status_controller_interface.resend_robot_program().success) + + # + # Tests + # + + def test_component_lifecycle(self): + hardware_info = self._controller_manager_interface.list_hardware_components() + self.assertIsNotNone(hardware_info) + self.assertEqual(len(hardware_info.component), 1) + self.assertEqual(hardware_info.component[0].state.id, State.PRIMARY_STATE_ACTIVE) + component_name = hardware_info.component[0].name + + command_interfaces = hardware_info.component[0].command_interfaces + state_interfaces = hardware_info.component[0].state_interfaces + + # Check that all interfaces are available after startup + for interface in command_interfaces: + self.assertTrue(interface.is_available, f"Interface {interface.name} is not available") + for interface in state_interfaces: + self.assertTrue(interface.is_available, f"Interface {interface.name} is not available") + + self.assertTrue( + self._controller_manager_interface.set_hardware_component_state( + name=component_name, target_state=State(id=State.PRIMARY_STATE_INACTIVE) + ).ok + ) + time.sleep(2) + + # Check all interfaces are available after deactivation (This is current behavior, but docs say they should not be?) + hardware_info = self._controller_manager_interface.list_hardware_components() + self.assertIsNotNone(hardware_info) + self.assertEqual(hardware_info.component[0].state.id, State.PRIMARY_STATE_INACTIVE) + command_interfaces = hardware_info.component[0].command_interfaces + state_interfaces = hardware_info.component[0].state_interfaces + for interface in command_interfaces: + self.assertTrue( + interface.is_available, + f"Interface {interface.name} is not available after deactivation", + ) + for interface in state_interfaces: + self.assertTrue( + interface.is_available, + f"Interface {interface.name} is not available after deactivation", + ) + + self.assertTrue( + self._controller_manager_interface.set_hardware_component_state( + name=component_name, target_state=State(id=State.PRIMARY_STATE_UNCONFIGURED) + ).ok + ) + time.sleep(2) + + # Check all interfaces are unavailable after shutdown of hardware + hardware_info = self._controller_manager_interface.list_hardware_components() + self.assertIsNotNone(hardware_info) + self.assertEqual(hardware_info.component[0].state.id, State.PRIMARY_STATE_UNCONFIGURED) + command_interfaces = hardware_info.component[0].command_interfaces + state_interfaces = hardware_info.component[0].state_interfaces + for interface in command_interfaces: + self.assertFalse( + interface.is_available, f"Interface {interface.name} is available after shutdown" + ) + for interface in state_interfaces: + self.assertFalse( + interface.is_available, f"Interface {interface.name} is available after shutdown" + ) + + self.assertTrue( + self._controller_manager_interface.set_hardware_component_state( + name=component_name, target_state=State(id=State.PRIMARY_STATE_ACTIVE) + ).ok + ) + time.sleep(2) + + # Check all interfaces are available after reactivation + hardware_info = self._controller_manager_interface.list_hardware_components() + self.assertIsNotNone(hardware_info) + self.assertEqual(hardware_info.component[0].state.id, State.PRIMARY_STATE_ACTIVE) + command_interfaces = hardware_info.component[0].command_interfaces + state_interfaces = hardware_info.component[0].state_interfaces + for interface in command_interfaces: + self.assertTrue( + interface.is_available, + f"Interface {interface.name} is not available after reactivation", + ) + for interface in state_interfaces: + self.assertTrue( + interface.is_available, + f"Interface {interface.name} is not available after reactivation", + ) diff --git a/ur_robot_driver/test/test_common.py b/ur_robot_driver/test/test_common.py index e43f3760e..8780132bb 100644 --- a/ur_robot_driver/test/test_common.py +++ b/ur_robot_driver/test/test_common.py @@ -34,6 +34,8 @@ SwitchController, LoadController, UnloadController, + SetHardwareComponentState, + ListHardwareComponents, ) from launch import LaunchDescription from launch.actions import ( @@ -254,7 +256,11 @@ class ControllerManagerInterface( "load_controller": LoadController, "unload_controller": UnloadController, }, - services={"list_controllers": ListControllers}, + services={ + "list_controllers": ListControllers, + "set_hardware_component_state": SetHardwareComponentState, + "list_hardware_components": ListHardwareComponents, + }, ): def wait_for_controller(self, controller_name, target_state=None, timeout=TIMEOUT_WAIT_SERVICE): start_time = time.time() From 07902d74dc647c7cf1e6aca11394fd3e0282bda7 Mon Sep 17 00:00:00 2001 From: "mergify[bot]" <37929162+mergify[bot]@users.noreply.github.com> Date: Thu, 2 Oct 2025 12:38:45 +0200 Subject: [PATCH 099/133] Auto-update pre-commit hooks (backport of #1511) (#1516) --- .pre-commit-config.yaml | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml index b7a2834bb..85cfa8e9e 100644 --- a/.pre-commit-config.yaml +++ b/.pre-commit-config.yaml @@ -39,7 +39,7 @@ repos: args: [--py36-plus] - repo: https://github.com/psf/black - rev: 25.1.0 + rev: 25.9.0 hooks: - id: black args: ["--line-length=100"] @@ -87,7 +87,7 @@ repos: args: ["--linelength=120"] - repo: https://github.com/pre-commit/mirrors-clang-format - rev: 'v21.1.0' + rev: 'v21.1.2' hooks: - id: clang-format From 278cb9507cf2bba7da47ea8feac64a1cefc0a58d Mon Sep 17 00:00:00 2001 From: Felix Exner Date: Mon, 6 Oct 2025 08:16:36 +0200 Subject: [PATCH 100/133] Fix link to limits man page (#1518) The old link was for bionic --- ur_robot_driver/doc/installation/real_time.rst | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ur_robot_driver/doc/installation/real_time.rst b/ur_robot_driver/doc/installation/real_time.rst index deb347c05..3dd4e5e68 100644 --- a/ur_robot_driver/doc/installation/real_time.rst +++ b/ur_robot_driver/doc/installation/real_time.rst @@ -223,7 +223,7 @@ Setup user privileges to use real-time scheduling ------------------------------------------------- To be able to schedule threads with user privileges (what the driver will do) you'll have to change -the user's limits by changing ``/etc/security/limits.conf`` (See `the manpage `_ for details) +the user's limits by changing ``/etc/security/limits.conf`` (See `the manpage `_ for details) We recommend to setup a group for real-time users instead of writing a fixed username into the config file: From 4f1f0a6952e7db4f81150a11f55590314a7d6391 Mon Sep 17 00:00:00 2001 From: "mergify[bot]" <37929162+mergify[bot]@users.noreply.github.com> Date: Fri, 10 Oct 2025 16:06:55 +0200 Subject: [PATCH 101/133] Running integration tests with mock hardware (backport #1226) (#1508) --- ur_robot_driver/CMakeLists.txt | 19 +- .../integration_test_config_controller.py | 89 +++++ .../integration_test_controller_switch.py | 10 +- .../test/integration_test_force_mode.py | 4 +- .../test/integration_test_io_controller.py | 132 ++++++++ ...integration_test_passthrough_controller.py | 272 +++++++++++++++ ...ntegration_test_scaled_joint_controller.py | 227 +++++++++++++ .../test/integration_test_tool_contact.py | 4 +- ur_robot_driver/test/test_common.py | 113 +++++++ ur_robot_driver/test/test_mock_hardware.py | 122 +++++++ ur_robot_driver/urdf/ur.ros2_control.xacro | 312 ++++++++++++++++++ 11 files changed, 1292 insertions(+), 12 deletions(-) create mode 100644 ur_robot_driver/test/integration_test_config_controller.py create mode 100644 ur_robot_driver/test/integration_test_io_controller.py create mode 100644 ur_robot_driver/test/integration_test_passthrough_controller.py create mode 100755 ur_robot_driver/test/integration_test_scaled_joint_controller.py create mode 100644 ur_robot_driver/test/test_mock_hardware.py create mode 100644 ur_robot_driver/urdf/ur.ros2_control.xacro diff --git a/ur_robot_driver/CMakeLists.txt b/ur_robot_driver/CMakeLists.txt index 43a653cc9..06c7bc2fa 100644 --- a/ur_robot_driver/CMakeLists.txt +++ b/ur_robot_driver/CMakeLists.txt @@ -200,6 +200,11 @@ if(BUILD_TESTING) find_package(ur_msgs REQUIRED) find_package(launch_testing_ament_cmake) + add_launch_test(test/test_mock_hardware.py + TIMEOUT + 800 + ) + if(${UR_ROBOT_DRIVER_BUILD_INTEGRATION_TESTS}) add_launch_test(test/launch_args.py TIMEOUT @@ -209,7 +214,7 @@ if(BUILD_TESTING) TIMEOUT 180 ) - add_launch_test(test/robot_driver.py + add_launch_test(test/integration_test_scaled_joint_controller.py TIMEOUT 800 ) @@ -225,6 +230,18 @@ if(BUILD_TESTING) TIMEOUT 500 ) + add_launch_test(test/integration_test_config_controller.py + TIMEOUT + 800 + ) + add_launch_test(test/integration_test_passthrough_controller.py + TIMEOUT + 800 + ) + add_launch_test(test/integration_test_io_controller.py + TIMEOUT + 800 + ) add_launch_test(test/integration_test_tool_contact.py TIMEOUT 800 diff --git a/ur_robot_driver/test/integration_test_config_controller.py b/ur_robot_driver/test/integration_test_config_controller.py new file mode 100644 index 000000000..ae427d881 --- /dev/null +++ b/ur_robot_driver/test/integration_test_config_controller.py @@ -0,0 +1,89 @@ +#!/usr/bin/env python +# 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 os +import sys +import time +import unittest + +import launch_testing +import pytest +import rclpy +from rclpy.node import Node + +sys.path.append(os.path.dirname(__file__)) +from test_common import ( # noqa: E402 + ControllerManagerInterface, + DashboardInterface, + IoStatusInterface, + ConfigurationInterface, + generate_driver_test_description, +) + + +@pytest.mark.launch_test +@launch_testing.parametrize("tf_prefix", [(""), ("my_ur_")]) +def generate_test_description(tf_prefix): + return generate_driver_test_description(tf_prefix=tf_prefix) + + +class ConfigControllerTest(unittest.TestCase): + @classmethod + def setUpClass(cls): + # Initialize the ROS context + rclpy.init() + cls.node = Node("config_controller_test") + time.sleep(1) + cls.init_robot(cls) + + @classmethod + def tearDownClass(cls): + # Shutdown the ROS context + cls.node.destroy_node() + rclpy.shutdown() + + def init_robot(self): + self._dashboard_interface = DashboardInterface(self.node) + self._controller_manager_interface = ControllerManagerInterface(self.node) + self._io_status_controller_interface = IoStatusInterface(self.node) + self._configuration_controller_interface = ConfigurationInterface(self.node) + + def setUp(self): + self._dashboard_interface.start_robot() + time.sleep(1) + self.assertTrue(self._io_status_controller_interface.resend_robot_program().success) + + # + # Test functions + # + + def test_get_robot_software_version(self): + self.assertGreater( + self._configuration_controller_interface.get_robot_software_version().major, 1 + ) diff --git a/ur_robot_driver/test/integration_test_controller_switch.py b/ur_robot_driver/test/integration_test_controller_switch.py index 58e11a1e4..8bfd8ac7b 100644 --- a/ur_robot_driver/test/integration_test_controller_switch.py +++ b/ur_robot_driver/test/integration_test_controller_switch.py @@ -60,21 +60,17 @@ @pytest.mark.launch_test -@launch_testing.parametrize( - "tf_prefix", - [""], - # [(""), ("my_ur_")], -) +@launch_testing.parametrize("tf_prefix", [(""), ("my_ur_")]) def generate_test_description(tf_prefix): return generate_driver_test_description(tf_prefix=tf_prefix) -class RobotDriverTest(unittest.TestCase): +class ControllerSwitchTest(unittest.TestCase): @classmethod def setUpClass(cls): # Initialize the ROS context rclpy.init() - cls.node = Node("robot_driver_test") + cls.node = Node("controller_switching_test") time.sleep(1) cls.init_robot(cls) diff --git a/ur_robot_driver/test/integration_test_force_mode.py b/ur_robot_driver/test/integration_test_force_mode.py index 40ad582a6..5fd09cec5 100644 --- a/ur_robot_driver/test/integration_test_force_mode.py +++ b/ur_robot_driver/test/integration_test_force_mode.py @@ -88,12 +88,12 @@ def generate_test_description(tf_prefix): return generate_driver_test_description(tf_prefix=tf_prefix) -class RobotDriverTest(unittest.TestCase): +class ForceModeTest(unittest.TestCase): @classmethod def setUpClass(cls): # Initialize the ROS context rclpy.init() - cls.node = Node("robot_driver_test") + cls.node = Node("force_mode_test") time.sleep(1) cls.init_robot(cls) diff --git a/ur_robot_driver/test/integration_test_io_controller.py b/ur_robot_driver/test/integration_test_io_controller.py new file mode 100644 index 000000000..0856fd9fb --- /dev/null +++ b/ur_robot_driver/test/integration_test_io_controller.py @@ -0,0 +1,132 @@ +#!/usr/bin/env python +# 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 logging +import os +import sys +import time +import unittest + +import launch_testing +import pytest +import rclpy +from rclpy.node import Node +from ur_msgs.msg import IOStates + +sys.path.append(os.path.dirname(__file__)) +from test_common import ( # noqa: E402 + ControllerManagerInterface, + DashboardInterface, + IoStatusInterface, + generate_driver_test_description, +) + + +@pytest.mark.launch_test +@launch_testing.parametrize("tf_prefix", [(""), ("my_ur_")]) +def generate_test_description(tf_prefix): + return generate_driver_test_description(tf_prefix=tf_prefix) + + +class IOControllerTest(unittest.TestCase): + @classmethod + def setUpClass(cls): + # Initialize the ROS context + rclpy.init() + cls.node = Node("io_controller_test") + time.sleep(1) + cls.init_robot(cls) + + @classmethod + def tearDownClass(cls): + # Shutdown the ROS context + cls.node.destroy_node() + rclpy.shutdown() + + def init_robot(self): + self._dashboard_interface = DashboardInterface(self.node) + self._controller_manager_interface = ControllerManagerInterface(self.node) + self._io_status_controller_interface = IoStatusInterface(self.node) + + def setUp(self): + self._dashboard_interface.start_robot() + time.sleep(1) + self.assertTrue(self._io_status_controller_interface.resend_robot_program().success) + + # + # Test functions + # + + def test_set_io(self): + """Test to set an IO and check whether it has been set.""" + # Create io callback to verify result + io_msg = None + + def io_msg_cb(msg): + nonlocal io_msg + io_msg = msg + + io_states_sub = self.node.create_subscription( + IOStates, + "/io_and_status_controller/io_states", + io_msg_cb, + rclpy.qos.qos_profile_system_default, + ) + + # Set pin 0 to 1.0 + test_pin = 0 + + logging.info("Setting pin %d to 1.0", test_pin) + self._io_status_controller_interface.set_io(fun=1, pin=test_pin, state=1.0) + + # Wait until the pin state has changed + pin_state = False + end_time = time.time() + 5 + while not pin_state and time.time() < end_time: + rclpy.spin_once(self.node, timeout_sec=0.1) + if io_msg is not None: + pin_state = io_msg.digital_out_states[test_pin].state + + self.assertEqual(pin_state, 1.0) + + # Set pin 0 to 0.0 + logging.info("Setting pin %d to 0.0", test_pin) + self._io_status_controller_interface.set_io(fun=1, pin=test_pin, state=0.0) + + # Wait until the pin state has changed back + end_time = time.time() + 5 + while pin_state and time.time() < end_time: + rclpy.spin_once(self.node, timeout_sec=0.1) + if io_msg is not None: + pin_state = io_msg.digital_out_states[test_pin].state + + self.assertEqual(pin_state, 0.0) + + # Clean up io subscription + self.node.destroy_subscription(io_states_sub) diff --git a/ur_robot_driver/test/integration_test_passthrough_controller.py b/ur_robot_driver/test/integration_test_passthrough_controller.py new file mode 100644 index 000000000..edfa3b90e --- /dev/null +++ b/ur_robot_driver/test/integration_test_passthrough_controller.py @@ -0,0 +1,272 @@ +#!/usr/bin/env python +# 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 os +import sys +import time +import unittest + +from math import pi +import launch_testing +import pytest +import rclpy +from builtin_interfaces.msg import Duration +from control_msgs.action import FollowJointTrajectory +from control_msgs.msg import JointTolerance +from controller_manager_msgs.srv import SwitchController +from rclpy.node import Node +from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint + +sys.path.append(os.path.dirname(__file__)) +from test_common import ( # noqa: E402 + ActionInterface, + ControllerManagerInterface, + DashboardInterface, + IoStatusInterface, + generate_driver_test_description, + ROBOT_JOINTS, + TIMEOUT_EXECUTE_TRAJECTORY, +) + + +# Mock hardware does not work with passthrough controller, so dont test with it +@pytest.mark.launch_test +@launch_testing.parametrize("tf_prefix", [(""), ("my_ur_")]) +def generate_test_description(tf_prefix): + return generate_driver_test_description(tf_prefix=tf_prefix) + + +HOME = { + "elbow_joint": 0.0, + "shoulder_lift_joint": -1.5708, + "shoulder_pan_joint": 0.0, + "wrist_1_joint": -1.5708, + "wrist_2_joint": 0.0, + "wrist_3_joint": 0.0, +} +waypts = [[HOME[joint] + i * pi / 4 for joint in ROBOT_JOINTS] for i in [0, -1, 1]] +time_vec = [ + Duration(sec=4, nanosec=0), + Duration(sec=8, nanosec=0), + Duration(sec=12, nanosec=0), +] +TEST_TRAJECTORY = [(time_vec[i], waypts[i]) for i in range(len(waypts))] + + +class PassthroughControllerTest(unittest.TestCase): + @classmethod + def setUpClass(cls): + # Initialize the ROS context + rclpy.init() + cls.node = Node("passthrough_controller_test") + time.sleep(1) + cls.init_robot(cls) + + @classmethod + def tearDownClass(cls): + # Shutdown the ROS context + cls.node.destroy_node() + rclpy.shutdown() + + def init_robot(self): + + self._dashboard_interface = DashboardInterface(self.node) + + self._controller_manager_interface = ControllerManagerInterface(self.node) + self._io_status_controller_interface = IoStatusInterface(self.node) + + self._passthrough_forward_joint_trajectory = ActionInterface( + self.node, + "/passthrough_trajectory_controller/follow_joint_trajectory", + FollowJointTrajectory, + ) + + def setUp(self): + self._dashboard_interface.start_robot() + time.sleep(1) + self.assertTrue(self._io_status_controller_interface.resend_robot_program().success) + + # + # Test functions + # + + def test_start_passthrough_controller(self): + self.assertTrue( + self._controller_manager_interface.switch_controller( + strictness=SwitchController.Request.BEST_EFFORT, + activate_controllers=["passthrough_trajectory_controller"], + deactivate_controllers=["scaled_joint_trajectory_controller"], + ).ok + ) + + def test_passthrough_trajectory(self, tf_prefix): + self.assertTrue( + self._controller_manager_interface.switch_controller( + strictness=SwitchController.Request.BEST_EFFORT, + activate_controllers=["passthrough_trajectory_controller"], + deactivate_controllers=["scaled_joint_trajectory_controller"], + ).ok + ) + + goal_tolerance = [ + JointTolerance(position=0.01, name=tf_prefix + joint) for joint in ROBOT_JOINTS + ] + goal_time_tolerance = Duration(sec=1, nanosec=0) + trajectory = JointTrajectory( + points=[ + JointTrajectoryPoint(positions=pos, time_from_start=times) + for (times, pos) in TEST_TRAJECTORY + ], + joint_names=[tf_prefix + joint for joint in ROBOT_JOINTS], + ) + goal_handle = self._passthrough_forward_joint_trajectory.send_goal( + trajectory=trajectory, + goal_time_tolerance=goal_time_tolerance, + goal_tolerance=goal_tolerance, + ) + self.assertTrue(goal_handle.accepted) + if goal_handle.accepted: + result = self._passthrough_forward_joint_trajectory.get_result( + goal_handle, TIMEOUT_EXECUTE_TRAJECTORY + ) + self.assertEqual(result.error_code, FollowJointTrajectory.Result.SUCCESSFUL) + + def test_quintic_trajectory(self, tf_prefix): + # Full quintic trajectory + self.assertTrue( + self._controller_manager_interface.switch_controller( + strictness=SwitchController.Request.BEST_EFFORT, + activate_controllers=["passthrough_trajectory_controller"], + deactivate_controllers=["scaled_joint_trajectory_controller"], + ).ok + ) + trajectory = JointTrajectory( + points=[ + JointTrajectoryPoint( + positions=pos, + time_from_start=times, + velocities=[0.0, 0.0, 0.0, 0.0, 0.0, 0.0], + accelerations=[0.0, 0.0, 0.0, 0.0, 0.0, 0.0], + ) + for (times, pos) in TEST_TRAJECTORY + ], + joint_names=[tf_prefix + joint for joint in ROBOT_JOINTS], + ) + goal_time_tolerance = Duration(sec=1, nanosec=0) + goal_tolerance = [ + JointTolerance(position=0.01, name=tf_prefix + joint) for joint in ROBOT_JOINTS + ] + goal_handle = self._passthrough_forward_joint_trajectory.send_goal( + trajectory=trajectory, + goal_time_tolerance=goal_time_tolerance, + goal_tolerance=goal_tolerance, + ) + + self.assertTrue(goal_handle.accepted) + if goal_handle.accepted: + result = self._passthrough_forward_joint_trajectory.get_result( + goal_handle, TIMEOUT_EXECUTE_TRAJECTORY + ) + self.assertEqual(result.error_code, FollowJointTrajectory.Result.SUCCESSFUL) + + def test_impossible_goal_tolerance_fails(self, tf_prefix): + # Test impossible goal tolerance, should fail. + self.assertTrue( + self._controller_manager_interface.switch_controller( + strictness=SwitchController.Request.BEST_EFFORT, + activate_controllers=["passthrough_trajectory_controller"], + deactivate_controllers=["scaled_joint_trajectory_controller"], + ).ok + ) + trajectory = JointTrajectory( + points=[ + JointTrajectoryPoint(positions=pos, time_from_start=times) + for (times, pos) in TEST_TRAJECTORY + ], + joint_names=[tf_prefix + joint for joint in ROBOT_JOINTS], + ) + goal_tolerance = [ + JointTolerance(position=0.000000001, name=tf_prefix + joint) for joint in ROBOT_JOINTS + ] + goal_time_tolerance = Duration(sec=1, nanosec=0) + goal_handle = self._passthrough_forward_joint_trajectory.send_goal( + trajectory=trajectory, + goal_time_tolerance=goal_time_tolerance, + goal_tolerance=goal_tolerance, + ) + self.assertTrue(goal_handle.accepted) + if goal_handle.accepted: + result = self._passthrough_forward_joint_trajectory.get_result( + goal_handle, TIMEOUT_EXECUTE_TRAJECTORY + ) + self.assertEqual( + result.error_code, FollowJointTrajectory.Result.GOAL_TOLERANCE_VIOLATED + ) + + def test_impossible_goal_time_tolerance_fails(self, tf_prefix): + # Test impossible goal time + self.assertTrue( + self._controller_manager_interface.switch_controller( + strictness=SwitchController.Request.BEST_EFFORT, + activate_controllers=["passthrough_trajectory_controller"], + deactivate_controllers=["scaled_joint_trajectory_controller"], + ).ok + ) + + goal_tolerance = [ + JointTolerance(position=0.01, name=tf_prefix + joint) for joint in ROBOT_JOINTS + ] + goal_time_tolerance = Duration(sec=0, nanosec=10) + trajectory = JointTrajectory( + points=[ + JointTrajectoryPoint(positions=pos, time_from_start=times) + for (times, pos) in TEST_TRAJECTORY + ], + joint_names=[tf_prefix + joint for joint in ROBOT_JOINTS], + ) + goal_handle = self._passthrough_forward_joint_trajectory.send_goal( + trajectory=trajectory, + goal_time_tolerance=goal_time_tolerance, + goal_tolerance=goal_tolerance, + ) + self.assertTrue(goal_handle.accepted) + if goal_handle.accepted: + result = self._passthrough_forward_joint_trajectory.get_result( + goal_handle, TIMEOUT_EXECUTE_TRAJECTORY + ) + self.assertEqual( + result.error_code, FollowJointTrajectory.Result.GOAL_TOLERANCE_VIOLATED + ) + self.assertTrue( + self._controller_manager_interface.switch_controller( + strictness=SwitchController.Request.BEST_EFFORT, + deactivate_controllers=["passthrough_trajectory_controller"], + activate_controllers=["scaled_joint_trajectory_controller"], + ).ok + ) diff --git a/ur_robot_driver/test/integration_test_scaled_joint_controller.py b/ur_robot_driver/test/integration_test_scaled_joint_controller.py new file mode 100755 index 000000000..809465012 --- /dev/null +++ b/ur_robot_driver/test/integration_test_scaled_joint_controller.py @@ -0,0 +1,227 @@ +#!/usr/bin/env python3 +# Copyright 2019, FZI Forschungszentrum Informatik +# +# 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 logging +import os +import sys +import time +import unittest + +import launch_testing +import pytest +import rclpy +from builtin_interfaces.msg import Duration +from control_msgs.action import FollowJointTrajectory +from controller_manager_msgs.srv import SwitchController +from rclpy.node import Node +from sensor_msgs.msg import JointState +from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint + +sys.path.append(os.path.dirname(__file__)) +from test_common import ( # noqa: E402 + ActionInterface, + ControllerManagerInterface, + DashboardInterface, + IoStatusInterface, + generate_driver_test_description, + ROBOT_JOINTS, + TIMEOUT_EXECUTE_TRAJECTORY, + sjtc_trajectory_test, + sjtc_illegal_trajectory_test, +) + + +@pytest.mark.launch_test +@launch_testing.parametrize("tf_prefix", [(""), ("my_ur_")]) +def generate_test_description(tf_prefix): + return generate_driver_test_description(tf_prefix=tf_prefix) + + +class SJTCTest(unittest.TestCase): + @classmethod + def setUpClass(cls): + # Initialize the ROS context + rclpy.init() + cls.node = Node("sjtc_test") + time.sleep(1) + cls.init_robot(cls) + + @classmethod + def tearDownClass(cls): + # Shutdown the ROS context + cls.node.destroy_node() + rclpy.shutdown() + + def init_robot(self): + self._dashboard_interface = DashboardInterface(self.node) + self._controller_manager_interface = ControllerManagerInterface(self.node) + self._io_status_controller_interface = IoStatusInterface(self.node) + + self._scaled_follow_joint_trajectory = ActionInterface( + self.node, + "/scaled_joint_trajectory_controller/follow_joint_trajectory", + FollowJointTrajectory, + ) + + def setUp(self): + self._dashboard_interface.start_robot() + time.sleep(1) + self.assertTrue(self._io_status_controller_interface.resend_robot_program().success) + + # + # Test functions + # + + def test_start_scaled_jtc_controller(self): + self.assertTrue( + self._controller_manager_interface.switch_controller( + strictness=SwitchController.Request.BEST_EFFORT, + activate_controllers=["scaled_joint_trajectory_controller"], + ).ok + ) + + def test_trajectory(self, tf_prefix): + sjtc_trajectory_test(self, tf_prefix) + + def test_illegal_trajectory(self, tf_prefix): + sjtc_illegal_trajectory_test(self, tf_prefix) + + def test_trajectory_scaled(self, tf_prefix): + """Test robot movement.""" + self.assertTrue( + self._controller_manager_interface.switch_controller( + strictness=SwitchController.Request.BEST_EFFORT, + deactivate_controllers=["passthrough_trajectory_controller"], + activate_controllers=["scaled_joint_trajectory_controller"], + ).ok + ) + # Construct test trajectory + test_trajectory = [ + (Duration(sec=6, nanosec=0), [0.0 for j in ROBOT_JOINTS]), + (Duration(sec=6, nanosec=500000000), [-1.0 for j in ROBOT_JOINTS]), + ] + + trajectory = JointTrajectory( + joint_names=[tf_prefix + joint for joint in ROBOT_JOINTS], + points=[ + JointTrajectoryPoint(positions=test_pos, time_from_start=test_time) + for (test_time, test_pos) in test_trajectory + ], + ) + + # Execute trajectory + logging.info("Sending goal for robot to follow") + goal_handle = self._scaled_follow_joint_trajectory.send_goal(trajectory=trajectory) + self.assertTrue(goal_handle.accepted) + + # Verify execution + result = self._scaled_follow_joint_trajectory.get_result( + goal_handle, + TIMEOUT_EXECUTE_TRAJECTORY, + ) + self.assertEqual(result.error_code, FollowJointTrajectory.Result.SUCCESSFUL) + + def test_trajectory_scaled_aborts_on_violation(self, tf_prefix): + """Test that the robot correctly aborts the trajectory when the constraints are violated.""" + # Construct test trajectory + test_trajectory = [ + (Duration(sec=6, nanosec=0), [0.0 for j in ROBOT_JOINTS]), + ( + Duration(sec=6, nanosec=50000000), + [-1.0 for j in ROBOT_JOINTS], + ), # physically unfeasible + ( + Duration(sec=8, nanosec=0), + [-1.5 for j in ROBOT_JOINTS], + ), # physically unfeasible + ] + + trajectory = JointTrajectory( + joint_names=[tf_prefix + joint for joint in ROBOT_JOINTS], + points=[ + JointTrajectoryPoint(positions=test_pos, time_from_start=test_time) + for (test_time, test_pos) in test_trajectory + ], + ) + + last_joint_state = None + + def js_cb(msg): + nonlocal last_joint_state + last_joint_state = msg + + joint_state_sub = self.node.create_subscription(JointState, "/joint_states", js_cb, 1) + joint_state_sub # prevent warning about unused variable + + # Send goal + logging.info("Sending goal for robot to follow") + goal_handle = self._scaled_follow_joint_trajectory.send_goal(trajectory=trajectory) + self.assertTrue(goal_handle.accepted) + + # Get result + result = self._scaled_follow_joint_trajectory.get_result( + goal_handle, + TIMEOUT_EXECUTE_TRAJECTORY, + ) + self.assertEqual(result.error_code, FollowJointTrajectory.Result.PATH_TOLERANCE_VIOLATED) + + state_when_aborted = last_joint_state + + # This section is to make sure the robot stopped moving once the trajectory was aborted + time.sleep(2.0) + # Ugly workaround since we want to wait for a joint state in the same thread + while last_joint_state == state_when_aborted: + rclpy.spin_once(self.node) + state_after_sleep = last_joint_state + + logging.info("Joint states before sleep:\t %s", state_when_aborted.position.tolist()) + logging.info("Joint states after sleep:\t %s", state_after_sleep.position.tolist()) + + self.assertTrue( + all( + [ + abs(a - b) < 0.01 + for a, b in zip(state_after_sleep.position, state_when_aborted.position) + ] + ) + ) + + # TODO: uncomment when JTC starts taking into account goal_time_tolerance from goal message + # see https://github.com/ros-controls/ros2_controllers/issues/249 + # Now do the same again, but with a goal time constraint + # self.node.get_logger().info("Sending scaled goal with time restrictions") + # + # goal.goal_time_tolerance = Duration(nanosec=10000000) + # goal_response = self.call_action("/scaled_joint_trajectory_controller/follow_joint_trajectory", goal) + # + # self.assertEqual(goal_response.accepted, True) + # + # if goal_response.accepted: + # result = self.get_result("/scaled_joint_trajectory_controller/follow_joint_trajectory", goal_response, TIMEOUT_EXECUTE_TRAJECTORY) + # self.assertEqual(result.error_code, FollowJointTrajectory.Result.GOAL_TOLERANCE_VIOLATED) + # self.node.get_logger().info("Received result GOAL_TOLERANCE_VIOLATED") diff --git a/ur_robot_driver/test/integration_test_tool_contact.py b/ur_robot_driver/test/integration_test_tool_contact.py index db6c60817..2cce70cf4 100644 --- a/ur_robot_driver/test/integration_test_tool_contact.py +++ b/ur_robot_driver/test/integration_test_tool_contact.py @@ -62,12 +62,12 @@ def generate_test_description(tf_prefix): return generate_driver_test_description(tf_prefix=tf_prefix) -class RobotDriverTest(unittest.TestCase): +class ToolContactTest(unittest.TestCase): @classmethod def setUpClass(cls): # Initialize the ROS context rclpy.init() - cls.node = Node("robot_driver_test") + cls.node = Node("tool_contact_test") time.sleep(1) cls.init_robot(cls) diff --git a/ur_robot_driver/test/test_common.py b/ur_robot_driver/test/test_common.py index 8780132bb..0826d8091 100644 --- a/ur_robot_driver/test/test_common.py +++ b/ur_robot_driver/test/test_common.py @@ -60,11 +60,16 @@ Load, ) from ur_msgs.srv import SetIO, GetRobotSoftwareVersion, SetForceMode +from builtin_interfaces.msg import Duration +from control_msgs.action import FollowJointTrajectory +from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint TIMEOUT_WAIT_SERVICE = 10 TIMEOUT_WAIT_SERVICE_INITIAL = 120 # If we download the docker image simultaneously to the tests, it can take quite some time until the dashboard server is reachable and usable. TIMEOUT_WAIT_ACTION = 10 +TIMEOUT_EXECUTE_TRAJECTORY = 30 + ROBOT_JOINTS = [ "elbow_joint", "shoulder_lift_joint", @@ -306,6 +311,79 @@ class ForceModeInterface( pass +def sjtc_trajectory_test(tester, tf_prefix): + """Test robot movement.""" + tester.assertTrue( + tester._controller_manager_interface.switch_controller( + strictness=SwitchController.Request.BEST_EFFORT, + deactivate_controllers=["passthrough_trajectory_controller"], + activate_controllers=["scaled_joint_trajectory_controller"], + ).ok + ) + # Construct test trajectory + test_trajectory = [ + (Duration(sec=6, nanosec=0), [0.0 for j in ROBOT_JOINTS]), + (Duration(sec=9, nanosec=0), [-0.5 for j in ROBOT_JOINTS]), + (Duration(sec=12, nanosec=0), [-1.0 for j in ROBOT_JOINTS]), + ] + + trajectory = JointTrajectory( + joint_names=[tf_prefix + joint for joint in ROBOT_JOINTS], + points=[ + JointTrajectoryPoint(positions=test_pos, time_from_start=test_time) + for (test_time, test_pos) in test_trajectory + ], + ) + + # Sending trajectory goal + logging.info("Sending simple goal") + goal_handle = tester._scaled_follow_joint_trajectory.send_goal(trajectory=trajectory) + tester.assertTrue(goal_handle.accepted) + + # Verify execution + result = tester._scaled_follow_joint_trajectory.get_result( + goal_handle, TIMEOUT_EXECUTE_TRAJECTORY + ) + tester.assertEqual(result.error_code, FollowJointTrajectory.Result.SUCCESSFUL) + + +def sjtc_illegal_trajectory_test(tester, tf_prefix): + """ + Test trajectory server. + + This is more of a validation test that the testing suite does the right thing + """ + tester.assertTrue( + tester._controller_manager_interface.switch_controller( + strictness=SwitchController.Request.BEST_EFFORT, + deactivate_controllers=["passthrough_trajectory_controller"], + activate_controllers=["scaled_joint_trajectory_controller"], + ).ok + ) + # Construct test trajectory, the second point wrongly starts before the first + test_trajectory = [ + (Duration(sec=6, nanosec=0), [0.0 for j in ROBOT_JOINTS]), + (Duration(sec=3, nanosec=0), [-0.5 for j in ROBOT_JOINTS]), + ] + + trajectory = JointTrajectory( + joint_names=[tf_prefix + joint for joint in ROBOT_JOINTS], + points=[ + JointTrajectoryPoint(positions=test_pos, time_from_start=test_time) + for (test_time, test_pos) in test_trajectory + ], + ) + + # Send illegal goal + logging.info("Sending illegal goal") + goal_handle = tester._scaled_follow_joint_trajectory.send_goal( + trajectory=trajectory, + ) + + # Verify the failure is correctly detected + tester.assertFalse(goal_handle.accepted) + + def _declare_launch_arguments(): declared_arguments = [] @@ -377,6 +455,41 @@ def generate_dashboard_test_description(): ) +def generate_mock_hardware_test_description( + tf_prefix="", + initial_joint_controller="scaled_joint_trajectory_controller", + controller_spawner_timeout=TIMEOUT_WAIT_SERVICE_INITIAL, +): + + ur_type = LaunchConfiguration("ur_type") + + launch_arguments = { + "robot_ip": "0.0.0.0", + "ur_type": ur_type, + "launch_rviz": "false", + "controller_spawner_timeout": str(controller_spawner_timeout), + "initial_joint_controller": initial_joint_controller, + "headless_mode": "true", + "launch_dashboard_client": "true", + "start_joint_controller": "false", + "use_fake_hardware": "true", + "fake_sensor_commands": "true", + } + if tf_prefix: + launch_arguments["tf_prefix"] = tf_prefix + + robot_driver = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + PathJoinSubstitution( + [FindPackageShare("ur_robot_driver"), "launch", "ur_control.launch.py"] + ) + ), + launch_arguments=launch_arguments.items(), + ) + + return LaunchDescription(_declare_launch_arguments() + [ReadyToTest(), robot_driver]) + + def generate_driver_test_description( tf_prefix="", initial_joint_controller="scaled_joint_trajectory_controller", diff --git a/ur_robot_driver/test/test_mock_hardware.py b/ur_robot_driver/test/test_mock_hardware.py new file mode 100644 index 000000000..0dfebec6a --- /dev/null +++ b/ur_robot_driver/test/test_mock_hardware.py @@ -0,0 +1,122 @@ +#!/usr/bin/env python +# 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 os +import sys +import time +import unittest + +import launch_testing +import pytest +import rclpy +from rclpy.node import Node +from control_msgs.action import FollowJointTrajectory +from controller_manager_msgs.srv import SwitchController + +sys.path.append(os.path.dirname(__file__)) +from test_common import ( # noqa: E402 + ActionInterface, + ControllerManagerInterface, + IoStatusInterface, + ConfigurationInterface, + generate_mock_hardware_test_description, + sjtc_trajectory_test, + sjtc_illegal_trajectory_test, +) + + +@pytest.mark.launch_test +@launch_testing.parametrize("tf_prefix", [(""), ("my_ur_")]) +def generate_test_description(tf_prefix): + return generate_mock_hardware_test_description(tf_prefix=tf_prefix) + + +class MockHWTest(unittest.TestCase): + @classmethod + def setUpClass(cls): + # Initialize the ROS context + rclpy.init() + cls.node = Node("mock_hardware_test") + time.sleep(1) + cls.init_robot(cls) + + @classmethod + def tearDownClass(cls): + # Shutdown the ROS context + cls.node.destroy_node() + rclpy.shutdown() + + # Connect to all interfaces and actions, even ones we know won't work with mock hardware (Except dashboard) + def init_robot(self): + self._controller_manager_interface = ControllerManagerInterface(self.node) + self._io_status_controller_interface = IoStatusInterface(self.node) + self._configuration_controller_interface = ConfigurationInterface(self.node) + + self._scaled_follow_joint_trajectory = ActionInterface( + self.node, + "/scaled_joint_trajectory_controller/follow_joint_trajectory", + FollowJointTrajectory, + ) + + self._passthrough_forward_joint_trajectory = ActionInterface( + self.node, + "/passthrough_trajectory_controller/follow_joint_trajectory", + FollowJointTrajectory, + ) + + # + # Test functions + # + + def test_get_robot_software_version(self): + self.assertEqual( + self._configuration_controller_interface.get_robot_software_version().major, 1 + ) + + def test_start_scaled_jtc_controller(self): + # Deactivate controller, if it is not already + self.assertTrue( + self._controller_manager_interface.switch_controller( + strictness=SwitchController.Request.BEST_EFFORT, + deactivate_controllers=["scaled_joint_trajectory_controller"], + ).ok + ) + # Activate controller + self.assertTrue( + self._controller_manager_interface.switch_controller( + strictness=SwitchController.Request.STRICT, + activate_controllers=["scaled_joint_trajectory_controller"], + ).ok + ) + + def test_trajectory(self, tf_prefix): + sjtc_trajectory_test(self, tf_prefix) + + def test_illegal_trajectory(self, tf_prefix): + sjtc_illegal_trajectory_test(self, tf_prefix) diff --git a/ur_robot_driver/urdf/ur.ros2_control.xacro b/ur_robot_driver/urdf/ur.ros2_control.xacro new file mode 100644 index 000000000..b480434b0 --- /dev/null +++ b/ur_robot_driver/urdf/ur.ros2_control.xacro @@ -0,0 +1,312 @@ + + + + + + + + + + + + + + + + + + + + mock_components/GenericSystem + ${mock_sensor_commands} + 0.0 + true + + + ur_robot_driver/URPositionHardwareInterface + ${robot_ip} + ${script_filename} + ${output_recipe_filename} + ${input_recipe_filename} + ${headless_mode} + ${reverse_port} + ${script_sender_port} + ${reverse_ip} + ${script_command_port} + ${trajectory_port} + ${tf_prefix} + ${non_blocking_read} + 2000 + 0.03 + ${use_tool_communication} + ${kinematics_hash} + ${tool_voltage} + ${tool_parity} + ${tool_baud_rate} + ${tool_stop_bits} + ${tool_rx_idle_chars} + ${tool_tx_idle_chars} + ${tool_device_name} + ${tool_tcp_port} + ${robot_receive_timeout} + + + + + + + + + + + 1.0 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + 1 + + + + + + + + From 4008da7d7b714b9cbb4111ecf8875c28d7f978fa Mon Sep 17 00:00:00 2001 From: "mergify[bot]" <37929162+mergify[bot]@users.noreply.github.com> Date: Mon, 13 Oct 2025 14:18:33 +0200 Subject: [PATCH 102/133] Wait for used controllers in test setup (backport #1519) (#1520) --- ur_robot_driver/test/integration_test_force_mode.py | 4 ++++ .../test/integration_test_passthrough_controller.py | 4 ++++ .../test/integration_test_scaled_joint_controller.py | 4 ++++ 3 files changed, 12 insertions(+) diff --git a/ur_robot_driver/test/integration_test_force_mode.py b/ur_robot_driver/test/integration_test_force_mode.py index 5fd09cec5..c513eb5d4 100644 --- a/ur_robot_driver/test/integration_test_force_mode.py +++ b/ur_robot_driver/test/integration_test_force_mode.py @@ -114,6 +114,10 @@ def init_robot(self): FollowJointTrajectory, ) + self._controller_manager_interface.wait_for_controller("force_mode_controller") + self._controller_manager_interface.wait_for_controller("scaled_joint_trajectory_controller") + self._controller_manager_interface.wait_for_controller("joint_trajectory_controller") + def setUp(self): self._dashboard_interface.start_robot() time.sleep(1) diff --git a/ur_robot_driver/test/integration_test_passthrough_controller.py b/ur_robot_driver/test/integration_test_passthrough_controller.py index edfa3b90e..55001f1f7 100644 --- a/ur_robot_driver/test/integration_test_passthrough_controller.py +++ b/ur_robot_driver/test/integration_test_passthrough_controller.py @@ -107,6 +107,10 @@ def init_robot(self): FollowJointTrajectory, ) + # Wait for all controllers needed below, as controller manager services might fail + # e.g. when attempting to deactivate an unknown controller + self._controller_manager_interface.wait_for_controller("scaled_joint_trajectory_controller") + def setUp(self): self._dashboard_interface.start_robot() time.sleep(1) diff --git a/ur_robot_driver/test/integration_test_scaled_joint_controller.py b/ur_robot_driver/test/integration_test_scaled_joint_controller.py index 809465012..073acac63 100755 --- a/ur_robot_driver/test/integration_test_scaled_joint_controller.py +++ b/ur_robot_driver/test/integration_test_scaled_joint_controller.py @@ -88,6 +88,10 @@ def init_robot(self): FollowJointTrajectory, ) + # Wait for all controllers needed below, as controller manager services might fail + # e.g. when attempting to deactivate an unknown controller + self._controller_manager_interface.wait_for_controller("passthrough_trajectory_controller") + def setUp(self): self._dashboard_interface.start_robot() time.sleep(1) From ee5e57be2d9be929bf172f5d28420c383b1df78b Mon Sep 17 00:00:00 2001 From: "mergify[bot]" <37929162+mergify[bot]@users.noreply.github.com> Date: Fri, 17 Oct 2025 15:51:50 +0200 Subject: [PATCH 103/133] Trajectory until node (backport of #1461) (#1522) 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. --- ur_robot_driver/CMakeLists.txt | 26 +- ur_robot_driver/doc/index.rst | 7 +- ur_robot_driver/doc/trajectory_until_node.rst | 23 + .../examples/move_until_example.py | 132 ++++++ .../ur_robot_driver/trajectory_until_node.hpp | 140 ++++++ ur_robot_driver/launch/ur_control.launch.py | 14 + ur_robot_driver/src/trajectory_until_node.cpp | 401 ++++++++++++++++++ .../test/integration_test_trajectory_until.py | 161 +++++++ ur_robot_driver/test/test_common.py | 1 + 9 files changed, 898 insertions(+), 7 deletions(-) create mode 100644 ur_robot_driver/doc/trajectory_until_node.rst create mode 100755 ur_robot_driver/examples/move_until_example.py create mode 100644 ur_robot_driver/include/ur_robot_driver/trajectory_until_node.hpp create mode 100644 ur_robot_driver/src/trajectory_until_node.cpp create mode 100644 ur_robot_driver/test/integration_test_trajectory_until.py diff --git a/ur_robot_driver/CMakeLists.txt b/ur_robot_driver/CMakeLists.txt index 06c7bc2fa..0721636af 100644 --- 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 index d2889276a..9b60dedf9 100644 --- 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 new file mode 100644 index 000000000..53aa6d63e --- /dev/null +++ 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 `_ 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:=`` 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 new file mode 100755 index 000000000..d0f10d894 --- /dev/null +++ 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 new file mode 100644 index 000000000..0d6cfc3eb --- /dev/null +++ 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 +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +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::SharedPtr action_server_; + rclcpp_action::Client::SharedPtr trajectory_action_client_; + std::string trajectory_action_uri_; + + // Add new until types here, when available + using TCClient = rclcpp_action::Client::SharedPtr; + std::variant until_action_client_variant; + std::string until_action_uri_; + + rclcpp::CallbackGroup::SharedPtr server_callback_group; + rclcpp::CallbackGroup::SharedPtr clients_callback_group; + + std::shared_ptr> server_goal_handle_; + + template + void send_until_goal(std::shared_ptr goal); + + template + void until_response_callback(const typename rclcpp_action::ClientGoalHandle::SharedPtr& goal_handle); + + template + void until_result_callback(const typename rclcpp_action::ClientGoalHandle::WrappedResult& result); + + void cancel_until_goal(); + + void trajectory_response_callback( + const rclcpp_action::ClientGoalHandle::SharedPtr& goal_handle); + void trajectory_feedback_callback( + const rclcpp_action::ClientGoalHandle::SharedPtr& goal_handle, + const std::shared_ptr feedback); + void trajectory_result_callback( + const rclcpp_action::ClientGoalHandle::WrappedResult& result); + + bool assign_until_action_client(std::shared_ptr goal); + + rclcpp_action::GoalResponse goal_received_callback(const rclcpp_action::GoalUUID& uuid, + std::shared_ptr goal); + void + goal_accepted_callback(const std::shared_ptr> goal_handle); + rclcpp_action::CancelResponse + goal_cancelled_callback(const std::shared_ptr> goal_handle); + + void send_trajectory_goal(std::shared_ptr goal); + + void report_goal(rclcpp_action::ClientGoalHandle::WrappedResult result); + + template + void report_goal(UntilResult result); + + void reset_node(); + + rclcpp_action::ClientGoalHandle::SharedPtr + current_trajectory_goal_handle_; + rclcpp_action::ClientGoalHandle::SharedPtr current_until_goal_handle_; + + std::shared_ptr prealloc_res_; + + std::shared_ptr prealloc_fb_; + + std::atomic trajectory_accepted_; + std::atomic 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 index ef2210894..49f4f33f7 100644 --- 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 new file mode 100644 index 000000000..c249a9209 --- /dev/null +++ b/ur_robot_driver/src/trajectory_until_node.cpp @@ -0,0 +1,401 @@ +// 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 + * + * + * + * + */ +//---------------------------------------------------------------------- + +#include +#include + +namespace ur_robot_driver +{ + +using TCAction = ur_msgs::action::ToolContact; +using GoalHandleTrajectoryUntil = rclcpp_action::ServerGoalHandle; +using FollowJointTrajectory = control_msgs::action::FollowJointTrajectory; +using TrajectoryResult = rclcpp_action::ClientGoalHandle::WrappedResult; + +using namespace std::chrono_literals; + +TrajectoryUntilNode::TrajectoryUntilNode(const rclcpp::NodeOptions& options) + : Node("TrajectoryUntilNode", options) + , server_goal_handle_(nullptr) + , current_trajectory_goal_handle_(nullptr) + , current_until_goal_handle_(nullptr) + , trajectory_accepted_(false) + , until_accepted_(false) +{ + prealloc_res_ = std::make_shared(TrajectoryUntilAction::Result()); + prealloc_fb_ = std::make_shared(TrajectoryUntilAction::Feedback()); + + // Different callback groups for the server and the clients, so their callbacks can run concurrently. + server_callback_group = create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); + clients_callback_group = create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); + + declare_parameter("motion_controller_uri", "scaled_joint_trajectory_controller/follow_joint_trajectory"); + get_parameter("motion_controller_uri", trajectory_action_uri_); + // Initialize a trajectory action client, to a generic action that does not exist. This is remapped via ros-args when + // launching the node + trajectory_action_client_ = + rclcpp_action::create_client(this, trajectory_action_uri_, clients_callback_group); + + declare_parameter("until_action_uri", "tool_contact_controller/detect_tool_contact"); + get_parameter("until_action_uri", until_action_uri_); + + // Create action server to advertise the "/trajectory_until_node/execute" + action_server_ = rclcpp_action::create_server( + this, "~/execute", + std::bind(&TrajectoryUntilNode::goal_received_callback, this, std::placeholders::_1, std::placeholders::_2), + std::bind(&TrajectoryUntilNode::goal_cancelled_callback, this, std::placeholders::_1), + std::bind(&TrajectoryUntilNode::goal_accepted_callback, this, std::placeholders::_1), + rcl_action_server_get_default_options(), server_callback_group); +} + +TrajectoryUntilNode::~TrajectoryUntilNode() +{ +} + +// Assign the correct type of action client to the variant +bool TrajectoryUntilNode::assign_until_action_client(std::shared_ptr goal) +{ + switch (goal->until_type) { + case TrajectoryUntilAction::Goal::TOOL_CONTACT: + until_action_client_variant = + rclcpp_action::create_client(this, until_action_uri_, clients_callback_group); + break; + + default: + RCLCPP_ERROR(this->get_logger(), "Received unknown until-type."); + return false; + } + + return true; +} + +// Check if the node is capable of accepting a new action goal right now. +rclcpp_action::GoalResponse TrajectoryUntilNode::goal_received_callback( + const rclcpp_action::GoalUUID& /* uuid */, std::shared_ptr goal) +{ + if (server_goal_handle_) { + RCLCPP_ERROR(this->get_logger(), "Node is currently busy, rejecting action goal."); + return rclcpp_action::GoalResponse::REJECT; + } + + if (!assign_until_action_client(goal)) { + RCLCPP_ERROR(this->get_logger(), "Until type not defined, double check the types in the action " + "definition."); + return rclcpp_action::GoalResponse::REJECT; + } + + if (!trajectory_action_client_->wait_for_action_server(std::chrono::seconds(1))) { + RCLCPP_ERROR(this->get_logger(), "Trajectory action server at %s not available.", trajectory_action_uri_.c_str()); + return rclcpp_action::GoalResponse::REJECT; + } + + if (!std::visit([](const auto& client) { return client->wait_for_action_server(std::chrono::seconds(1)); }, + until_action_client_variant)) { + RCLCPP_ERROR(this->get_logger(), "Until action server not available."); + return rclcpp_action::GoalResponse::REJECT; + } + + // Check until action server, send action goal to until-controller and wait for it to be accepted. + if (std::holds_alternative(until_action_client_variant)) { + send_until_goal(goal); + } else { + throw std::runtime_error("Until type not implemented. This should not happen."); + } + + // If it is not accepted within 1 seconds, it is + // assumed to be rejected. + std::unique_lock until_lock(mutex_until); + cv_until_.wait_for(until_lock, 1s); + if (!until_accepted_) { + return rclcpp_action::GoalResponse::REJECT; + } + + // Send action goal to trajectory controller and wait for it to be accepted. If it is not accepted within 1 seconds, + // it is assumed to be rejected. + send_trajectory_goal(goal); + std::unique_lock traj_lock(mutex_trajectory); + cv_until_.wait_for(traj_lock, 1s); + if (!trajectory_accepted_) { + reset_node(); + return rclcpp_action::GoalResponse::REJECT; + } + + return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; +} + +// Save the server goal handle for later use. +void TrajectoryUntilNode::goal_accepted_callback(const std::shared_ptr goal_handle) +{ + server_goal_handle_ = goal_handle; + RCLCPP_INFO(this->get_logger(), "Accepted goal"); + return; +} + +// If the trajectory_until action is canceled, it should cancel the until and trajectory actions, if any are running. +rclcpp_action::CancelResponse +TrajectoryUntilNode::goal_cancelled_callback(const std::shared_ptr /* goal_handle */) +{ + RCLCPP_INFO(this->get_logger(), "Cancel received. Cancelling trajectory-until action."); + if (current_until_goal_handle_) { + cancel_until_goal(); + } + if (current_trajectory_goal_handle_) { + trajectory_action_client_->async_cancel_goal(current_trajectory_goal_handle_); + } + + return rclcpp_action::CancelResponse::ACCEPT; +} + +// Send the trajectory goal, using the relevant fields supplied from the action server +void TrajectoryUntilNode::send_trajectory_goal(std::shared_ptr goal) +{ + auto goal_msg = FollowJointTrajectory::Goal(); + goal_msg.trajectory = goal->trajectory; + goal_msg.goal_time_tolerance = goal->goal_time_tolerance; + goal_msg.goal_tolerance = goal->goal_tolerance; + goal_msg.path_tolerance = goal->path_tolerance; + + auto send_goal_options = rclcpp_action::Client::SendGoalOptions(); + send_goal_options.goal_response_callback = + std::bind(&TrajectoryUntilNode::trajectory_response_callback, this, std::placeholders::_1); + send_goal_options.result_callback = + std::bind(&TrajectoryUntilNode::trajectory_result_callback, this, std::placeholders::_1); + send_goal_options.feedback_callback = + std::bind(&TrajectoryUntilNode::trajectory_feedback_callback, this, std::placeholders::_1, std::placeholders::_2); + trajectory_action_client_->async_send_goal(goal_msg, send_goal_options); +} + +// Send the until goal, using the relevant fields supplied from the action server. +template +void TrajectoryUntilNode::send_until_goal(std::shared_ptr /* goal */) +{ + // Setup goal + auto goal_msg = typename ActionType::Goal(); + auto send_goal_options = typename rclcpp_action::Client::SendGoalOptions(); + send_goal_options.goal_response_callback = + std::bind(&TrajectoryUntilNode::until_response_callback, this, std::placeholders::_1); + send_goal_options.result_callback = + std::bind(&TrajectoryUntilNode::until_result_callback, this, std::placeholders::_1); + + // Send goal + std::get(until_action_client_variant)->async_send_goal(goal_msg, send_goal_options); +} + +void TrajectoryUntilNode::trajectory_response_callback( + const rclcpp_action::ClientGoalHandle::SharedPtr& goal_handle) +{ + { + std::lock_guard guard(mutex_trajectory); + if (goal_handle) { + RCLCPP_INFO(this->get_logger(), "Trajectory accepted by server."); + current_trajectory_goal_handle_ = goal_handle; + trajectory_accepted_ = true; + } else { + RCLCPP_ERROR(this->get_logger(), "Trajectory rejected by server."); + } + } + cv_trajectory_.notify_one(); +} + +template +void TrajectoryUntilNode::until_response_callback( + const typename rclcpp_action::ClientGoalHandle::SharedPtr& goal_handle) +{ + { + std::lock_guard guard(mutex_until); + if (goal_handle) { + RCLCPP_INFO(this->get_logger(), "Until condition accepted by server."); + current_until_goal_handle_ = goal_handle; + until_accepted_ = true; + } else { + RCLCPP_ERROR(this->get_logger(), "Until condition was rejected by server."); + } + } + cv_until_.notify_one(); +} + +// Just forward feedback from trajectory controller. No feedback from until controllers so far. +void TrajectoryUntilNode::trajectory_feedback_callback( + const rclcpp_action::ClientGoalHandle::SharedPtr& /* goal_handle */, + const std::shared_ptr feedback) +{ + if (server_goal_handle_) { + prealloc_fb_->actual = feedback->actual; + prealloc_fb_->desired = feedback->desired; + prealloc_fb_->error = feedback->error; + prealloc_fb_->header = feedback->header; + prealloc_fb_->joint_names = feedback->joint_names; + if (server_goal_handle_) { + server_goal_handle_->publish_feedback(prealloc_fb_); + } + } +} + +// When a result is received from either trajectory or until condition, report it back to user +void TrajectoryUntilNode::trajectory_result_callback(const TrajectoryResult& result) +{ + RCLCPP_INFO(this->get_logger(), "Trajectory result received."); + current_trajectory_goal_handle_.reset(); + report_goal(result); +} + +template +void TrajectoryUntilNode::until_result_callback( + const typename rclcpp_action::ClientGoalHandle::WrappedResult& result) +{ + RCLCPP_INFO(this->get_logger(), "Until result received."); + current_until_goal_handle_.reset(); + report_goal(result); +} + +void TrajectoryUntilNode::report_goal(TrajectoryResult result) +{ + if (server_goal_handle_) { + prealloc_res_->until_condition_result = TrajectoryUntilAction::Result::NOT_TRIGGERED; + prealloc_res_->error_code = result.result->error_code; + prealloc_res_->error_string = result.result->error_string; + switch (result.code) { + case rclcpp_action::ResultCode::SUCCEEDED: + server_goal_handle_->succeed(prealloc_res_); + break; + + case rclcpp_action::ResultCode::ABORTED: + prealloc_res_->error_string += " Trajectory action was aborted. Aborting goal."; + server_goal_handle_->abort(prealloc_res_); + break; + + case rclcpp_action::ResultCode::CANCELED: + prealloc_res_->error_string += " Trajectory action was canceled."; + server_goal_handle_->canceled(prealloc_res_); + break; + + default: + prealloc_res_->error_string += " Unknown result code received from trajectory action, this should not happen. " + "Aborting goal."; + server_goal_handle_->abort(prealloc_res_); + break; + } + if (result.code != rclcpp_action::ResultCode::SUCCEEDED) { + RCLCPP_ERROR(this->get_logger(), "%s", prealloc_res_->error_string.c_str()); + } else { + RCLCPP_INFO(this->get_logger(), "Trajectory finished successfully, did not trigger until condition."); + } + } + reset_node(); +} + +template +void TrajectoryUntilNode::report_goal(UntilResult result) +{ + if (server_goal_handle_) { + switch (result.code) { + case rclcpp_action::ResultCode::SUCCEEDED: + prealloc_res_->error_code = TrajectoryUntilAction::Result::SUCCESSFUL; + prealloc_res_->until_condition_result = TrajectoryUntilAction::Result::TRIGGERED; + prealloc_res_->error_string += "Trajectory finished successfully by triggering until condition."; + server_goal_handle_->succeed(prealloc_res_); + break; + + case rclcpp_action::ResultCode::ABORTED: + prealloc_res_->error_string += "Until action was aborted. Aborting goal."; + server_goal_handle_->abort(prealloc_res_); + break; + + case rclcpp_action::ResultCode::CANCELED: + prealloc_res_->error_string += "Until action was canceled."; + server_goal_handle_->canceled(prealloc_res_); + break; + + default: + prealloc_res_->error_string += "Unknown result code received from until action, this should not happen. " + "Aborting " + "goal."; + server_goal_handle_->abort(prealloc_res_); + + break; + } + if (result.code != rclcpp_action::ResultCode::SUCCEEDED) { + RCLCPP_ERROR(this->get_logger(), "%s", prealloc_res_->error_string.c_str()); + } else { + RCLCPP_INFO(this->get_logger(), "Trajectory finished by triggering until condition."); + } + } + reset_node(); +} + +// Reset all variables and cancel running actions, if any. +void TrajectoryUntilNode::reset_node() +{ + if (current_until_goal_handle_) { + cancel_until_goal(); + } + if (current_trajectory_goal_handle_) { + trajectory_action_client_->async_cancel_goal(current_trajectory_goal_handle_); + } + current_trajectory_goal_handle_ = nullptr; + current_until_goal_handle_ = nullptr; + trajectory_accepted_ = false; + until_accepted_ = false; + prealloc_res_ = std::make_shared(TrajectoryUntilAction::Result()); + prealloc_fb_ = std::make_shared(TrajectoryUntilAction::Feedback()); + + server_goal_handle_ = nullptr; +} + +// Cancel the action contained in the variant, regardless of what type it is. +void TrajectoryUntilNode::cancel_until_goal() +{ + std::visit([this](const auto& until_client) { until_client->async_cancel_goal(current_until_goal_handle_); }, + until_action_client_variant); +} + +} // namespace ur_robot_driver + +int main(int argc, char** argv) +{ + rclcpp::init(argc, argv); + auto node = std::make_shared(); + // Use multithreaded executor because we have two callback groups + rclcpp::executors::MultiThreadedExecutor executor; + executor.add_node(node); + executor.spin(); + rclcpp::shutdown(); + return 0; +} diff --git a/ur_robot_driver/test/integration_test_trajectory_until.py b/ur_robot_driver/test/integration_test_trajectory_until.py new file mode 100644 index 000000000..0d4f3fa37 --- /dev/null +++ b/ur_robot_driver/test/integration_test_trajectory_until.py @@ -0,0 +1,161 @@ +#!/usr/bin/env python +# 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 os +import sys +import time +import unittest + +import pytest + +import launch_testing +import rclpy +from rclpy.node import Node + +from controller_manager_msgs.srv import SwitchController +from ur_msgs.action import FollowJointTrajectoryUntil +from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint +from builtin_interfaces.msg import Duration + +sys.path.append(os.path.dirname(__file__)) +from test_common import ( # noqa: E402 + ControllerManagerInterface, + DashboardInterface, + IoStatusInterface, + ActionInterface, + ROBOT_JOINTS, + TIMEOUT_EXECUTE_TRAJECTORY, + generate_driver_test_description, +) + + +@pytest.mark.launch_test +@launch_testing.parametrize( + "tf_prefix, initial_joint_controller", + [("", "scaled_joint_trajectory_controller"), ("my_ur_", "passthrough_trajectory_controller")], +) +def generate_test_description(tf_prefix, initial_joint_controller): + return generate_driver_test_description( + tf_prefix=tf_prefix, initial_joint_controller=initial_joint_controller + ) + + +class RobotDriverTest(unittest.TestCase): + @classmethod + def setUpClass(cls): + # Initialize the ROS context + rclpy.init() + cls.node = Node("robot_driver_test") + time.sleep(1) + cls.init_robot(cls) + + @classmethod + def tearDownClass(cls): + # Shutdown the ROS context + cls.node.destroy_node() + rclpy.shutdown() + + def init_robot(self): + self._dashboard_interface = DashboardInterface(self.node) + self._controller_manager_interface = ControllerManagerInterface(self.node) + self._io_status_controller_interface = IoStatusInterface(self.node) + self._trajectory_until_interface = ActionInterface( + self.node, "/trajectory_until_node/execute", FollowJointTrajectoryUntil + ) + 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._controller_manager_interface.wait_for_controller("tool_contact_controller") + + def setUp(self): + self._dashboard_interface.start_robot() + time.sleep(1) + self.assertTrue(self._io_status_controller_interface.resend_robot_program().success) + + # + # Tests + # + + def test_trajectory_with_tool_contact_no_trigger_succeeds( + self, tf_prefix, initial_joint_controller + ): + self._controller_manager_interface.wait_for_controller(initial_joint_controller) + self.assertTrue( + self._controller_manager_interface.switch_controller( + strictness=SwitchController.Request.BEST_EFFORT, + activate_controllers=["tool_contact_controller", initial_joint_controller], + ).ok + ) + trajectory = JointTrajectory() + trajectory.joint_names = [tf_prefix + joint for joint in 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_handle = self._trajectory_until_interface.send_goal( + trajectory=trajectory, until_type=FollowJointTrajectoryUntil.Goal.TOOL_CONTACT + ) + self.assertTrue(goal_handle.accepted) + if goal_handle.accepted: + result = self._trajectory_until_interface.get_result( + goal_handle, TIMEOUT_EXECUTE_TRAJECTORY + ) + self.assertEqual(result.error_code, FollowJointTrajectoryUntil.Result.SUCCESSFUL) + self.assertEqual( + result.until_condition_result, FollowJointTrajectoryUntil.Result.NOT_TRIGGERED + ) + + def test_trajectory_until_can_cancel(self, tf_prefix, initial_joint_controller): + self._controller_manager_interface.wait_for_controller(initial_joint_controller) + self.assertTrue( + self._controller_manager_interface.switch_controller( + strictness=SwitchController.Request.BEST_EFFORT, + activate_controllers=["tool_contact_controller", initial_joint_controller], + ).ok + ) + trajectory = JointTrajectory() + trajectory.joint_names = [tf_prefix + joint for joint in 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_handle = self._trajectory_until_interface.send_goal( + trajectory=trajectory, until_type=FollowJointTrajectoryUntil.Goal.TOOL_CONTACT + ) + self.assertTrue(goal_handle.accepted) + result = self._trajectory_until_interface.cancel_goal(goal_handle) + self.assertTrue(len(result.goals_canceling) > 0) diff --git a/ur_robot_driver/test/test_common.py b/ur_robot_driver/test/test_common.py index 0826d8091..41bc50d08 100644 --- a/ur_robot_driver/test/test_common.py +++ b/ur_robot_driver/test/test_common.py @@ -67,6 +67,7 @@ TIMEOUT_WAIT_SERVICE = 10 TIMEOUT_WAIT_SERVICE_INITIAL = 120 # If we download the docker image simultaneously to the tests, it can take quite some time until the dashboard server is reachable and usable. TIMEOUT_WAIT_ACTION = 10 +TIMEOUT_EXECUTE_TRAJECTORY = 30 TIMEOUT_EXECUTE_TRAJECTORY = 30 From 580e1ffed16ec36371193847f3b8466b24736b78 Mon Sep 17 00:00:00 2001 From: "mergify[bot]" <37929162+mergify[bot]@users.noreply.github.com> Date: Tue, 21 Oct 2025 09:59:38 +0200 Subject: [PATCH 104/133] Auto-update pre-commit hooks (backport of #1527) (#1535) --- .pre-commit-config.yaml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml index 85cfa8e9e..5df34ac41 100644 --- a/.pre-commit-config.yaml +++ b/.pre-commit-config.yaml @@ -33,7 +33,7 @@ repos: # Python hooks - repo: https://github.com/asottile/pyupgrade - rev: v3.20.0 + rev: v3.21.0 hooks: - id: pyupgrade args: [--py36-plus] From b73c919858b3fa8269b465b3268060b0fde4b281 Mon Sep 17 00:00:00 2001 From: "mergify[bot]" <37929162+mergify[bot]@users.noreply.github.com> Date: Wed, 22 Oct 2025 16:02:47 +0200 Subject: [PATCH 105/133] Add support for UR18 (backport #1524) (#1525) --- README.md | 2 +- ur_bringup/config/ur18_update_rate.yaml | 3 + ur_bringup/launch/ur18.launch.py | 102 ++++++ ur_bringup/launch/ur_control.launch.py | 1 + ur_moveit_config/launch/ur_moveit.launch.py | 1 + ur_robot_driver/config/ur18_update_rate.yaml | 3 + ur_robot_driver/doc/usage/startup.rst | 2 +- ur_robot_driver/launch/ur18.launch.py | 101 ++++++ ur_robot_driver/launch/ur_control.launch.py | 1 + ur_robot_driver/test/test_common.py | 1 + ur_robot_driver/urdf/ur.ros2_control.xacro | 312 ------------------- 11 files changed, 215 insertions(+), 314 deletions(-) create mode 100644 ur_bringup/config/ur18_update_rate.yaml create mode 100644 ur_bringup/launch/ur18.launch.py create mode 100644 ur_robot_driver/config/ur18_update_rate.yaml create mode 100644 ur_robot_driver/launch/ur18.launch.py delete mode 100644 ur_robot_driver/urdf/ur.ros2_control.xacro diff --git a/README.md b/README.md index 9ce977bcf..5157db806 100644 --- a/README.md +++ b/README.md @@ -124,7 +124,7 @@ For getting started, you'll basically need three steps: details. ```bash - # Replace ur5e with one of ur3, ur5, ur10, ur3e, ur5e, ur7e, ur10e, ur12e, ur16e, ur8long, ur15, ur20, ur30 + # Replace ur5e with one of ur3, ur5, ur10, ur3e, ur5e, ur7e, ur10e, ur12e, ur16e, ur8long, ur15, ur18, ur20, ur30 # Replace the IP address with the IP address of your actual robot / URSim ros2 launch ur_robot_driver ur_control.launch.py ur_type:=ur5e robot_ip:=192.168.56.101 ``` diff --git a/ur_bringup/config/ur18_update_rate.yaml b/ur_bringup/config/ur18_update_rate.yaml new file mode 100644 index 000000000..66ef3d736 --- /dev/null +++ b/ur_bringup/config/ur18_update_rate.yaml @@ -0,0 +1,3 @@ +controller_manager: + ros__parameters: + update_rate: 500 # Hz diff --git a/ur_bringup/launch/ur18.launch.py b/ur_bringup/launch/ur18.launch.py new file mode 100644 index 000000000..2b804bc77 --- /dev/null +++ b/ur_bringup/launch/ur18.launch.py @@ -0,0 +1,102 @@ +# Copyright (c) 2021 PickNik, Inc. +# +# 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. + +# +# Author: Denis Stogl + +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import LaunchConfiguration, ThisLaunchFileDir + + +def generate_launch_description(): + # Declare arguments + declared_arguments = [] + declared_arguments.append( + DeclareLaunchArgument( + "robot_ip", + description="IP address by which the robot can be reached.", + ) + ) + declared_arguments.append( + DeclareLaunchArgument( + "use_fake_hardware", + default_value="false", + description="Start robot with fake hardware mirroring command to its states.", + ) + ) + declared_arguments.append( + DeclareLaunchArgument( + "fake_sensor_commands", + default_value="false", + description="Enable fake command interfaces for sensors used for simple simulations. \ + Used only if 'use_fake_hardware' parameter is true.", + ) + ) + declared_arguments.append( + DeclareLaunchArgument( + "initial_joint_controller", + default_value="scaled_joint_trajectory_controller", + description="Initially loaded robot controller.", + choices=[ + "scaled_joint_trajectory_controller", + "joint_trajectory_controller", + "forward_velocity_controller", + "forward_position_controller", + ], + ) + ) + declared_arguments.append( + DeclareLaunchArgument( + "activate_joint_controller", + default_value="true", + description="Activate loaded joint controller.", + ) + ) + + # Initialize Arguments + robot_ip = LaunchConfiguration("robot_ip") + use_fake_hardware = LaunchConfiguration("use_fake_hardware") + fake_sensor_commands = LaunchConfiguration("fake_sensor_commands") + initial_joint_controller = LaunchConfiguration("initial_joint_controller") + activate_joint_controller = LaunchConfiguration("activate_joint_controller") + + base_launch = IncludeLaunchDescription( + PythonLaunchDescriptionSource([ThisLaunchFileDir(), "/ur_control.launch.py"]), + launch_arguments={ + "ur_type": "ur18", + "robot_ip": robot_ip, + "use_fake_hardware": use_fake_hardware, + "fake_sensor_commands": fake_sensor_commands, + "initial_joint_controller": initial_joint_controller, + "activate_joint_controller": activate_joint_controller, + }.items(), + ) + + return LaunchDescription(declared_arguments + [base_launch]) diff --git a/ur_bringup/launch/ur_control.launch.py b/ur_bringup/launch/ur_control.launch.py index 90a5f57a3..5af282305 100644 --- a/ur_bringup/launch/ur_control.launch.py +++ b/ur_bringup/launch/ur_control.launch.py @@ -62,6 +62,7 @@ def generate_launch_description(): "ur16e", "ur8long", "ur15", + "ur18", "ur20", "ur30", ], diff --git a/ur_moveit_config/launch/ur_moveit.launch.py b/ur_moveit_config/launch/ur_moveit.launch.py index fe4447a77..af4d94c29 100644 --- a/ur_moveit_config/launch/ur_moveit.launch.py +++ b/ur_moveit_config/launch/ur_moveit.launch.py @@ -295,6 +295,7 @@ def generate_launch_description(): "ur16e", "ur8long", "ur15", + "ur18", "ur20", "ur30", ], diff --git a/ur_robot_driver/config/ur18_update_rate.yaml b/ur_robot_driver/config/ur18_update_rate.yaml new file mode 100644 index 000000000..66ef3d736 --- /dev/null +++ b/ur_robot_driver/config/ur18_update_rate.yaml @@ -0,0 +1,3 @@ +controller_manager: + ros__parameters: + update_rate: 500 # Hz diff --git a/ur_robot_driver/doc/usage/startup.rst b/ur_robot_driver/doc/usage/startup.rst index f8c2024ab..fb16578f9 100644 --- a/ur_robot_driver/doc/usage/startup.rst +++ b/ur_robot_driver/doc/usage/startup.rst @@ -23,7 +23,7 @@ nodes for UR robots. The only required arguments are the ``ur_type`` and ``robot $ ros2 launch ur_robot_driver ur_control.launch.py ur_type:=ur5e robot_ip:=192.168.56.101 Allowed ``ur_type`` strings: ``ur3``, ``ur5``, ``ur10``, ``ur3e``, ``ur5e``, ``ur7e``, ``ur10e``, -``ur12e``, ``ur16e``, ``ur8long``, ``ur15``, ``ur20``, ``ur30``. +``ur12e``, ``ur16e``, ``ur8long``, ``ur15``, ``ur18``, ``ur20``, ``ur30``. Other important arguments are: diff --git a/ur_robot_driver/launch/ur18.launch.py b/ur_robot_driver/launch/ur18.launch.py new file mode 100644 index 000000000..abd444d68 --- /dev/null +++ b/ur_robot_driver/launch/ur18.launch.py @@ -0,0 +1,101 @@ +# Copyright (c) 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. + +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import LaunchConfiguration, ThisLaunchFileDir + + +def generate_launch_description(): + # Declare arguments + declared_arguments = [] + declared_arguments.append( + DeclareLaunchArgument( + "robot_ip", + description="IP address by which the robot can be reached.", + ) + ) + declared_arguments.append( + DeclareLaunchArgument( + "use_mock_hardware", + default_value="false", + description="Start robot with mock hardware mirroring command to its states.", + ) + ) + declared_arguments.append( + DeclareLaunchArgument( + "mock_sensor_commands", + default_value="false", + description="Enable mock command interfaces for sensors used for simple simulations. " + "Used only if 'use_mock_hardware' parameter is true.", + ) + ) + declared_arguments.append( + DeclareLaunchArgument( + "initial_joint_controller", + default_value="scaled_joint_trajectory_controller", + description="Initially loaded robot controller.", + choices=[ + "scaled_joint_trajectory_controller", + "joint_trajectory_controller", + "forward_velocity_controller", + "forward_position_controller", + "freedrive_mode_controller", + "passthrough_trajectory_controller", + ], + ) + ) + declared_arguments.append( + DeclareLaunchArgument( + "activate_joint_controller", + default_value="true", + description="Activate loaded joint controller.", + ) + ) + + # Initialize Arguments + robot_ip = LaunchConfiguration("robot_ip") + use_mock_hardware = LaunchConfiguration("use_mock_hardware") + mock_sensor_commands = LaunchConfiguration("mock_sensor_commands") + initial_joint_controller = LaunchConfiguration("initial_joint_controller") + activate_joint_controller = LaunchConfiguration("activate_joint_controller") + + base_launch = IncludeLaunchDescription( + PythonLaunchDescriptionSource([ThisLaunchFileDir(), "/ur_control.launch.py"]), + launch_arguments={ + "ur_type": "ur18", + "robot_ip": robot_ip, + "use_mock_hardware": use_mock_hardware, + "mock_sensor_commands": mock_sensor_commands, + "initial_joint_controller": initial_joint_controller, + "activate_joint_controller": activate_joint_controller, + }.items(), + ) + + return LaunchDescription(declared_arguments + [base_launch]) diff --git a/ur_robot_driver/launch/ur_control.launch.py b/ur_robot_driver/launch/ur_control.launch.py index 49f4f33f7..b93280d06 100644 --- a/ur_robot_driver/launch/ur_control.launch.py +++ b/ur_robot_driver/launch/ur_control.launch.py @@ -424,6 +424,7 @@ def generate_launch_description(): "ur16e", "ur8long", "ur15", + "ur18", "ur20", "ur30", ], diff --git a/ur_robot_driver/test/test_common.py b/ur_robot_driver/test/test_common.py index 41bc50d08..a256dbf7a 100644 --- a/ur_robot_driver/test/test_common.py +++ b/ur_robot_driver/test/test_common.py @@ -405,6 +405,7 @@ def _declare_launch_arguments(): "ur16e", "ur8long", "ur15", + "ur18", "ur20", "ur30", ], diff --git a/ur_robot_driver/urdf/ur.ros2_control.xacro b/ur_robot_driver/urdf/ur.ros2_control.xacro deleted file mode 100644 index b480434b0..000000000 --- a/ur_robot_driver/urdf/ur.ros2_control.xacro +++ /dev/null @@ -1,312 +0,0 @@ - - - - - - - - - - - - - - - - - - - - mock_components/GenericSystem - ${mock_sensor_commands} - 0.0 - true - - - ur_robot_driver/URPositionHardwareInterface - ${robot_ip} - ${script_filename} - ${output_recipe_filename} - ${input_recipe_filename} - ${headless_mode} - ${reverse_port} - ${script_sender_port} - ${reverse_ip} - ${script_command_port} - ${trajectory_port} - ${tf_prefix} - ${non_blocking_read} - 2000 - 0.03 - ${use_tool_communication} - ${kinematics_hash} - ${tool_voltage} - ${tool_parity} - ${tool_baud_rate} - ${tool_stop_bits} - ${tool_rx_idle_chars} - ${tool_tx_idle_chars} - ${tool_device_name} - ${tool_tcp_port} - ${robot_receive_timeout} - - - - - - - - - - - 1.0 - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - 1 - - - - - - - - From d364e1870e79db665ac917b05840608eeb39b80e Mon Sep 17 00:00:00 2001 From: "mergify[bot]" <37929162+mergify[bot]@users.noreply.github.com> Date: Thu, 23 Oct 2025 13:42:19 +0200 Subject: [PATCH 106/133] Add missing update_rate config files for UR7e and UR12e (backport of #1544) (#1547) --- ur_robot_driver/config/ur12e_update_rate.yaml | 3 +++ ur_robot_driver/config/ur7e_update_rate.yaml | 3 +++ 2 files changed, 6 insertions(+) create mode 100644 ur_robot_driver/config/ur12e_update_rate.yaml create mode 100644 ur_robot_driver/config/ur7e_update_rate.yaml diff --git a/ur_robot_driver/config/ur12e_update_rate.yaml b/ur_robot_driver/config/ur12e_update_rate.yaml new file mode 100644 index 000000000..66ef3d736 --- /dev/null +++ b/ur_robot_driver/config/ur12e_update_rate.yaml @@ -0,0 +1,3 @@ +controller_manager: + ros__parameters: + update_rate: 500 # Hz diff --git a/ur_robot_driver/config/ur7e_update_rate.yaml b/ur_robot_driver/config/ur7e_update_rate.yaml new file mode 100644 index 000000000..66ef3d736 --- /dev/null +++ b/ur_robot_driver/config/ur7e_update_rate.yaml @@ -0,0 +1,3 @@ +controller_manager: + ros__parameters: + update_rate: 500 # Hz From 2f628f8bfbb6913cd1134012994b15cd8a1e9b94 Mon Sep 17 00:00:00 2001 From: Felix Exner Date: Thu, 23 Oct 2025 14:26:38 +0200 Subject: [PATCH 107/133] Prepare changelogs for upcoming release --- ur/CHANGELOG.rst | 3 +++ ur_bringup/CHANGELOG.rst | 5 +++++ ur_calibration/CHANGELOG.rst | 3 +++ ur_controllers/CHANGELOG.rst | 3 +++ ur_dashboard_msgs/CHANGELOG.rst | 3 +++ ur_moveit_config/CHANGELOG.rst | 5 +++++ ur_robot_driver/CHANGELOG.rst | 11 +++++++++++ 7 files changed, 33 insertions(+) diff --git a/ur/CHANGELOG.rst b/ur/CHANGELOG.rst index 2894bc31d..16d905b48 100644 --- a/ur/CHANGELOG.rst +++ b/ur/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package ur ^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 2.9.0 (2025-09-16) ------------------ diff --git a/ur_bringup/CHANGELOG.rst b/ur_bringup/CHANGELOG.rst index a1d90d7bd..739ac314b 100644 --- a/ur_bringup/CHANGELOG.rst +++ b/ur_bringup/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package ur_bringup ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Add support for UR18 (backport `#1524 `_) (`#1525 `_) +* Contributors: mergify[bot] + 2.9.0 (2025-09-16) ------------------ * Add support for UR8 Long (`#1491 `_) diff --git a/ur_calibration/CHANGELOG.rst b/ur_calibration/CHANGELOG.rst index 00108108f..3429467bf 100644 --- a/ur_calibration/CHANGELOG.rst +++ b/ur_calibration/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package ur_calibration ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 2.9.0 (2025-09-16) ------------------ * Fix ur_calibration compilation on Windows (backport of `#1400 `_) (`#1408 `_) diff --git a/ur_controllers/CHANGELOG.rst b/ur_controllers/CHANGELOG.rst index 8b06f8d50..1965e22d9 100644 --- a/ur_controllers/CHANGELOG.rst +++ b/ur_controllers/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package ur_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 2.9.0 (2025-09-16) ------------------ * ur_controllers: Fix compilation on Windows (backport `#1402 `_) (`#1412 `_) diff --git a/ur_dashboard_msgs/CHANGELOG.rst b/ur_dashboard_msgs/CHANGELOG.rst index 8b1de132e..ba6f9bee7 100644 --- a/ur_dashboard_msgs/CHANGELOG.rst +++ b/ur_dashboard_msgs/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package ur_dashboard_msgs ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 2.9.0 (2025-09-16) ------------------ * Added 'is in remote control' call as a dashboard service (backport of `#1433 `_) (`#1436 `_) diff --git a/ur_moveit_config/CHANGELOG.rst b/ur_moveit_config/CHANGELOG.rst index db4dbeff0..2aaec4f0d 100644 --- a/ur_moveit_config/CHANGELOG.rst +++ b/ur_moveit_config/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package ur_moveit_config ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Add support for UR18 (backport `#1524 `_) (`#1525 `_) +* Contributors: mergify[bot] + 2.9.0 (2025-09-16) ------------------ * Add support for UR8 Long (`#1491 `_) diff --git a/ur_robot_driver/CHANGELOG.rst b/ur_robot_driver/CHANGELOG.rst index 0a0c61614..565ccb602 100644 --- a/ur_robot_driver/CHANGELOG.rst +++ b/ur_robot_driver/CHANGELOG.rst @@ -1,3 +1,14 @@ +Forthcoming +----------- +* Add missing update_rate config files for UR7e and UR12e (backport of `#1544 `_) (`#1547 `_) +* Add support for UR18 (backport `#1524 `_) (`#1525 `_) +* Trajectory until node (backport of `#1461 `_) (`#1522 `_) +* Wait for used controllers in test setup (backport `#1519 `_) (`#1520 `_) +* Running integration tests with mock hardware (backport `#1226 `_) (`#1508 `_) +* Fix link to limits man page (`#1518 `_) +* Add test for hardware component lifecycle (backport `#1476 `_) (`#1506 `_) +* Contributors: Felix Exner, mergify[bot] + 2.9.0 (2025-09-16) ------------------ * Add support for UR8 Long (`#1491 `_) From 4c5b87ada1c71cd5f73c5cef8b83590a0c910b25 Mon Sep 17 00:00:00 2001 From: Felix Exner Date: Thu, 23 Oct 2025 14:26:57 +0200 Subject: [PATCH 108/133] 2.10.0 --- ur/CHANGELOG.rst | 4 ++-- ur/package.xml | 2 +- ur_bringup/CHANGELOG.rst | 4 ++-- ur_bringup/package.xml | 2 +- ur_calibration/CHANGELOG.rst | 4 ++-- ur_calibration/package.xml | 2 +- ur_controllers/CHANGELOG.rst | 4 ++-- ur_controllers/package.xml | 2 +- ur_dashboard_msgs/CHANGELOG.rst | 4 ++-- ur_dashboard_msgs/package.xml | 2 +- ur_moveit_config/CHANGELOG.rst | 4 ++-- ur_moveit_config/package.xml | 2 +- ur_robot_driver/CHANGELOG.rst | 4 ++-- ur_robot_driver/package.xml | 2 +- 14 files changed, 21 insertions(+), 21 deletions(-) diff --git a/ur/CHANGELOG.rst b/ur/CHANGELOG.rst index 16d905b48..1761c7ea2 100644 --- a/ur/CHANGELOG.rst +++ b/ur/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package ur ^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.10.0 (2025-10-23) +------------------- 2.9.0 (2025-09-16) ------------------ diff --git a/ur/package.xml b/ur/package.xml index a567618ca..b323603ea 100644 --- a/ur/package.xml +++ b/ur/package.xml @@ -2,7 +2,7 @@ ur - 2.9.0 + 2.10.0 Metapackage for universal robots Felix Exner diff --git a/ur_bringup/CHANGELOG.rst b/ur_bringup/CHANGELOG.rst index 739ac314b..241283b2f 100644 --- a/ur_bringup/CHANGELOG.rst +++ b/ur_bringup/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package ur_bringup ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.10.0 (2025-10-23) +------------------- * Add support for UR18 (backport `#1524 `_) (`#1525 `_) * Contributors: mergify[bot] diff --git a/ur_bringup/package.xml b/ur_bringup/package.xml index 2384decd7..afd4f87b4 100644 --- a/ur_bringup/package.xml +++ b/ur_bringup/package.xml @@ -2,7 +2,7 @@ ur_bringup - 2.9.0 + 2.10.0 Launch file and run-time configurations, e.g. controllers. Felix Exner diff --git a/ur_calibration/CHANGELOG.rst b/ur_calibration/CHANGELOG.rst index 3429467bf..68eb120aa 100644 --- a/ur_calibration/CHANGELOG.rst +++ b/ur_calibration/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package ur_calibration ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.10.0 (2025-10-23) +------------------- 2.9.0 (2025-09-16) ------------------ diff --git a/ur_calibration/package.xml b/ur_calibration/package.xml index 2a7b4abdf..0af695a78 100644 --- a/ur_calibration/package.xml +++ b/ur_calibration/package.xml @@ -1,7 +1,7 @@ ur_calibration - 2.9.0 + 2.10.0 Package for extracting the factory calibration from a UR robot and change it such that it can be used by ur_description to gain a correct URDF Felix Exner diff --git a/ur_controllers/CHANGELOG.rst b/ur_controllers/CHANGELOG.rst index 1965e22d9..7beb750f6 100644 --- a/ur_controllers/CHANGELOG.rst +++ b/ur_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package ur_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.10.0 (2025-10-23) +------------------- 2.9.0 (2025-09-16) ------------------ diff --git a/ur_controllers/package.xml b/ur_controllers/package.xml index 75027f855..e7da41559 100644 --- a/ur_controllers/package.xml +++ b/ur_controllers/package.xml @@ -2,7 +2,7 @@ ur_controllers - 2.9.0 + 2.10.0 Provides controllers that use the speed scaling interface of Universal Robots. Felix Exner diff --git a/ur_dashboard_msgs/CHANGELOG.rst b/ur_dashboard_msgs/CHANGELOG.rst index ba6f9bee7..50cf589ad 100644 --- a/ur_dashboard_msgs/CHANGELOG.rst +++ b/ur_dashboard_msgs/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package ur_dashboard_msgs ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.10.0 (2025-10-23) +------------------- 2.9.0 (2025-09-16) ------------------ diff --git a/ur_dashboard_msgs/package.xml b/ur_dashboard_msgs/package.xml index b07ab7f7f..65e706254 100644 --- a/ur_dashboard_msgs/package.xml +++ b/ur_dashboard_msgs/package.xml @@ -2,7 +2,7 @@ ur_dashboard_msgs - 2.9.0 + 2.10.0 Messages around the UR Dashboard server. Felix Exner diff --git a/ur_moveit_config/CHANGELOG.rst b/ur_moveit_config/CHANGELOG.rst index 2aaec4f0d..e6b3ec243 100644 --- a/ur_moveit_config/CHANGELOG.rst +++ b/ur_moveit_config/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package ur_moveit_config ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.10.0 (2025-10-23) +------------------- * Add support for UR18 (backport `#1524 `_) (`#1525 `_) * Contributors: mergify[bot] diff --git a/ur_moveit_config/package.xml b/ur_moveit_config/package.xml index 9724ca9b4..477c26b8e 100644 --- a/ur_moveit_config/package.xml +++ b/ur_moveit_config/package.xml @@ -2,7 +2,7 @@ ur_moveit_config - 2.9.0 + 2.10.0 An example package with MoveIt2 configurations for UR robots. diff --git a/ur_robot_driver/CHANGELOG.rst b/ur_robot_driver/CHANGELOG.rst index 565ccb602..1e7c9eaf6 100644 --- a/ur_robot_driver/CHANGELOG.rst +++ b/ur_robot_driver/CHANGELOG.rst @@ -1,5 +1,5 @@ -Forthcoming ------------ +2.10.0 (2025-10-23) +------------------- * Add missing update_rate config files for UR7e and UR12e (backport of `#1544 `_) (`#1547 `_) * Add support for UR18 (backport `#1524 `_) (`#1525 `_) * Trajectory until node (backport of `#1461 `_) (`#1522 `_) diff --git a/ur_robot_driver/package.xml b/ur_robot_driver/package.xml index 620b57775..20fa158bd 100644 --- a/ur_robot_driver/package.xml +++ b/ur_robot_driver/package.xml @@ -2,7 +2,7 @@ ur_robot_driver - 2.9.0 + 2.10.0 The new driver for Universal Robots UR3, UR5 and UR10 robots with CB3 controllers and the e-series. Felix Exner From 0876dc7fc0fd11594854325fe60a3067f5470736 Mon Sep 17 00:00:00 2001 From: "mergify[bot]" <37929162+mergify[bot]@users.noreply.github.com> Date: Wed, 5 Nov 2025 10:05:59 +0100 Subject: [PATCH 109/133] Bump actions/upload-artifact from 4 to 5 (backport #1552) (#1556) --- .github/workflows/coverage-build.yml | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/.github/workflows/coverage-build.yml b/.github/workflows/coverage-build.yml index 2b1fe5d71..55dcd17a3 100644 --- a/.github/workflows/coverage-build.yml +++ b/.github/workflows/coverage-build.yml @@ -47,7 +47,8 @@ jobs: file: ros_ws/lcov/total_coverage.info flags: unittests name: codecov-umbrella - - uses: actions/upload-artifact@v1 + token: ${{ secrets.CODECOV_TOKEN }} + - uses: actions/upload-artifact@v5 with: name: colcon-logs-${{ matrix.os }} path: ros_ws/log From 13b4e4fe0090e970994764f38cbbf5f179ad33fd Mon Sep 17 00:00:00 2001 From: "mergify[bot]" <37929162+mergify[bot]@users.noreply.github.com> Date: Wed, 5 Nov 2025 15:20:23 +0100 Subject: [PATCH 110/133] Add effort command interface to hardware interface (backport #1411) (#1528) --- ur_robot_driver/config/ur_controllers.yaml | 14 ++ .../ur_robot_driver/hardware_interface.hpp | 4 + ur_robot_driver/launch/ur_control.launch.py | 1 + ur_robot_driver/package.xml | 1 + ur_robot_driver/src/hardware_interface.cpp | 132 +++++++++++++----- .../integration_test_controller_switch.py | 8 ++ 6 files changed, 128 insertions(+), 32 deletions(-) diff --git a/ur_robot_driver/config/ur_controllers.yaml b/ur_robot_driver/config/ur_controllers.yaml index 333069294..51ed65438 100644 --- a/ur_robot_driver/config/ur_controllers.yaml +++ b/ur_robot_driver/config/ur_controllers.yaml @@ -21,6 +21,9 @@ controller_manager: forward_velocity_controller: type: velocity_controllers/JointGroupVelocityController + forward_effort_controller: + type: effort_controllers/JointGroupEffortController + forward_position_controller: type: position_controllers/JointGroupPositionController @@ -154,6 +157,17 @@ forward_velocity_controller: - $(var tf_prefix)wrist_3_joint interface_name: velocity +forward_effort_controller: + ros__parameters: + joints: + - $(var tf_prefix)shoulder_pan_joint + - $(var tf_prefix)shoulder_lift_joint + - $(var tf_prefix)elbow_joint + - $(var tf_prefix)wrist_1_joint + - $(var tf_prefix)wrist_2_joint + - $(var tf_prefix)wrist_3_joint + interface_name: effort + forward_position_controller: ros__parameters: joints: diff --git a/ur_robot_driver/include/ur_robot_driver/hardware_interface.hpp b/ur_robot_driver/include/ur_robot_driver/hardware_interface.hpp index e6cc00c86..be2e40bb1 100644 --- a/ur_robot_driver/include/ur_robot_driver/hardware_interface.hpp +++ b/ur_robot_driver/include/ur_robot_driver/hardware_interface.hpp @@ -82,6 +82,7 @@ enum StoppingInterface STOP_FORCE_MODE, STOP_FREEDRIVE, STOP_TOOL_CONTACT, + STOP_TORQUE, }; // We define our own quaternion to use it as a buffer, since we need to pass pointers to the state @@ -177,6 +178,7 @@ class URPositionHardwareInterface : public hardware_interface::SystemInterface urcl::vector6d_t urcl_position_commands_; urcl::vector6d_t urcl_position_commands_old_; urcl::vector6d_t urcl_velocity_commands_; + urcl::vector6d_t urcl_torque_commands_; urcl::vector6d_t urcl_joint_positions_; urcl::vector6d_t urcl_joint_velocities_; urcl::vector6d_t urcl_joint_efforts_; @@ -229,6 +231,7 @@ class URPositionHardwareInterface : public hardware_interface::SystemInterface bool initialized_; double system_interface_initialized_; std::atomic_bool async_thread_shutdown_; + urcl::VersionInformation version_info_; double get_robot_software_version_major_; double get_robot_software_version_minor_; double get_robot_software_version_bugfix_; @@ -305,6 +308,7 @@ class URPositionHardwareInterface : public hardware_interface::SystemInterface std::vector> start_modes_; bool position_controller_running_; bool velocity_controller_running_; + bool torque_controller_running_; bool force_mode_controller_running_ = false; std::unique_ptr ur_driver_; diff --git a/ur_robot_driver/launch/ur_control.launch.py b/ur_robot_driver/launch/ur_control.launch.py index b93280d06..d4afd0782 100644 --- a/ur_robot_driver/launch/ur_control.launch.py +++ b/ur_robot_driver/launch/ur_control.launch.py @@ -372,6 +372,7 @@ def controller_spawner(controllers, active=True): "joint_trajectory_controller", "forward_velocity_controller", "forward_position_controller", + "forward_effort_controller", "force_mode_controller", "passthrough_trajectory_controller", "freedrive_mode_controller", diff --git a/ur_robot_driver/package.xml b/ur_robot_driver/package.xml index 20fa158bd..c531bf24a 100644 --- a/ur_robot_driver/package.xml +++ b/ur_robot_driver/package.xml @@ -46,6 +46,7 @@ ur_description ur_msgs + effort_controllers force_torque_sensor_broadcaster joint_state_broadcaster joint_state_publisher diff --git a/ur_robot_driver/src/hardware_interface.cpp b/ur_robot_driver/src/hardware_interface.cpp index a051148b8..2cccb1426 100644 --- a/ur_robot_driver/src/hardware_interface.cpp +++ b/ur_robot_driver/src/hardware_interface.cpp @@ -60,37 +60,50 @@ namespace ur_robot_driver URPositionHardwareInterface::URPositionHardwareInterface() { mode_compatibility_[hardware_interface::HW_IF_POSITION][hardware_interface::HW_IF_VELOCITY] = false; + mode_compatibility_[hardware_interface::HW_IF_POSITION][hardware_interface::HW_IF_EFFORT] = false; mode_compatibility_[hardware_interface::HW_IF_POSITION][FORCE_MODE_GPIO] = false; mode_compatibility_[hardware_interface::HW_IF_POSITION][PASSTHROUGH_GPIO] = false; mode_compatibility_[hardware_interface::HW_IF_POSITION][FREEDRIVE_MODE_GPIO] = false; mode_compatibility_[hardware_interface::HW_IF_POSITION][TOOL_CONTACT_GPIO] = true; mode_compatibility_[hardware_interface::HW_IF_VELOCITY][hardware_interface::HW_IF_POSITION] = false; + mode_compatibility_[hardware_interface::HW_IF_VELOCITY][hardware_interface::HW_IF_EFFORT] = false; mode_compatibility_[hardware_interface::HW_IF_VELOCITY][FORCE_MODE_GPIO] = false; mode_compatibility_[hardware_interface::HW_IF_VELOCITY][PASSTHROUGH_GPIO] = false; mode_compatibility_[hardware_interface::HW_IF_VELOCITY][FREEDRIVE_MODE_GPIO] = false; mode_compatibility_[hardware_interface::HW_IF_VELOCITY][TOOL_CONTACT_GPIO] = true; + mode_compatibility_[hardware_interface::HW_IF_EFFORT][hardware_interface::HW_IF_POSITION] = false; + mode_compatibility_[hardware_interface::HW_IF_EFFORT][hardware_interface::HW_IF_VELOCITY] = false; + mode_compatibility_[hardware_interface::HW_IF_EFFORT][FORCE_MODE_GPIO] = false; + mode_compatibility_[hardware_interface::HW_IF_EFFORT][PASSTHROUGH_GPIO] = false; + mode_compatibility_[hardware_interface::HW_IF_EFFORT][FREEDRIVE_MODE_GPIO] = false; + mode_compatibility_[hardware_interface::HW_IF_EFFORT][TOOL_CONTACT_GPIO] = true; + mode_compatibility_[FORCE_MODE_GPIO][hardware_interface::HW_IF_POSITION] = false; mode_compatibility_[FORCE_MODE_GPIO][hardware_interface::HW_IF_VELOCITY] = false; + mode_compatibility_[FORCE_MODE_GPIO][hardware_interface::HW_IF_EFFORT] = false; mode_compatibility_[FORCE_MODE_GPIO][PASSTHROUGH_GPIO] = true; mode_compatibility_[FORCE_MODE_GPIO][FREEDRIVE_MODE_GPIO] = false; mode_compatibility_[FORCE_MODE_GPIO][TOOL_CONTACT_GPIO] = false; mode_compatibility_[PASSTHROUGH_GPIO][hardware_interface::HW_IF_POSITION] = false; mode_compatibility_[PASSTHROUGH_GPIO][hardware_interface::HW_IF_VELOCITY] = false; + mode_compatibility_[PASSTHROUGH_GPIO][hardware_interface::HW_IF_EFFORT] = false; mode_compatibility_[PASSTHROUGH_GPIO][FORCE_MODE_GPIO] = true; mode_compatibility_[PASSTHROUGH_GPIO][FREEDRIVE_MODE_GPIO] = false; mode_compatibility_[PASSTHROUGH_GPIO][TOOL_CONTACT_GPIO] = true; mode_compatibility_[FREEDRIVE_MODE_GPIO][hardware_interface::HW_IF_POSITION] = false; mode_compatibility_[FREEDRIVE_MODE_GPIO][hardware_interface::HW_IF_VELOCITY] = false; + mode_compatibility_[FREEDRIVE_MODE_GPIO][hardware_interface::HW_IF_EFFORT] = false; mode_compatibility_[FREEDRIVE_MODE_GPIO][FORCE_MODE_GPIO] = false; mode_compatibility_[FREEDRIVE_MODE_GPIO][PASSTHROUGH_GPIO] = false; mode_compatibility_[FREEDRIVE_MODE_GPIO][TOOL_CONTACT_GPIO] = false; mode_compatibility_[TOOL_CONTACT_GPIO][hardware_interface::HW_IF_POSITION] = true; mode_compatibility_[TOOL_CONTACT_GPIO][hardware_interface::HW_IF_VELOCITY] = true; + mode_compatibility_[TOOL_CONTACT_GPIO][hardware_interface::HW_IF_EFFORT] = true; mode_compatibility_[TOOL_CONTACT_GPIO][FORCE_MODE_GPIO] = false; mode_compatibility_[TOOL_CONTACT_GPIO][PASSTHROUGH_GPIO] = true; mode_compatibility_[TOOL_CONTACT_GPIO][FREEDRIVE_MODE_GPIO] = false; @@ -120,6 +133,7 @@ URPositionHardwareInterface::on_init(const hardware_interface::HardwareInfo& sys urcl_velocity_commands_ = { { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 } }; position_controller_running_ = false; velocity_controller_running_ = false; + torque_controller_running_ = false; freedrive_mode_controller_running_ = false; passthrough_trajectory_controller_running_ = false; tool_contact_controller_running_ = false; @@ -143,51 +157,55 @@ URPositionHardwareInterface::on_init(const hardware_interface::HardwareInfo& sys trajectory_joint_accelerations_.reserve(32768); for (const hardware_interface::ComponentInfo& joint : info_.joints) { - if (joint.command_interfaces.size() != 2) { + auto has_cmd_interface = [](const hardware_interface::ComponentInfo& joint, const std::string& interface_name) { + auto it = find_if( + joint.command_interfaces.begin(), joint.command_interfaces.end(), + [&interface_name](const hardware_interface::InterfaceInfo& obj) { return obj.name == interface_name; }); + return it != joint.command_interfaces.end(); + }; + + if (!has_cmd_interface(joint, hardware_interface::HW_IF_POSITION)) { RCLCPP_FATAL(rclcpp::get_logger("URPositionHardwareInterface"), - "Joint '%s' has %zu command interfaces found. 2 expected.", joint.name.c_str(), - joint.command_interfaces.size()); + "Joint '%s' does not contain a '%s' command interface.", joint.name.c_str(), + hardware_interface::HW_IF_POSITION); return hardware_interface::CallbackReturn::ERROR; } - if (joint.command_interfaces[0].name != hardware_interface::HW_IF_POSITION) { + if (!has_cmd_interface(joint, hardware_interface::HW_IF_VELOCITY)) { RCLCPP_FATAL(rclcpp::get_logger("URPositionHardwareInterface"), - "Joint '%s' have %s command interfaces found as first command interface. '%s' expected.", - joint.name.c_str(), joint.command_interfaces[0].name.c_str(), hardware_interface::HW_IF_POSITION); + "Joint '%s' does not contain a '%s' command interface.", joint.name.c_str(), + hardware_interface::HW_IF_VELOCITY); return hardware_interface::CallbackReturn::ERROR; } - if (joint.command_interfaces[1].name != hardware_interface::HW_IF_VELOCITY) { - RCLCPP_FATAL(rclcpp::get_logger("URPositionHardwareInterface"), - "Joint '%s' have %s command interfaces found as second command interface. '%s' expected.", - joint.name.c_str(), joint.command_interfaces[1].name.c_str(), hardware_interface::HW_IF_VELOCITY); - return hardware_interface::CallbackReturn::ERROR; - } + // We treat the torque command interface as optional here for now. This should ensure backwards + // compatibility with older descriptions. - if (joint.state_interfaces.size() != 3) { - RCLCPP_FATAL(rclcpp::get_logger("URPositionHardwareInterface"), "Joint '%s' has %zu state interface. 3 expected.", - joint.name.c_str(), joint.state_interfaces.size()); - return hardware_interface::CallbackReturn::ERROR; - } + auto has_state_interface = [](const hardware_interface::ComponentInfo& joint, const std::string& interface_name) { + auto it = find_if( + joint.state_interfaces.begin(), joint.state_interfaces.end(), + [&interface_name](const hardware_interface::InterfaceInfo& obj) { return obj.name == interface_name; }); + return it != joint.state_interfaces.end(); + }; - if (joint.state_interfaces[0].name != hardware_interface::HW_IF_POSITION) { + if (!has_state_interface(joint, hardware_interface::HW_IF_POSITION)) { RCLCPP_FATAL(rclcpp::get_logger("URPositionHardwareInterface"), - "Joint '%s' have %s state interface as first state interface. '%s' expected.", joint.name.c_str(), - joint.state_interfaces[0].name.c_str(), hardware_interface::HW_IF_POSITION); + "Joint '%s' does not contain a '%s' state interface.", joint.name.c_str(), + hardware_interface::HW_IF_POSITION); return hardware_interface::CallbackReturn::ERROR; } - if (joint.state_interfaces[1].name != hardware_interface::HW_IF_VELOCITY) { + if (!has_state_interface(joint, hardware_interface::HW_IF_VELOCITY)) { RCLCPP_FATAL(rclcpp::get_logger("URPositionHardwareInterface"), - "Joint '%s' have %s state interface as second state interface. '%s' expected.", joint.name.c_str(), - joint.state_interfaces[1].name.c_str(), hardware_interface::HW_IF_VELOCITY); + "Joint '%s' does not contain a '%s' state interface.", joint.name.c_str(), + hardware_interface::HW_IF_VELOCITY); return hardware_interface::CallbackReturn::ERROR; } - if (joint.state_interfaces[2].name != hardware_interface::HW_IF_EFFORT) { + if (!has_state_interface(joint, hardware_interface::HW_IF_EFFORT)) { RCLCPP_FATAL(rclcpp::get_logger("URPositionHardwareInterface"), - "Joint '%s' have %s state interface as third state interface. '%s' expected.", joint.name.c_str(), - joint.state_interfaces[2].name.c_str(), hardware_interface::HW_IF_EFFORT); + "Joint '%s' does not contain a '%s' state interface.", joint.name.c_str(), + hardware_interface::HW_IF_EFFORT); return hardware_interface::CallbackReturn::ERROR; } } @@ -321,6 +339,12 @@ std::vector URPositionHardwareInterface::exp std::vector URPositionHardwareInterface::export_command_interfaces() { + auto has_cmd_interface = [](const hardware_interface::ComponentInfo& joint, const std::string& interface_name) { + auto it = + find_if(joint.command_interfaces.begin(), joint.command_interfaces.end(), + [&interface_name](const hardware_interface::InterfaceInfo& obj) { return obj.name == interface_name; }); + return it != joint.command_interfaces.end(); + }; std::vector command_interfaces; for (size_t i = 0; i < info_.joints.size(); ++i) { command_interfaces.emplace_back(hardware_interface::CommandInterface( @@ -328,6 +352,11 @@ std::vector URPositionHardwareInterface::e command_interfaces.emplace_back(hardware_interface::CommandInterface( info_.joints[i].name, hardware_interface::HW_IF_VELOCITY, &urcl_velocity_commands_[i])); + + if (has_cmd_interface(info_.joints[i], hardware_interface::HW_IF_EFFORT)) { + command_interfaces.emplace_back(hardware_interface::CommandInterface( + info_.joints[i].name, hardware_interface::HW_IF_EFFORT, &urcl_torque_commands_[i])); + } } // Obtain the tf_prefix from the urdf so that we can have the general interface multiple times // NOTE using the tf_prefix at this point is some kind of workaround. One should actually go through the list of gpio @@ -626,11 +655,11 @@ URPositionHardwareInterface::on_configure(const rclcpp_lifecycle::State& previou } // Export version information to state interfaces - urcl::VersionInformation version_info = ur_driver_->getVersion(); - get_robot_software_version_major_ = version_info.major; - get_robot_software_version_minor_ = version_info.minor; - get_robot_software_version_build_ = version_info.build; - get_robot_software_version_bugfix_ = version_info.bugfix; + version_info_ = ur_driver_->getVersion(); + get_robot_software_version_major_ = version_info_.major; + get_robot_software_version_minor_ = version_info_.minor; + get_robot_software_version_build_ = version_info_.build; + get_robot_software_version_bugfix_ = version_info_.bugfix; async_thread_ = std::make_shared(&URPositionHardwareInterface::asyncThread, this); @@ -807,6 +836,7 @@ hardware_interface::return_type URPositionHardwareInterface::read(const rclcpp:: // initialize commands urcl_position_commands_ = urcl_position_commands_old_ = urcl_joint_positions_; urcl_velocity_commands_ = { { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 } }; + urcl_torque_commands_ = { { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 } }; target_speed_fraction_cmd_ = NO_NEW_CMD_; resend_robot_program_cmd_ = NO_NEW_CMD_; zero_ftsensor_cmd_ = NO_NEW_CMD_; @@ -841,7 +871,8 @@ hardware_interface::return_type URPositionHardwareInterface::write(const rclcpp: } else if (velocity_controller_running_) { ur_driver_->writeJointCommand(urcl_velocity_commands_, urcl::comm::ControlMode::MODE_SPEEDJ, receive_timeout_); - + } else if (torque_controller_running_) { + ur_driver_->writeJointCommand(urcl_torque_commands_, urcl::comm::ControlMode::MODE_TORQUE, receive_timeout_); } else if (freedrive_mode_controller_running_ && freedrive_activated_) { ur_driver_->writeFreedriveControlMessage(urcl::control::FreedriveControlMessage::FREEDRIVE_NOOP); @@ -1119,6 +1150,9 @@ hardware_interface::return_type URPositionHardwareInterface::prepare_command_mod if (velocity_controller_running_) { control_modes[i] = { hardware_interface::HW_IF_VELOCITY }; } + if (torque_controller_running_) { + control_modes[i] = { hardware_interface::HW_IF_EFFORT }; + } if (force_mode_controller_running_) { control_modes[i].push_back(FORCE_MODE_GPIO); } @@ -1155,6 +1189,7 @@ hardware_interface::return_type URPositionHardwareInterface::prepare_command_mod const std::vector> start_modes_to_check{ { info_.joints[i].name + "/" + hardware_interface::HW_IF_POSITION, hardware_interface::HW_IF_POSITION }, { info_.joints[i].name + "/" + hardware_interface::HW_IF_VELOCITY, hardware_interface::HW_IF_VELOCITY }, + { info_.joints[i].name + "/" + hardware_interface::HW_IF_EFFORT, hardware_interface::HW_IF_EFFORT }, { tf_prefix + FORCE_MODE_GPIO + "/type", FORCE_MODE_GPIO }, { tf_prefix + PASSTHROUGH_GPIO + "/setpoint_positions_" + std::to_string(i), PASSTHROUGH_GPIO }, { tf_prefix + FREEDRIVE_MODE_GPIO + "/async_success", FREEDRIVE_MODE_GPIO }, @@ -1173,6 +1208,21 @@ hardware_interface::return_type URPositionHardwareInterface::prepare_command_mod } } + // effort control is only available from 5.23.0 / 10.10.0 + if (std::any_of(start_modes_.begin(), start_modes_.end(), [&](const std::vector& modes) { + return std::any_of(modes.begin(), modes.end(), + [&](const std::string& mode) { return mode == hardware_interface::HW_IF_EFFORT; }); + })) { + if ((version_info_.major == 5 && version_info_.minor < 23) || + (version_info_.major == 10 && version_info_.minor < 10) || version_info_.major < 5) { + RCLCPP_ERROR(rclcpp::get_logger("URPositionHardwareInterface"), "Requested to use effort interface on a robot " + "version that doesn't support it. Torque " + "control is available from robot software 5.23.0 " + "/ 10.10.0 on."); + return hardware_interface::return_type::ERROR; + } + } + if (!std::all_of(start_modes_.begin() + 1, start_modes_.end(), [&](const std::vector& other) { return other == start_modes_[0]; })) { RCLCPP_ERROR(rclcpp::get_logger("URPositionHardwareInterface"), "Start modes of all joints have to be the same."); @@ -1188,6 +1238,8 @@ hardware_interface::return_type URPositionHardwareInterface::prepare_command_mod StoppingInterface::STOP_POSITION }, { info_.joints[i].name + "/" + hardware_interface::HW_IF_VELOCITY, hardware_interface::HW_IF_VELOCITY, StoppingInterface::STOP_VELOCITY }, + { info_.joints[i].name + "/" + hardware_interface::HW_IF_EFFORT, hardware_interface::HW_IF_EFFORT, + StoppingInterface::STOP_TORQUE }, { tf_prefix + FORCE_MODE_GPIO + "/disable_cmd", FORCE_MODE_GPIO, StoppingInterface::STOP_FORCE_MODE }, { tf_prefix + PASSTHROUGH_GPIO + "/setpoint_positions_" + std::to_string(i), PASSTHROUGH_GPIO, StoppingInterface::STOP_PASSTHROUGH }, @@ -1233,6 +1285,11 @@ hardware_interface::return_type URPositionHardwareInterface::perform_command_mod velocity_controller_running_ = false; urcl_velocity_commands_ = { { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 } }; } + if (stop_modes_[0].size() != 0 && + std::find(stop_modes_[0].begin(), stop_modes_[0].end(), StoppingInterface::STOP_TORQUE) != stop_modes_[0].end()) { + torque_controller_running_ = false; + urcl_torque_commands_ = { { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 } }; + } if (stop_modes_[0].size() != 0 && std::find(stop_modes_[0].begin(), stop_modes_[0].end(), StoppingInterface::STOP_FORCE_MODE) != stop_modes_[0].end()) { force_mode_controller_running_ = false; @@ -1263,6 +1320,7 @@ hardware_interface::return_type URPositionHardwareInterface::perform_command_mod if (start_modes_.size() != 0 && std::find(start_modes_[0].begin(), start_modes_[0].end(), hardware_interface::HW_IF_POSITION) != start_modes_[0].end()) { velocity_controller_running_ = false; + torque_controller_running_ = false; passthrough_trajectory_controller_running_ = false; urcl_position_commands_ = urcl_position_commands_old_ = urcl_joint_positions_; position_controller_running_ = true; @@ -1270,9 +1328,17 @@ hardware_interface::return_type URPositionHardwareInterface::perform_command_mod } else if (start_modes_[0].size() != 0 && std::find(start_modes_[0].begin(), start_modes_[0].end(), hardware_interface::HW_IF_VELOCITY) != start_modes_[0].end()) { position_controller_running_ = false; + torque_controller_running_ = false; passthrough_trajectory_controller_running_ = false; urcl_velocity_commands_ = { { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 } }; velocity_controller_running_ = true; + } else if (start_modes_[0].size() != 0 && std::find(start_modes_[0].begin(), start_modes_[0].end(), + hardware_interface::HW_IF_EFFORT) != start_modes_[0].end()) { + position_controller_running_ = false; + velocity_controller_running_ = false; + torque_controller_running_ = true; + passthrough_trajectory_controller_running_ = false; + urcl_torque_commands_ = { { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 } }; } if (start_modes_[0].size() != 0 && std::find(start_modes_[0].begin(), start_modes_[0].end(), FORCE_MODE_GPIO) != start_modes_[0].end()) { @@ -1282,6 +1348,7 @@ hardware_interface::return_type URPositionHardwareInterface::perform_command_mod std::find(start_modes_[0].begin(), start_modes_[0].end(), PASSTHROUGH_GPIO) != start_modes_[0].end()) { velocity_controller_running_ = false; position_controller_running_ = false; + torque_controller_running_ = false; passthrough_trajectory_controller_running_ = true; passthrough_trajectory_abort_ = 0.0; } @@ -1289,6 +1356,7 @@ hardware_interface::return_type URPositionHardwareInterface::perform_command_mod std::find(start_modes_[0].begin(), start_modes_[0].end(), FREEDRIVE_MODE_GPIO) != start_modes_[0].end()) { velocity_controller_running_ = false; position_controller_running_ = false; + torque_controller_running_ = false; freedrive_mode_controller_running_ = true; freedrive_activated_ = false; } diff --git a/ur_robot_driver/test/integration_test_controller_switch.py b/ur_robot_driver/test/integration_test_controller_switch.py index 8bfd8ac7b..9e66c510b 100644 --- a/ur_robot_driver/test/integration_test_controller_switch.py +++ b/ur_robot_driver/test/integration_test_controller_switch.py @@ -219,6 +219,14 @@ def test_activating_controller_with_running_position_controller_fails(self): ], ).ok ) + self.assertFalse( + self._controller_manager_interface.switch_controller( + strictness=SwitchController.Request.STRICT, + activate_controllers=[ + "forward_effort_controller", + ], + ).ok + ) self.assertFalse( self._controller_manager_interface.switch_controller( strictness=SwitchController.Request.STRICT, From c6f92fd70f96b09e6a27dbcec64d4e80c8528681 Mon Sep 17 00:00:00 2001 From: "mergify[bot]" <37929162+mergify[bot]@users.noreply.github.com> Date: Wed, 12 Nov 2025 09:50:45 +0100 Subject: [PATCH 111/133] Fix flaky tests (backport #1559) (#1566) Wait for controller to become active Otherwise there is a race condition between sending the program and starting the tests. --- .../test/integration_test_scaled_joint_controller.py | 4 ++++ ur_robot_driver/test/integration_test_trajectory_until.py | 4 +++- 2 files changed, 7 insertions(+), 1 deletion(-) diff --git a/ur_robot_driver/test/integration_test_scaled_joint_controller.py b/ur_robot_driver/test/integration_test_scaled_joint_controller.py index 073acac63..e496276c0 100755 --- a/ur_robot_driver/test/integration_test_scaled_joint_controller.py +++ b/ur_robot_driver/test/integration_test_scaled_joint_controller.py @@ -97,6 +97,10 @@ def setUp(self): time.sleep(1) self.assertTrue(self._io_status_controller_interface.resend_robot_program().success) + self._controller_manager_interface.wait_for_controller( + "scaled_joint_trajectory_controller", "active" + ) + # # Test functions # diff --git a/ur_robot_driver/test/integration_test_trajectory_until.py b/ur_robot_driver/test/integration_test_trajectory_until.py index 0d4f3fa37..0fbddfcf8 100644 --- a/ur_robot_driver/test/integration_test_trajectory_until.py +++ b/ur_robot_driver/test/integration_test_trajectory_until.py @@ -95,11 +95,13 @@ def init_robot(self): self._controller_manager_interface.wait_for_controller("tool_contact_controller") - def setUp(self): + def setUp(self, initial_joint_controller): self._dashboard_interface.start_robot() time.sleep(1) self.assertTrue(self._io_status_controller_interface.resend_robot_program().success) + self._controller_manager_interface.wait_for_controller(initial_joint_controller, "active") + # # Tests # From 87a85801f2f44c5e33729b40343bf1015a6ec494 Mon Sep 17 00:00:00 2001 From: "mergify[bot]" <37929162+mergify[bot]@users.noreply.github.com> Date: Thu, 13 Nov 2025 12:47:48 +0100 Subject: [PATCH 112/133] Auto-update pre-commit hooks (backport #1565) (#1571) --- .pre-commit-config.yaml | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml index 5df34ac41..0e388cddc 100644 --- a/.pre-commit-config.yaml +++ b/.pre-commit-config.yaml @@ -33,7 +33,7 @@ repos: # Python hooks - repo: https://github.com/asottile/pyupgrade - rev: v3.21.0 + rev: v3.21.1 hooks: - id: pyupgrade args: [--py36-plus] @@ -87,7 +87,7 @@ repos: args: ["--linelength=120"] - repo: https://github.com/pre-commit/mirrors-clang-format - rev: 'v21.1.2' + rev: 'v21.1.5' hooks: - id: clang-format From a4b1692b1394f1580a1493d3df79c753f293cb79 Mon Sep 17 00:00:00 2001 From: "mergify[bot]" <37929162+mergify[bot]@users.noreply.github.com> Date: Thu, 13 Nov 2025 12:48:32 +0100 Subject: [PATCH 113/133] Dashboard client polyscopex (backport #1546) (#1562) * Dashboard client polyscopex (#1546) * Support PolyScopeX Robot API through the dashboard client * Add resume service to docs * Increase default dashboard client timeout to 2 seconds (#1564) The default timeout of 1 second can collide with certain actions. For example, the power_off command on PolyScope 5 takes 1 sec almost exactly until it sends the answer. With that, the timeout was right on the edge and led to a flaky integration test. Defaulting to 2 seconds should prevent those tests from failing randomly. --- ur_robot_driver/doc/ROS_INTERFACE.md | 5 + .../ur_robot_driver/dashboard_client_ros.hpp | 74 +++- ur_robot_driver/src/dashboard_client_ros.cpp | 349 ++++++++---------- ur_robot_driver/test/dashboard_client.py | 1 + ur_robot_driver/test/test_common.py | 1 + 5 files changed, 230 insertions(+), 200 deletions(-) diff --git a/ur_robot_driver/doc/ROS_INTERFACE.md b/ur_robot_driver/doc/ROS_INTERFACE.md index 00c7082a4..9f269ee51 100644 --- a/ur_robot_driver/doc/ROS_INTERFACE.md +++ b/ur_robot_driver/doc/ROS_INTERFACE.md @@ -190,6 +190,11 @@ General purpose service to send arbitrary messages to the dashboard server Used when robot gets a safety fault or violation to restart the safety. After safety has been rebooted the robot will be in Power Off. NOTE: You should always ensure it is okay to restart the system. It is highly recommended to check the error log before using this command (either via PolyScope or e.g. ssh connection). +##### resume ([std_srvs/Trigger](http://docs.ros.org/en/rolling/p/std_srvs/srv/Trigger.html)) + +Resume a paused program on a PolyScope X robot. This service is only available on a PolyScope X +robot. + ##### shutdown ([std_srvs/Trigger](https://docs.ros.org/en/humble/p/std_srvs/srv/Trigger.html)) Shutdown the robot controller diff --git a/ur_robot_driver/include/ur_robot_driver/dashboard_client_ros.hpp b/ur_robot_driver/include/ur_robot_driver/dashboard_client_ros.hpp index 7fb4c913f..1f12ca5dc 100644 --- a/ur_robot_driver/include/ur_robot_driver/dashboard_client_ros.hpp +++ b/ur_robot_driver/include/ur_robot_driver/dashboard_client_ros.hpp @@ -85,16 +85,19 @@ class DashboardClientROS virtual ~DashboardClientROS() = default; private: - inline rclcpp::Service::SharedPtr createDashboardTriggerSrv(const std::string& topic, - const std::string& command, - const std::string& expected) + inline rclcpp::Service::SharedPtr + createDashboardTriggerSrv(const std::string& topic, std::function command) { rclcpp::Service::SharedPtr service = node_->create_service( - topic, [&, command, expected](const std::shared_ptr req, - const std::shared_ptr resp) { + topic, [command](const std::shared_ptr req, + const std::shared_ptr resp) { try { - resp->message = this->client_.sendAndReceive(command); - resp->success = std::regex_match(resp->message, std::regex(expected)); + auto response = command(); + resp->message = response.message; + if (response.data.find("status_code") != response.data.end()) { + resp->message += ", status_code: " + std::to_string(std::get(response.data["status_code"])); + } + resp->success = response.ok; } catch (const urcl::UrException& e) { RCLCPP_ERROR(rclcpp::get_logger("Dashboard_Client"), "Service Call failed: '%s'", e.what()); resp->message = e.what(); @@ -104,6 +107,60 @@ class DashboardClientROS return service; } + template + urcl::DashboardResponse dashboardCallWithChecks(std::function dashboard_call, + SrvResponseT resp) + { + urcl::DashboardResponse dashboard_response; + try { + dashboard_response = dashboard_call(); + resp->success = dashboard_response.ok; + resp->answer = dashboard_response.message; + } catch (const urcl::NotImplementedException& e) { + RCLCPP_ERROR(rclcpp::get_logger("Dashboard_Client"), "This service call seems not to be implemented (for this " + "robot version)."); + resp->answer = "Not implemented for this robot software version."; + resp->success = false; + } catch (const urcl::UrException& e) { + RCLCPP_ERROR(rclcpp::get_logger("Dashboard_Client"), "Service Call failed: '%s'", e.what()); + resp->answer = e.what(); + resp->success = false; + return dashboard_response; + } + return dashboard_response; + } + + template + void handleDashboardResponseData(std::function fun, SrvResponseT& resp, + const urcl::DashboardResponse& dashboard_response) + { + try { + fun(); + } catch (const std::bad_variant_access& e) { + RCLCPP_ERROR(rclcpp::get_logger("Dashboard_Client"), "Service Call failed: '%s'", e.what()); + std::ostringstream oss; + for (const auto& [key, value] : dashboard_response.data) { + oss << key << ": "; + std::visit([&oss](const auto& arg) { oss << arg; }, value); + oss << "\n"; + } + RCLCPP_INFO(rclcpp::get_logger("Dashboard_Client"), "Available data:\n%s", oss.str().c_str()); + resp->answer = e.what(); + resp->success = false; + } catch (const std::out_of_range& e) { + RCLCPP_ERROR(rclcpp::get_logger("Dashboard_Client"), "Service Call failed: '%s'", e.what()); + std::ostringstream oss; + for (const auto& [key, value] : dashboard_response.data) { + oss << key << ": "; + std::visit([&oss](const auto& arg) { oss << arg; }, value); + oss << "\n"; + } + RCLCPP_INFO(rclcpp::get_logger("Dashboard_Client"), "Available data:\n%s", oss.str().c_str()); + resp->answer = e.what(); + resp->success = false; + } + } + bool handleRunningQuery(ur_dashboard_msgs::srv::IsProgramRunning::Request::SharedPtr req, ur_dashboard_msgs::srv::IsProgramRunning::Response::SharedPtr resp); @@ -122,7 +179,7 @@ class DashboardClientROS bool connect(); std::shared_ptr node_; - urcl::DashboardClient client_; + std::unique_ptr client_; urcl::comm::INotifier notifier_; urcl::primary_interface::PrimaryClient primary_client_; @@ -134,6 +191,7 @@ class DashboardClientROS rclcpp::Service::SharedPtr close_safety_popup_service_; rclcpp::Service::SharedPtr pause_service_; rclcpp::Service::SharedPtr play_service_; + rclcpp::Service::SharedPtr resume_service_; rclcpp::Service::SharedPtr power_off_service_; rclcpp::Service::SharedPtr power_on_service_; rclcpp::Service::SharedPtr restart_safety_service_; diff --git a/ur_robot_driver/src/dashboard_client_ros.cpp b/ur_robot_driver/src/dashboard_client_ros.cpp index 3a729c8c0..93e85a9d2 100644 --- a/ur_robot_driver/src/dashboard_client_ros.cpp +++ b/ur_robot_driver/src/dashboard_client_ros.cpp @@ -39,7 +39,9 @@ #include #include +#include +#include #include #include @@ -47,64 +49,91 @@ namespace ur_robot_driver { DashboardClientROS::DashboardClientROS(const rclcpp::Node::SharedPtr& node, const std::string& robot_ip) - : node_(node), client_(robot_ip), primary_client_(robot_ip, notifier_) + : node_(node), primary_client_(robot_ip, notifier_) { - node_->declare_parameter("receive_timeout", 1); + node_->declare_parameter("receive_timeout", 2); primary_client_.start(10, std::chrono::seconds(10)); auto robot_version = primary_client_.getRobotVersion(); + primary_client_.stop(); + auto dashboard_policy = urcl::DashboardClient::ClientPolicy::G5; if (robot_version->major > 5) { - throw(urcl::UrException("The dashboard server is only available for CB3 and e-Series robots.")); + if (robot_version->major == 10 && robot_version->minor < 11) { + RCLCPP_FATAL(node_->get_logger(), + "The dashboard server for PolyScope X is only available from version 10.11.0 and later. The " + "connected robot has version %s. Exiting now.", + robot_version->toString().c_str()); + exit(1); + } + dashboard_policy = urcl::DashboardClient::ClientPolicy::POLYSCOPE_X; } + RCLCPP_INFO(node_->get_logger(), "Connecting to Dashboard Server at %s with policy %s", robot_ip.c_str(), + dashboard_policy == urcl::DashboardClient::ClientPolicy::G5 ? "G5" : "Polyscope X"); + client_ = std::make_unique(robot_ip, dashboard_policy); + connect(); // Service to release the brakes. If the robot is currently powered off, it will get powered on on the fly. - brake_release_service_ = createDashboardTriggerSrv("~/brake_release", "brake release\n", "Brake releasing"); + brake_release_service_ = createDashboardTriggerSrv( + "~/brake_release", std::bind(&urcl::DashboardClient::commandBrakeReleaseWithResponse, client_.get())); // If this service is called the operational mode can again be changed from PolyScope, and the user password is // enabled. - clear_operational_mode_service_ = createDashboardTriggerSrv("~/clear_operational_mode", "clear operational mode\n", - "No longer controlling the operational mode\\. " - "Current " - "operational mode: " - "'(MANUAL|AUTOMATIC)'\\."); + clear_operational_mode_service_ = createDashboardTriggerSrv( + "~/clear_operational_mode", + std::bind(&urcl::DashboardClient::commandClearOperationalModeWithResponse, client_.get())); // Close a (non-safety) popup on the teach pendant. - close_popup_service_ = createDashboardTriggerSrv("~/close_popup", "close popup\n", "closing popup"); + close_popup_service_ = createDashboardTriggerSrv( + "~/close_popup", std::bind(&urcl::DashboardClient::commandClosePopupWithResponse, client_.get())); // Close a safety popup on the teach pendant. - close_safety_popup_service_ = - createDashboardTriggerSrv("~/close_safety_popup", "close safety popup\n", "closing safety popup"); + close_safety_popup_service_ = createDashboardTriggerSrv( + "~/close_safety_popup", std::bind(&urcl::DashboardClient::commandCloseSafetyPopupWithResponse, client_.get())); // Pause a running program. - pause_service_ = createDashboardTriggerSrv("~/pause", "pause\n", "Pausing program"); + pause_service_ = + createDashboardTriggerSrv("~/pause", std::bind(&urcl::DashboardClient::commandPauseWithResponse, client_.get())); // Start execution of a previously loaded program - play_service_ = createDashboardTriggerSrv("~/play", "play\n", "Starting program"); + play_service_ = + createDashboardTriggerSrv("~/play", std::bind(&urcl::DashboardClient::commandPlayWithResponse, client_.get())); + + if (dashboard_policy == urcl::DashboardClient::ClientPolicy::POLYSCOPE_X) { + resume_service_ = createDashboardTriggerSrv( + "~/resume", std::bind(&urcl::DashboardClient::commandResumeWithResponse, client_.get())); + } // Power off the robot motors - power_off_service_ = createDashboardTriggerSrv("~/power_off", "power off\n", "Powering off"); + power_off_service_ = createDashboardTriggerSrv( + "~/power_off", std::bind(&urcl::DashboardClient::commandPowerOffWithResponse, client_.get())); // Power on the robot motors. To fully start the robot, call 'brake_release' afterwards. - power_on_service_ = createDashboardTriggerSrv("~/power_on", "power on\n", "Powering on"); + power_on_service_ = + createDashboardTriggerSrv("~/power_on", std::bind(&urcl::DashboardClient::commandPowerOnWithResponse, + client_.get(), std::chrono::seconds(300))); // Used when robot gets a safety fault or violation to restart the safety. After safety has been rebooted the robot // will be in Power Off. NOTE: You should always ensure it is okay to restart the system. It is highly recommended to // check the error log before using this command (either via PolyScope or e.g. ssh connection). - restart_safety_service_ = createDashboardTriggerSrv("~/restart_safety", "restart safety\n", "Restarting safety"); + restart_safety_service_ = createDashboardTriggerSrv( + "~/restart_safety", std::bind(&urcl::DashboardClient::commandRestartSafetyWithResponse, client_.get())); // Shutdown the robot controller - shutdown_service_ = createDashboardTriggerSrv("~/shutdown", "shutdown\n", "Shutting down"); + shutdown_service_ = createDashboardTriggerSrv( + "~/shutdown", std::bind(&urcl::DashboardClient::commandShutdownWithResponse, client_.get())); // Stop program execution on the robot - stop_service_ = createDashboardTriggerSrv("~/stop", "stop\n", "Stopped"); + stop_service_ = + createDashboardTriggerSrv("~/stop", std::bind(&urcl::DashboardClient::commandStopWithResponse, client_.get())); // Dismiss a protective stop to continue robot movements. NOTE: It is the responsibility of the user to ensure the // cause of the protective stop is resolved before calling this service. - unlock_protective_stop_service_ = - createDashboardTriggerSrv("~/unlock_protective_stop", "unlock protective stop\n", "Protective stop releasing"); + unlock_protective_stop_service_ = createDashboardTriggerSrv( + "~/unlock_protective_stop", + std::bind(&urcl::DashboardClient::commandUnlockProtectiveStopWithResponse, client_.get())); // Query whether there is currently a program running running_service_ = node_->create_service( @@ -115,18 +144,14 @@ DashboardClientROS::DashboardClientROS(const rclcpp::Node::SharedPtr& node, cons get_loaded_program_service_ = node_->create_service( "~/get_loaded_program", [&](const ur_dashboard_msgs::srv::GetLoadedProgram::Request::SharedPtr req, ur_dashboard_msgs::srv::GetLoadedProgram::Response::SharedPtr resp) { - try { - resp->answer = this->client_.sendAndReceive("get loaded program\n"); - std::smatch match; - std::regex expected("Loaded program: (.+)"); - resp->success = std::regex_match(resp->answer, match, expected); - if (resp->success) { - resp->program_name = match[1]; - } - } catch (const urcl::UrException& e) { - RCLCPP_ERROR(rclcpp::get_logger("Dashboard_Client"), "Service Call failed: '%s'", e.what()); - resp->answer = e.what(); - resp->success = false; + auto dashboard_response = + dashboardCallWithChecks([this, req]() { return client_->commandGetLoadedProgramWithResponse(); }, resp); + if (resp->success) { + handleDashboardResponseData( + [dashboard_response, resp]() { + resp->program_name = std::get(dashboard_response.data.at("program_name")); + }, + resp, dashboard_response); } return true; }); @@ -135,14 +160,8 @@ DashboardClientROS::DashboardClientROS(const rclcpp::Node::SharedPtr& node, cons load_installation_service_ = node_->create_service( "~/load_installation", [&](const ur_dashboard_msgs::srv::Load::Request::SharedPtr req, ur_dashboard_msgs::srv::Load::Response::SharedPtr resp) { - try { - resp->answer = this->client_.sendAndReceive("load installation " + req->filename + "\n"); - resp->success = std::regex_match(resp->answer, std::regex("Loading installation: .+")); - } catch (const urcl::UrException& e) { - RCLCPP_ERROR(rclcpp::get_logger("Dashboard_Client"), "Service Call failed: '%s'", e.what()); - resp->answer = e.what(); - resp->success = false; - } + auto dashboard_response = dashboardCallWithChecks( + [this, req]() { return client_->commandLoadInstallationWithResponse(req->filename); }, resp); return true; }); @@ -150,13 +169,10 @@ DashboardClientROS::DashboardClientROS(const rclcpp::Node::SharedPtr& node, cons load_program_service_ = node->create_service( "~/load_program", [&](const ur_dashboard_msgs::srv::Load::Request::SharedPtr req, ur_dashboard_msgs::srv::Load::Response::SharedPtr resp) { - try { - resp->answer = this->client_.sendAndReceive("load " + req->filename + "\n"); - resp->success = std::regex_match(resp->answer, std::regex("Loading program: .+")); - } catch (const urcl::UrException& e) { - RCLCPP_ERROR(rclcpp::get_logger("Dashboard_Client"), "Service Call failed: '%s'", e.what()); - resp->answer = e.what(); - resp->success = false; + auto dashboard_response = dashboardCallWithChecks( + [this, req]() { return client_->commandLoadProgramWithResponse(req->filename); }, resp); + if (dashboard_response.data.find("status_code") != dashboard_response.data.end()) { + resp->answer += ", status_code: " + std::to_string(std::get(dashboard_response.data["status_code"])); } return true; }); @@ -170,14 +186,9 @@ DashboardClientROS::DashboardClientROS(const rclcpp::Node::SharedPtr& node, cons popup_service_ = node_->create_service( "~/popup", [&](ur_dashboard_msgs::srv::Popup::Request::SharedPtr req, ur_dashboard_msgs::srv::Popup::Response::SharedPtr resp) { - try { - resp->answer = this->client_.sendAndReceive("popup " + req->message + "\n"); - resp->success = std::regex_match(resp->answer, std::regex("showing popup")); - } catch (const urcl::UrException& e) { - RCLCPP_ERROR(rclcpp::get_logger("Dashboard_Client"), "Service Call failed: '%s'", e.what()); - resp->answer = e.what(); - resp->success = false; - } + auto dashboard_response = + dashboardCallWithChecks([this, req]() { return client_->commandPopupWithResponse(req->message); }, resp); + return true; }); @@ -185,19 +196,19 @@ DashboardClientROS::DashboardClientROS(const rclcpp::Node::SharedPtr& node, cons program_state_service_ = node_->create_service( "~/program_state", [&](const ur_dashboard_msgs::srv::GetProgramState::Request::SharedPtr /*unused*/, ur_dashboard_msgs::srv::GetProgramState::Response::SharedPtr resp) { - try { - resp->answer = this->client_.sendAndReceive("programState\n"); - std::smatch match; - std::regex expected("(STOPPED|PLAYING|PAUSED) (.+)"); - resp->success = std::regex_match(resp->answer, match, expected); - if (resp->success) { - resp->state.state = match[1]; - resp->program_name = match[2]; - } - } catch (const urcl::UrException& e) { - RCLCPP_ERROR(rclcpp::get_logger("Dashboard_Client"), "Service Call failed: '%s'", e.what()); - resp->answer = e.what(); - resp->success = false; + auto dashboard_response = + dashboardCallWithChecks([this]() { return client_->commandProgramStateWithResponse(); }, resp); + + if (resp->success) { + handleDashboardResponseData( + [dashboard_response, resp]() { + resp->state.state = std::get(dashboard_response.data.at("program_state")); + // PolyScope X doesn't report the program name + if (dashboard_response.data.find("program_name") != dashboard_response.data.end()) { + resp->program_name = std::get(dashboard_response.data.at("program_name")); + } + }, + resp, dashboard_response); } return true; }); @@ -216,14 +227,7 @@ DashboardClientROS::DashboardClientROS(const rclcpp::Node::SharedPtr& node, cons add_to_log_service_ = node->create_service( "~/add_to_log", [&](const ur_dashboard_msgs::srv::AddToLog::Request::SharedPtr req, ur_dashboard_msgs::srv::AddToLog::Response::SharedPtr resp) { - try { - resp->answer = this->client_.sendAndReceive("addToLog " + req->message + "\n"); - resp->success = std::regex_match(resp->answer, std::regex("(Added log message|No log message to add)")); - } catch (const urcl::UrException& e) { - RCLCPP_ERROR(rclcpp::get_logger("Dashboard_Client"), "Service Call failed: '%s'", e.what()); - resp->answer = e.what(); - resp->success = false; - } + dashboardCallWithChecks([this, req]() { return client_->commandAddToLogWithResponse(req->message); }, resp); return true; }); @@ -232,7 +236,7 @@ DashboardClientROS::DashboardClientROS(const rclcpp::Node::SharedPtr& node, cons "~/raw_request", [&](const ur_dashboard_msgs::srv::RawRequest::Request::SharedPtr req, ur_dashboard_msgs::srv::RawRequest::Response::SharedPtr resp) { try { - resp->answer = this->client_.sendAndReceive(req->query + "\n"); + resp->answer = this->client_->sendAndReceive(req->query + "\n"); } catch (const urcl::UrException& e) { RCLCPP_ERROR(rclcpp::get_logger("Dashboard_Client"), "Service Call failed: '%s'", e.what()); resp->answer = e.what(); @@ -256,19 +260,7 @@ DashboardClientROS::DashboardClientROS(const rclcpp::Node::SharedPtr& node, cons // Disconnect from the dashboard service. quit_service_ = - node_->create_service("~/quit", [&](const std_srvs::srv::Trigger::Request::SharedPtr req, - std_srvs::srv::Trigger::Response::SharedPtr resp) { - try { - resp->message = this->client_.sendAndReceive("quit\n"); - resp->success = std::regex_match(resp->message, std::regex("Disconnected")); - client_.disconnect(); - } catch (const urcl::UrException& e) { - RCLCPP_ERROR(rclcpp::get_logger("Dashboard_Client"), "Service Call failed: '%s'", e.what()); - resp->message = e.what(); - resp->success = false; - } - return true; - }); + createDashboardTriggerSrv("~/quit", std::bind(&urcl::DashboardClient::commandQuitWithResponse, client_.get())); // Service to query whether the robot is in remote control. is_in_remote_control_service_ = node_->create_service( @@ -284,26 +276,18 @@ bool DashboardClientROS::connect() node_->get_parameter("receive_timeout", time_buffer); tv.tv_sec = time_buffer; tv.tv_usec = 0; - client_.setReceiveTimeout(tv); - return client_.connect(); + client_->setReceiveTimeout(tv); + return client_->connect(); } bool DashboardClientROS::handleRunningQuery(const ur_dashboard_msgs::srv::IsProgramRunning::Request::SharedPtr req, ur_dashboard_msgs::srv::IsProgramRunning::Response::SharedPtr resp) { - try { - resp->answer = this->client_.sendAndReceive("running\n"); - std::regex expected("Program running: (true|false)"); - std::smatch match; - resp->success = std::regex_match(resp->answer, match, expected); - - if (resp->success) { - resp->program_running = (match[1] == "true"); - } - } catch (const urcl::UrException& e) { - RCLCPP_ERROR(rclcpp::get_logger("Dashboard_Client"), "Service Call failed: '%s'", e.what()); - resp->answer = e.what(); - resp->success = false; + auto dashboard_response = dashboardCallWithChecks([this]() { return client_->commandRunningWithResponse(); }, resp); + if (resp->success) { + handleDashboardResponseData( + [dashboard_response, resp]() { resp->program_running = std::get(dashboard_response.data.at("running")); }, + resp, dashboard_response); } return true; @@ -312,20 +296,17 @@ bool DashboardClientROS::handleRunningQuery(const ur_dashboard_msgs::srv::IsProg bool DashboardClientROS::handleSavedQuery(ur_dashboard_msgs::srv::IsProgramSaved::Request::SharedPtr req, ur_dashboard_msgs::srv::IsProgramSaved::Response::SharedPtr resp) { - try { - resp->answer = this->client_.sendAndReceive("isProgramSaved\n"); - std::regex expected("(true|false) ([^\\s]+)"); - std::smatch match; - resp->success = std::regex_match(resp->answer, match, expected); - - if (resp->success) { - resp->program_saved = (match[1] == "true"); - resp->program_name = match[2]; - } - } catch (const urcl::UrException& e) { - RCLCPP_ERROR(rclcpp::get_logger("Dashboard_Client"), "Service Call failed: '%s'", e.what()); - resp->answer = e.what(); - resp->success = false; + auto dashboard_response = + dashboardCallWithChecks([this]() { return client_->commandIsProgramSavedWithResponse(); }, resp); + if (resp->success) { + handleDashboardResponseData( + [dashboard_response, resp]() { + resp->program_saved = std::get(dashboard_response.data.at("saved")); + if (dashboard_response.data.find("program_name") != dashboard_response.data.end()) { + resp->program_name = std::get(dashboard_response.data.at("program_name")); + } + }, + resp, dashboard_response); } return true; @@ -334,45 +315,33 @@ bool DashboardClientROS::handleSavedQuery(ur_dashboard_msgs::srv::IsProgramSaved bool DashboardClientROS::handleSafetyModeQuery(const ur_dashboard_msgs::srv::GetSafetyMode::Request::SharedPtr req, ur_dashboard_msgs::srv::GetSafetyMode::Response::SharedPtr resp) { - try { - resp->answer = this->client_.sendAndReceive("safetymode\n"); - std::smatch match; - std::regex expected("Safetymode: (.+)"); - resp->success = std::regex_match(resp->answer, match, expected); - if (resp->success) { - if (match[1] == "NORMAL") { - resp->safety_mode.mode = ur_dashboard_msgs::msg::SafetyMode::NORMAL; - } else if (match[1] == "REDUCED") { - resp->safety_mode.mode = ur_dashboard_msgs::msg::SafetyMode::REDUCED; - } else if (match[1] == "PROTECTIVE_STOP") { - resp->safety_mode.mode = ur_dashboard_msgs::msg::SafetyMode::PROTECTIVE_STOP; - } else if (match[1] == "RECOVERY") { - resp->safety_mode.mode = ur_dashboard_msgs::msg::SafetyMode::RECOVERY; - } else if (match[1] == "SAFEGUARD_STOP") { - resp->safety_mode.mode = ur_dashboard_msgs::msg::SafetyMode::SAFEGUARD_STOP; - } else if (match[1] == "SYSTEM_EMERGENCY_STOP") { - resp->safety_mode.mode = ur_dashboard_msgs::msg::SafetyMode::SYSTEM_EMERGENCY_STOP; - } else if (match[1] == "ROBOT_EMERGENCY_STOP") { - resp->safety_mode.mode = ur_dashboard_msgs::msg::SafetyMode::ROBOT_EMERGENCY_STOP; - } else if (match[1] == "VIOLATION") { - resp->safety_mode.mode = ur_dashboard_msgs::msg::SafetyMode::VIOLATION; - } else if (match[1] == "FAULT") { - resp->safety_mode.mode = ur_dashboard_msgs::msg::SafetyMode::FAULT; - } - // The following are only available in SafetyStatus from 5.5 on - // else if (match[1] == "AUTOMATIC_MODE_SAFEGUARD_STOP") - //{ - // resp->safety_mode.mode = ur_dashboard_msgs::msg::SafetyMode::AUTOMATIC_MODE_SAFEGUARD_STOP; - //} - // else if (match[1] == "SYSTEM_THREE_POSITION_ENABLING_STOP") - //{ - // resp->safety_mode.mode = ur_dashboard_msgs::msg::SafetyMode::SYSTEM_THREE_POSITION_ENABLING_STOP; - //} - } - } catch (const urcl::UrException& e) { - RCLCPP_ERROR(rclcpp::get_logger("Dashboard_Client"), "Service Call failed: '%s'", e.what()); - resp->answer = e.what(); - resp->success = false; + auto dashboard_response = + dashboardCallWithChecks([this]() { return client_->commandSafetyModeWithResponse(); }, resp); + if (resp->success) { + handleDashboardResponseData( + [dashboard_response, resp]() { + const std::string safetymode_str = std::get(dashboard_response.data.at("safety_mode")); + if (safetymode_str == "NORMAL") { + resp->safety_mode.mode = ur_dashboard_msgs::msg::SafetyMode::NORMAL; + } else if (safetymode_str == "REDUCED") { + resp->safety_mode.mode = ur_dashboard_msgs::msg::SafetyMode::REDUCED; + } else if (safetymode_str == "PROTECTIVE_STOP") { + resp->safety_mode.mode = ur_dashboard_msgs::msg::SafetyMode::PROTECTIVE_STOP; + } else if (safetymode_str == "RECOVERY") { + resp->safety_mode.mode = ur_dashboard_msgs::msg::SafetyMode::RECOVERY; + } else if (safetymode_str == "SAFEGUARD_STOP") { + resp->safety_mode.mode = ur_dashboard_msgs::msg::SafetyMode::SAFEGUARD_STOP; + } else if (safetymode_str == "SYSTEM_EMERGENCY_STOP") { + resp->safety_mode.mode = ur_dashboard_msgs::msg::SafetyMode::SYSTEM_EMERGENCY_STOP; + } else if (safetymode_str == "ROBOT_EMERGENCY_STOP") { + resp->safety_mode.mode = ur_dashboard_msgs::msg::SafetyMode::ROBOT_EMERGENCY_STOP; + } else if (safetymode_str == "VIOLATION") { + resp->safety_mode.mode = ur_dashboard_msgs::msg::SafetyMode::VIOLATION; + } else if (safetymode_str == "FAULT") { + resp->safety_mode.mode = ur_dashboard_msgs::msg::SafetyMode::FAULT; + } + }, + resp, dashboard_response); } return true; } @@ -380,38 +349,34 @@ bool DashboardClientROS::handleSafetyModeQuery(const ur_dashboard_msgs::srv::Get bool DashboardClientROS::handleRobotModeQuery(const ur_dashboard_msgs::srv::GetRobotMode::Request::SharedPtr req, ur_dashboard_msgs::srv::GetRobotMode::Response::SharedPtr resp) { - try { - resp->answer = this->client_.sendAndReceive("robotmode\n"); - std::smatch match; - std::regex expected("Robotmode: (.+)"); - resp->success = std::regex_match(resp->answer, match, expected); - if (resp->success) { - if (match[1] == "NO_CONTROLLER") { - resp->robot_mode.mode = ur_dashboard_msgs::msg::RobotMode::NO_CONTROLLER; - } else if (match[1] == "DISCONNECTED") { - resp->robot_mode.mode = ur_dashboard_msgs::msg::RobotMode::DISCONNECTED; - } else if (match[1] == "CONFIRM_SAFETY") { - resp->robot_mode.mode = ur_dashboard_msgs::msg::RobotMode::CONFIRM_SAFETY; - } else if (match[1] == "BOOTING") { - resp->robot_mode.mode = ur_dashboard_msgs::msg::RobotMode::BOOTING; - } else if (match[1] == "POWER_OFF") { - resp->robot_mode.mode = ur_dashboard_msgs::msg::RobotMode::POWER_OFF; - } else if (match[1] == "POWER_ON") { - resp->robot_mode.mode = ur_dashboard_msgs::msg::RobotMode::POWER_ON; - } else if (match[1] == "IDLE") { - resp->robot_mode.mode = ur_dashboard_msgs::msg::RobotMode::IDLE; - } else if (match[1] == "BACKDRIVE") { - resp->robot_mode.mode = ur_dashboard_msgs::msg::RobotMode::BACKDRIVE; - } else if (match[1] == "RUNNING") { - resp->robot_mode.mode = ur_dashboard_msgs::msg::RobotMode::RUNNING; - } else if (match[1] == "UPDATING_FIRMWARE") { - resp->robot_mode.mode = ur_dashboard_msgs::msg::RobotMode::UPDATING_FIRMWARE; - } - } - } catch (const urcl::UrException& e) { - RCLCPP_ERROR(rclcpp::get_logger("Dashboard_Client"), "Service Call failed: '%s'", e.what()); - resp->answer = e.what(); - resp->success = false; + auto dashboard_response = dashboardCallWithChecks([this]() { return client_->commandRobotModeWithResponse(); }, resp); + if (resp->success) { + handleDashboardResponseData( + [dashboard_response, resp]() { + const std::string robotmode_str = std::get(dashboard_response.data.at("robot_mode")); + if (robotmode_str == "NO_CONTROLLER") { + resp->robot_mode.mode = ur_dashboard_msgs::msg::RobotMode::NO_CONTROLLER; + } else if (robotmode_str == "DISCONNECTED") { + resp->robot_mode.mode = ur_dashboard_msgs::msg::RobotMode::DISCONNECTED; + } else if (robotmode_str == "CONFIRM_SAFETY") { + resp->robot_mode.mode = ur_dashboard_msgs::msg::RobotMode::CONFIRM_SAFETY; + } else if (robotmode_str == "BOOTING") { + resp->robot_mode.mode = ur_dashboard_msgs::msg::RobotMode::BOOTING; + } else if (robotmode_str == "POWER_OFF") { + resp->robot_mode.mode = ur_dashboard_msgs::msg::RobotMode::POWER_OFF; + } else if (robotmode_str == "POWER_ON") { + resp->robot_mode.mode = ur_dashboard_msgs::msg::RobotMode::POWER_ON; + } else if (robotmode_str == "IDLE") { + resp->robot_mode.mode = ur_dashboard_msgs::msg::RobotMode::IDLE; + } else if (robotmode_str == "BACKDRIVE") { + resp->robot_mode.mode = ur_dashboard_msgs::msg::RobotMode::BACKDRIVE; + } else if (robotmode_str == "RUNNING") { + resp->robot_mode.mode = ur_dashboard_msgs::msg::RobotMode::RUNNING; + } else if (robotmode_str == "UPDATING_FIRMWARE") { + resp->robot_mode.mode = ur_dashboard_msgs::msg::RobotMode::UPDATING_FIRMWARE; + } + }, + resp, dashboard_response); } return true; } @@ -421,7 +386,7 @@ bool DashboardClientROS::handleRemoteControlQuery( ur_dashboard_msgs::srv::IsInRemoteControl::Response::SharedPtr resp) { try { - resp->remote_control = this->client_.commandIsInRemoteControl(); + resp->remote_control = this->client_->commandIsInRemoteControl(); resp->success = true; } catch (const urcl::UrException& e) { RCLCPP_ERROR(rclcpp::get_logger("Dashboard_Client"), "Service Call failed: '%s'", e.what()); diff --git a/ur_robot_driver/test/dashboard_client.py b/ur_robot_driver/test/dashboard_client.py index d55d27828..78e8affe3 100755 --- a/ur_robot_driver/test/dashboard_client.py +++ b/ur_robot_driver/test/dashboard_client.py @@ -61,6 +61,7 @@ def tearDownClass(cls): def init_robot(self): self._dashboard_interface = DashboardInterface(self.node) + self._dashboard_interface.power_off() # create a defined starting state def test_switch_on(self): """Test power on a robot.""" diff --git a/ur_robot_driver/test/test_common.py b/ur_robot_driver/test/test_common.py index a256dbf7a..f96624538 100644 --- a/ur_robot_driver/test/test_common.py +++ b/ur_robot_driver/test/test_common.py @@ -235,6 +235,7 @@ class DashboardInterface( }, ): def start_robot(self): + self._check_call(self.power_off()) self._check_call(self.power_on()) self._check_call(self.brake_release()) From 64d6e619fdb1704b0d40c03a0bb31061fc72894d Mon Sep 17 00:00:00 2001 From: "mergify[bot]" <37929162+mergify[bot]@users.noreply.github.com> Date: Mon, 17 Nov 2025 16:23:13 +0100 Subject: [PATCH 114/133] Auto-update pre-commit hooks (backport #1576) (#1577) --- .pre-commit-config.yaml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml index 0e388cddc..481b2495d 100644 --- a/.pre-commit-config.yaml +++ b/.pre-commit-config.yaml @@ -39,7 +39,7 @@ repos: args: [--py36-plus] - repo: https://github.com/psf/black - rev: 25.9.0 + rev: 25.11.0 hooks: - id: black args: ["--line-length=100"] From 94f89657cb9e16829e04622c142145281c232737 Mon Sep 17 00:00:00 2001 From: "mergify[bot]" <37929162+mergify[bot]@users.noreply.github.com> Date: Mon, 24 Nov 2025 12:12:10 +0100 Subject: [PATCH 115/133] Bump actions/checkout from 5 to 6 (backport #1581) (#1583) --- .github/workflows/check_links.yml | 2 +- .github/workflows/ci-format.yml | 2 +- .github/workflows/coverage-build.yml | 2 +- 3 files changed, 3 insertions(+), 3 deletions(-) diff --git a/.github/workflows/check_links.yml b/.github/workflows/check_links.yml index 672bfa264..78e9a5ce7 100644 --- a/.github/workflows/check_links.yml +++ b/.github/workflows/check_links.yml @@ -10,7 +10,7 @@ jobs: check_links: runs-on: ubuntu-latest steps: - - uses: actions/checkout@v5 + - uses: actions/checkout@v6 - name: Restore lychee cache id: restore-cache uses: actions/cache/restore@v4 diff --git a/.github/workflows/ci-format.yml b/.github/workflows/ci-format.yml index 6dae95d28..f61012a51 100644 --- a/.github/workflows/ci-format.yml +++ b/.github/workflows/ci-format.yml @@ -15,7 +15,7 @@ jobs: name: Format runs-on: ubuntu-22.04 steps: - - uses: actions/checkout@v5 + - uses: actions/checkout@v6 - uses: actions/setup-python@v6 with: python-version: 3.10.4 diff --git a/.github/workflows/coverage-build.yml b/.github/workflows/coverage-build.yml index 55dcd17a3..b75dcb15e 100644 --- a/.github/workflows/coverage-build.yml +++ b/.github/workflows/coverage-build.yml @@ -20,7 +20,7 @@ jobs: with: required-ros-distributions: ${{ env.ROS_DISTRO }} use-ros2-testing: true - - uses: actions/checkout@v5 + - uses: actions/checkout@v6 - uses: ros-tooling/action-ros-ci@v0.4 with: target-ros2-distro: ${{ env.ROS_DISTRO }} From 995ad873e2ef37343cbfd02e1a8513b1819a5205 Mon Sep 17 00:00:00 2001 From: "mergify[bot]" <37929162+mergify[bot]@users.noreply.github.com> Date: Mon, 24 Nov 2025 12:12:54 +0100 Subject: [PATCH 116/133] Auto-update pre-commit hooks (backport #1580) (#1584) --- .pre-commit-config.yaml | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml index 481b2495d..f8264ccb4 100644 --- a/.pre-commit-config.yaml +++ b/.pre-commit-config.yaml @@ -33,7 +33,7 @@ repos: # Python hooks - repo: https://github.com/asottile/pyupgrade - rev: v3.21.1 + rev: v3.21.2 hooks: - id: pyupgrade args: [--py36-plus] @@ -87,7 +87,7 @@ repos: args: ["--linelength=120"] - repo: https://github.com/pre-commit/mirrors-clang-format - rev: 'v21.1.5' + rev: 'v21.1.6' hooks: - id: clang-format From 54e504c991667b3254d1bd6ba51e0112b10482a1 Mon Sep 17 00:00:00 2001 From: Felix Exner Date: Mon, 1 Dec 2025 11:38:29 +0000 Subject: [PATCH 117/133] Update changelogs for upcoming release --- ur/CHANGELOG.rst | 3 +++ ur_bringup/CHANGELOG.rst | 3 +++ ur_calibration/CHANGELOG.rst | 3 +++ ur_controllers/CHANGELOG.rst | 3 +++ ur_dashboard_msgs/CHANGELOG.rst | 3 +++ ur_moveit_config/CHANGELOG.rst | 3 +++ ur_robot_driver/CHANGELOG.rst | 7 +++++++ 7 files changed, 25 insertions(+) diff --git a/ur/CHANGELOG.rst b/ur/CHANGELOG.rst index 1761c7ea2..cc36e8703 100644 --- a/ur/CHANGELOG.rst +++ b/ur/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package ur ^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 2.10.0 (2025-10-23) ------------------- diff --git a/ur_bringup/CHANGELOG.rst b/ur_bringup/CHANGELOG.rst index 241283b2f..f41d28528 100644 --- a/ur_bringup/CHANGELOG.rst +++ b/ur_bringup/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package ur_bringup ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 2.10.0 (2025-10-23) ------------------- * Add support for UR18 (backport `#1524 `_) (`#1525 `_) diff --git a/ur_calibration/CHANGELOG.rst b/ur_calibration/CHANGELOG.rst index 68eb120aa..7f6451929 100644 --- a/ur_calibration/CHANGELOG.rst +++ b/ur_calibration/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package ur_calibration ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 2.10.0 (2025-10-23) ------------------- diff --git a/ur_controllers/CHANGELOG.rst b/ur_controllers/CHANGELOG.rst index 7beb750f6..ba84d3aeb 100644 --- a/ur_controllers/CHANGELOG.rst +++ b/ur_controllers/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package ur_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 2.10.0 (2025-10-23) ------------------- diff --git a/ur_dashboard_msgs/CHANGELOG.rst b/ur_dashboard_msgs/CHANGELOG.rst index 50cf589ad..221f5e70c 100644 --- a/ur_dashboard_msgs/CHANGELOG.rst +++ b/ur_dashboard_msgs/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package ur_dashboard_msgs ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 2.10.0 (2025-10-23) ------------------- diff --git a/ur_moveit_config/CHANGELOG.rst b/ur_moveit_config/CHANGELOG.rst index e6b3ec243..18b7976f3 100644 --- a/ur_moveit_config/CHANGELOG.rst +++ b/ur_moveit_config/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package ur_moveit_config ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 2.10.0 (2025-10-23) ------------------- * Add support for UR18 (backport `#1524 `_) (`#1525 `_) diff --git a/ur_robot_driver/CHANGELOG.rst b/ur_robot_driver/CHANGELOG.rst index 1e7c9eaf6..b316b3991 100644 --- a/ur_robot_driver/CHANGELOG.rst +++ b/ur_robot_driver/CHANGELOG.rst @@ -1,3 +1,10 @@ +Forthcoming +----------- +* Dashboard client polyscopex (backport `#1546 `_) (`#1562 `_) +* Fix flaky tests (backport `#1559 `_) (`#1566 `_) +* Add effort command interface to hardware interface (backport `#1411 `_) (`#1528 `_) +* Contributors: mergify[bot] + 2.10.0 (2025-10-23) ------------------- * Add missing update_rate config files for UR7e and UR12e (backport of `#1544 `_) (`#1547 `_) From 6bca8445970efe658b2b729acf5c1def884d914d Mon Sep 17 00:00:00 2001 From: Felix Exner Date: Mon, 1 Dec 2025 11:38:39 +0000 Subject: [PATCH 118/133] 2.11.0 --- ur/CHANGELOG.rst | 4 ++-- ur/package.xml | 2 +- ur_bringup/CHANGELOG.rst | 4 ++-- ur_bringup/package.xml | 2 +- ur_calibration/CHANGELOG.rst | 4 ++-- ur_calibration/package.xml | 2 +- ur_controllers/CHANGELOG.rst | 4 ++-- ur_controllers/package.xml | 2 +- ur_dashboard_msgs/CHANGELOG.rst | 4 ++-- ur_dashboard_msgs/package.xml | 2 +- ur_moveit_config/CHANGELOG.rst | 4 ++-- ur_moveit_config/package.xml | 2 +- ur_robot_driver/CHANGELOG.rst | 4 ++-- ur_robot_driver/package.xml | 2 +- 14 files changed, 21 insertions(+), 21 deletions(-) diff --git a/ur/CHANGELOG.rst b/ur/CHANGELOG.rst index cc36e8703..6a23f0a1e 100644 --- a/ur/CHANGELOG.rst +++ b/ur/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package ur ^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.11.0 (2025-12-01) +------------------- 2.10.0 (2025-10-23) ------------------- diff --git a/ur/package.xml b/ur/package.xml index b323603ea..9da1ebfdb 100644 --- a/ur/package.xml +++ b/ur/package.xml @@ -2,7 +2,7 @@ ur - 2.10.0 + 2.11.0 Metapackage for universal robots Felix Exner diff --git a/ur_bringup/CHANGELOG.rst b/ur_bringup/CHANGELOG.rst index f41d28528..e22339c80 100644 --- a/ur_bringup/CHANGELOG.rst +++ b/ur_bringup/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package ur_bringup ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.11.0 (2025-12-01) +------------------- 2.10.0 (2025-10-23) ------------------- diff --git a/ur_bringup/package.xml b/ur_bringup/package.xml index afd4f87b4..d632bc0fd 100644 --- a/ur_bringup/package.xml +++ b/ur_bringup/package.xml @@ -2,7 +2,7 @@ ur_bringup - 2.10.0 + 2.11.0 Launch file and run-time configurations, e.g. controllers. Felix Exner diff --git a/ur_calibration/CHANGELOG.rst b/ur_calibration/CHANGELOG.rst index 7f6451929..2feb6b8b1 100644 --- a/ur_calibration/CHANGELOG.rst +++ b/ur_calibration/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package ur_calibration ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.11.0 (2025-12-01) +------------------- 2.10.0 (2025-10-23) ------------------- diff --git a/ur_calibration/package.xml b/ur_calibration/package.xml index 0af695a78..3a10da09d 100644 --- a/ur_calibration/package.xml +++ b/ur_calibration/package.xml @@ -1,7 +1,7 @@ ur_calibration - 2.10.0 + 2.11.0 Package for extracting the factory calibration from a UR robot and change it such that it can be used by ur_description to gain a correct URDF Felix Exner diff --git a/ur_controllers/CHANGELOG.rst b/ur_controllers/CHANGELOG.rst index ba84d3aeb..dbcd42f18 100644 --- a/ur_controllers/CHANGELOG.rst +++ b/ur_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package ur_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.11.0 (2025-12-01) +------------------- 2.10.0 (2025-10-23) ------------------- diff --git a/ur_controllers/package.xml b/ur_controllers/package.xml index e7da41559..2feabb802 100644 --- a/ur_controllers/package.xml +++ b/ur_controllers/package.xml @@ -2,7 +2,7 @@ ur_controllers - 2.10.0 + 2.11.0 Provides controllers that use the speed scaling interface of Universal Robots. Felix Exner diff --git a/ur_dashboard_msgs/CHANGELOG.rst b/ur_dashboard_msgs/CHANGELOG.rst index 221f5e70c..b2a41551c 100644 --- a/ur_dashboard_msgs/CHANGELOG.rst +++ b/ur_dashboard_msgs/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package ur_dashboard_msgs ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.11.0 (2025-12-01) +------------------- 2.10.0 (2025-10-23) ------------------- diff --git a/ur_dashboard_msgs/package.xml b/ur_dashboard_msgs/package.xml index 65e706254..3ff5debf7 100644 --- a/ur_dashboard_msgs/package.xml +++ b/ur_dashboard_msgs/package.xml @@ -2,7 +2,7 @@ ur_dashboard_msgs - 2.10.0 + 2.11.0 Messages around the UR Dashboard server. Felix Exner diff --git a/ur_moveit_config/CHANGELOG.rst b/ur_moveit_config/CHANGELOG.rst index 18b7976f3..2fab0168d 100644 --- a/ur_moveit_config/CHANGELOG.rst +++ b/ur_moveit_config/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package ur_moveit_config ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.11.0 (2025-12-01) +------------------- 2.10.0 (2025-10-23) ------------------- diff --git a/ur_moveit_config/package.xml b/ur_moveit_config/package.xml index 477c26b8e..f2f2e3e8a 100644 --- a/ur_moveit_config/package.xml +++ b/ur_moveit_config/package.xml @@ -2,7 +2,7 @@ ur_moveit_config - 2.10.0 + 2.11.0 An example package with MoveIt2 configurations for UR robots. diff --git a/ur_robot_driver/CHANGELOG.rst b/ur_robot_driver/CHANGELOG.rst index b316b3991..7e4bd5439 100644 --- a/ur_robot_driver/CHANGELOG.rst +++ b/ur_robot_driver/CHANGELOG.rst @@ -1,5 +1,5 @@ -Forthcoming ------------ +2.11.0 (2025-12-01) +------------------- * Dashboard client polyscopex (backport `#1546 `_) (`#1562 `_) * Fix flaky tests (backport `#1559 `_) (`#1566 `_) * Add effort command interface to hardware interface (backport `#1411 `_) (`#1528 `_) diff --git a/ur_robot_driver/package.xml b/ur_robot_driver/package.xml index c531bf24a..7a1ad58b0 100644 --- a/ur_robot_driver/package.xml +++ b/ur_robot_driver/package.xml @@ -2,7 +2,7 @@ ur_robot_driver - 2.10.0 + 2.11.0 The new driver for Universal Robots UR3, UR5 and UR10 robots with CB3 controllers and the e-series. Felix Exner From 1ecba595e8c8560226608d371b07f79bc6805209 Mon Sep 17 00:00:00 2001 From: "mergify[bot]" <37929162+mergify[bot]@users.noreply.github.com> Date: Mon, 8 Dec 2025 10:12:31 +0100 Subject: [PATCH 119/133] Auto-update pre-commit hooks (backport of #1593) (#1595) --- .pre-commit-config.yaml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml index f8264ccb4..58f3f33e5 100644 --- a/.pre-commit-config.yaml +++ b/.pre-commit-config.yaml @@ -87,7 +87,7 @@ repos: args: ["--linelength=120"] - repo: https://github.com/pre-commit/mirrors-clang-format - rev: 'v21.1.6' + rev: 'v21.1.7' hooks: - id: clang-format From 25dc87cdc8b81a0ed820b30ba79814f2e9f18a75 Mon Sep 17 00:00:00 2001 From: "mergify[bot]" <37929162+mergify[bot]@users.noreply.github.com> Date: Fri, 12 Dec 2025 11:20:47 +0100 Subject: [PATCH 120/133] Add ros2run as a test_depend (backport of #1597) (#1600) Since we use a "ros2 run" command line command to run the example in the tests, we need to add ros2run as a test dependency as well, to make sure that it is present in the testing container. Before, it was installed as a transitive dependency, but we start seeing test errors on the testing repo because of this. (cherry picked from commit 787d223edca563effdd41f0ae9fd5b9dadf207c5) Co-authored-by: Felix Exner --- ur_robot_driver/package.xml | 1 + 1 file changed, 1 insertion(+) diff --git a/ur_robot_driver/package.xml b/ur_robot_driver/package.xml index 7a1ad58b0..59587a26a 100644 --- a/ur_robot_driver/package.xml +++ b/ur_robot_driver/package.xml @@ -64,6 +64,7 @@ xacro launch_testing_ament_cmake + ros2run ament_cmake From 7be3e55e08a7b0610eef1c9579f31cd939b0bb66 Mon Sep 17 00:00:00 2001 From: "mergify[bot]" <37929162+mergify[bot]@users.noreply.github.com> Date: Fri, 12 Dec 2025 13:46:56 +0100 Subject: [PATCH 121/133] Replace dashboard client on PolyScope X warning (backport of #1599) (#1602) Since connections to PolyScope X robots are now handled differently, the warning isn't true any longer. If an error is raised during initialization, that most likely is a problem and we should stop the client in that case. Returning 1 instead of 0 makes it transparent to the outside world, that there has been an unexpected exit. --- ur_robot_driver/src/dashboard_client_node.cpp | 8 +++----- 1 file changed, 3 insertions(+), 5 deletions(-) diff --git a/ur_robot_driver/src/dashboard_client_node.cpp b/ur_robot_driver/src/dashboard_client_node.cpp index da9156b5e..96b46f546 100644 --- a/ur_robot_driver/src/dashboard_client_node.cpp +++ b/ur_robot_driver/src/dashboard_client_node.cpp @@ -58,11 +58,9 @@ int main(int argc, char** argv) try { client = std::make_shared(node, robot_ip); } catch (const urcl::UrException& e) { - RCLCPP_WARN(rclcpp::get_logger("Dashboard_Client"), - "%s This warning is expected on a PolyScopeX robot. If you don't want to see this warning, " - "please don't start the dashboard client. Exiting dashboard client now.", - e.what()); - return 0; + RCLCPP_ERROR(rclcpp::get_logger("Dashboard_Client"), + "Error raised during Dashboard Client startup: %s. Exiting dashboard client now.", e.what()); + return 1; } rclcpp::spin(node); From ee06335a33a67dbddf9de97a408da51b2f1bd7f9 Mon Sep 17 00:00:00 2001 From: "mergify[bot]" <37929162+mergify[bot]@users.noreply.github.com> Date: Mon, 15 Dec 2025 10:08:45 +0100 Subject: [PATCH 122/133] [DashboardClient] Add a parameter callback (backport #1598) (#1604) * [DashboardClient] Add a parameter callback (#1598) * Use sub-seconds for timeout configuration, as well Before, only the full seconds of the parameter were used. This commit also uses the non-integer part of the given timeout. For example, a timeout of 1.4 seconds before was configured as 1 sec. Now it is configured as 1 sec + 400000 us. * Add parameter callback to dashboard client Add a callback to change the receive_timeout parameter on the fly. * Add dashboard_receive_timeout parameter to launchfiles Also use the dashboard launchfile from the ur_control launchfile instead of starting the node directly. This way, it is possible to propagate the receive timeout to the ur_control launchfile. * Update documentation --- .../ur_robot_driver/dashboard_client_ros.hpp | 5 +++ ur_robot_driver/launch/ur_control.launch.py | 19 ++++++----- .../launch/ur_dashboard_client.launch.py | 13 ++++++- ur_robot_driver/src/dashboard_client_ros.cpp | 34 +++++++++++++++++-- 4 files changed, 60 insertions(+), 11 deletions(-) diff --git a/ur_robot_driver/include/ur_robot_driver/dashboard_client_ros.hpp b/ur_robot_driver/include/ur_robot_driver/dashboard_client_ros.hpp index 1f12ca5dc..55cd8700b 100644 --- a/ur_robot_driver/include/ur_robot_driver/dashboard_client_ros.hpp +++ b/ur_robot_driver/include/ur_robot_driver/dashboard_client_ros.hpp @@ -44,6 +44,7 @@ #include #include #include +#include // ROS #include "rclcpp/rclcpp.hpp" @@ -161,6 +162,8 @@ class DashboardClientROS } } + rcl_interfaces::msg::SetParametersResult parametersCallback(const std::vector& parameters); + bool handleRunningQuery(ur_dashboard_msgs::srv::IsProgramRunning::Request::SharedPtr req, ur_dashboard_msgs::srv::IsProgramRunning::Response::SharedPtr resp); @@ -214,6 +217,8 @@ class DashboardClientROS rclcpp::Service::SharedPtr safety_mode_service_; rclcpp::Service::SharedPtr robot_mode_service_; rclcpp::Service::SharedPtr is_in_remote_control_service_; + + rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr param_callback_handle_; }; } // namespace ur_robot_driver diff --git a/ur_robot_driver/launch/ur_control.launch.py b/ur_robot_driver/launch/ur_control.launch.py index d4afd0782..673e4fd6d 100644 --- a/ur_robot_driver/launch/ur_control.launch.py +++ b/ur_robot_driver/launch/ur_control.launch.py @@ -34,7 +34,8 @@ from launch_ros.substitutions import FindPackageShare from launch import LaunchDescription -from launch.actions import DeclareLaunchArgument, OpaqueFunction +from launch.actions import DeclareLaunchArgument, OpaqueFunction, IncludeLaunchDescription +from launch.launch_description_sources import AnyLaunchDescriptionSource from launch.conditions import IfCondition, UnlessCondition from launch.substitutions import ( AndSubstitution, @@ -245,16 +246,18 @@ def launch_setup(context, *args, **kwargs): condition=UnlessCondition(use_fake_hardware), ) - dashboard_client_node = Node( - package="ur_robot_driver", + dashboard_client_node = IncludeLaunchDescription( condition=IfCondition( AndSubstitution(launch_dashboard_client, NotSubstitution(use_fake_hardware)) ), - executable="dashboard_client", - name="dashboard_client", - output="screen", - emulate_tty=True, - parameters=[{"robot_ip": robot_ip}], + launch_description_source=AnyLaunchDescriptionSource( + PathJoinSubstitution( + [FindPackageShare("ur_robot_driver"), "launch", "ur_dashboard_client.launch.py"] + ) + ), + launch_arguments={ + "robot_ip": robot_ip, + }.items(), ) robot_state_helper_node = Node( diff --git a/ur_robot_driver/launch/ur_dashboard_client.launch.py b/ur_robot_driver/launch/ur_dashboard_client.launch.py index 4b6383906..3aef92859 100755 --- a/ur_robot_driver/launch/ur_dashboard_client.launch.py +++ b/ur_robot_driver/launch/ur_dashboard_client.launch.py @@ -42,9 +42,17 @@ def generate_launch_description(): description="IP address by which the robot can be reached.", ) ) + declared_arguments.append( + DeclareLaunchArgument( + "dashboard_receive_timeout", + default_value="20.0", + description="Timeout that the dashboard client will wait for a response from the robot.", + ) + ) # Initialize Arguments robot_ip = LaunchConfiguration("robot_ip") + dashboard_receive_timeout = LaunchConfiguration("dashboard_receive_timeout") dashboard_client_node = Node( package="ur_robot_driver", @@ -52,7 +60,10 @@ def generate_launch_description(): name="dashboard_client", output="screen", emulate_tty=True, - parameters=[{"robot_ip": robot_ip}], + parameters=[ + {"robot_ip": robot_ip}, + {"receive_timeout": dashboard_receive_timeout}, + ], ) return LaunchDescription(declared_arguments + [dashboard_client_node]) diff --git a/ur_robot_driver/src/dashboard_client_ros.cpp b/ur_robot_driver/src/dashboard_client_ros.cpp index 93e85a9d2..f093763cf 100644 --- a/ur_robot_driver/src/dashboard_client_ros.cpp +++ b/ur_robot_driver/src/dashboard_client_ros.cpp @@ -51,7 +51,10 @@ namespace ur_robot_driver DashboardClientROS::DashboardClientROS(const rclcpp::Node::SharedPtr& node, const std::string& robot_ip) : node_(node), primary_client_(robot_ip, notifier_) { - node_->declare_parameter("receive_timeout", 2); + node_->declare_parameter("receive_timeout", 20); + + param_callback_handle_ = node_->add_on_set_parameters_callback( + std::bind(&DashboardClientROS::parametersCallback, this, std::placeholders::_1)); primary_client_.start(10, std::chrono::seconds(10)); auto robot_version = primary_client_.getRobotVersion(); @@ -275,7 +278,7 @@ bool DashboardClientROS::connect() double time_buffer = 0; node_->get_parameter("receive_timeout", time_buffer); tv.tv_sec = time_buffer; - tv.tv_usec = 0; + tv.tv_usec = (time_buffer - static_cast(time_buffer)) * 1e6; client_->setReceiveTimeout(tv); return client_->connect(); } @@ -396,4 +399,31 @@ bool DashboardClientROS::handleRemoteControlQuery( return true; } +rcl_interfaces::msg::SetParametersResult +DashboardClientROS::parametersCallback(const std::vector& parameters) + +{ + rcl_interfaces::msg::SetParametersResult result; + + for (const auto& parameter : parameters) { + if (parameter.get_name() == "receive_timeout") { + timeval tv; + double time_buffer = parameter.as_double(); + tv.tv_sec = time_buffer; + tv.tv_usec = (time_buffer - static_cast(time_buffer)) * 1e6; + client_->setReceiveTimeout(tv); + RCLCPP_INFO(node_->get_logger(), "Set receive_timeout to %f seconds", time_buffer); + } else { + result.successful = false; + result.reason = "Requested to change parameter '" + parameter.get_name() + + "'. Changing this parameter during runtime is not supported / will not have any effect"; + return result; + } + } + result.successful = true; + result.reason = "success"; + + return result; +} + } // namespace ur_robot_driver From e9aaf17b9ff2756e71fe21bacbb19bb0a9c75230 Mon Sep 17 00:00:00 2001 From: "mergify[bot]" <37929162+mergify[bot]@users.noreply.github.com> Date: Thu, 18 Dec 2025 12:11:38 +0100 Subject: [PATCH 123/133] Bump actions/upload-artifact from 5 to 6 (backport of #1608) (#1613) Bumps [actions/upload-artifact](https://github.com/actions/upload-artifact) from 5 to 6. - [Release notes](https://github.com/actions/upload-artifact/releases) - [Commits](https://github.com/actions/upload-artifact/compare/v5...v6) --- updated-dependencies: - dependency-name: actions/upload-artifact dependency-version: '6' dependency-type: direct:production update-type: version-update:semver-major ... (cherry picked from commit be46df5a72a8376fb2e71406fa4a70ac4a7991e9) Signed-off-by: dependabot[bot] Co-authored-by: dependabot[bot] <49699333+dependabot[bot]@users.noreply.github.com> --- .github/workflows/coverage-build.yml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/workflows/coverage-build.yml b/.github/workflows/coverage-build.yml index b75dcb15e..3016d5d1d 100644 --- a/.github/workflows/coverage-build.yml +++ b/.github/workflows/coverage-build.yml @@ -48,7 +48,7 @@ jobs: flags: unittests name: codecov-umbrella token: ${{ secrets.CODECOV_TOKEN }} - - uses: actions/upload-artifact@v5 + - uses: actions/upload-artifact@v6 with: name: colcon-logs-${{ matrix.os }} path: ros_ws/log From c6b3af8959cc088bcb4af2aca50b6ffe19370943 Mon Sep 17 00:00:00 2001 From: "mergify[bot]" <37929162+mergify[bot]@users.noreply.github.com> Date: Mon, 12 Jan 2026 09:26:39 +0100 Subject: [PATCH 124/133] Auto-update pre-commit hooks (backport #1607) (#1616) --- .pre-commit-config.yaml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml index 58f3f33e5..7a1f2e796 100644 --- a/.pre-commit-config.yaml +++ b/.pre-commit-config.yaml @@ -39,7 +39,7 @@ repos: args: [--py36-plus] - repo: https://github.com/psf/black - rev: 25.11.0 + rev: 25.12.0 hooks: - id: black args: ["--line-length=100"] From f76159d72cd5877d30e54786f082892c8080b8cc Mon Sep 17 00:00:00 2001 From: "mergify[bot]" <37929162+mergify[bot]@users.noreply.github.com> Date: Mon, 12 Jan 2026 09:28:11 +0100 Subject: [PATCH 125/133] Bump actions/cache from 4 to 5 (backport #1609) (#1618) * Bump actions/cache from 4 to 5 (#1609) Bumps [actions/cache](https://github.com/actions/cache) from 4 to 5. - [Release notes](https://github.com/actions/cache/releases) - [Changelog](https://github.com/actions/cache/blob/main/RELEASES.md) - [Commits](https://github.com/actions/cache/compare/v4...v5) --- updated-dependencies: - dependency-name: actions/cache dependency-version: '5' dependency-type: direct:production update-type: version-update:semver-major ... Signed-off-by: dependabot[bot] Co-authored-by: dependabot[bot] <49699333+dependabot[bot]@users.noreply.github.com> (cherry picked from commit 335bfcd97139f91df5fbe16384520a97bf242493) # Conflicts: # .github/workflows/reusable_ici.yml * Delete .github/workflows/reusable_ici.yml --------- Co-authored-by: dependabot[bot] <49699333+dependabot[bot]@users.noreply.github.com> Co-authored-by: Felix Exner --- .github/workflows/check_links.yml | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/.github/workflows/check_links.yml b/.github/workflows/check_links.yml index 78e9a5ce7..e9725c634 100644 --- a/.github/workflows/check_links.yml +++ b/.github/workflows/check_links.yml @@ -13,7 +13,7 @@ jobs: - uses: actions/checkout@v6 - name: Restore lychee cache id: restore-cache - uses: actions/cache/restore@v4 + uses: actions/cache/restore@v5 with: path: .lycheecache key: cache-lychee-${{ github.run_id }}-${{ github.run_attempt }} @@ -34,7 +34,7 @@ jobs: --exclude 'releases/download/v\$%7BURCAP_VERSION%7D/externalcontrol-\$%7BURCAP_VERSION%7D\.jar' './**/*.md' './**/*.html' './**/*.rst' './**/*.cpp' './**/*.h' './**/*.py' - name: Save lychee cache - uses: actions/cache/save@v4 + uses: actions/cache/save@v5 if: always() with: path: .lycheecache From d3be43722f72cddfa6cbdab3cb02d2919b7bf7fb Mon Sep 17 00:00:00 2001 From: "mergify[bot]" <37929162+mergify[bot]@users.noreply.github.com> Date: Tue, 13 Jan 2026 06:27:24 +0100 Subject: [PATCH 126/133] Initialize force mode interfaces to NaN on init (backport #1625) (#1627) Initializing them on activate would lead to an error. When the program is running on the robot, the write method would invoke starting force mode with uninitialized (zero-initialized) values when the robot is in configured state. --- ur_robot_driver/src/hardware_interface.cpp | 14 +++++++------- 1 file changed, 7 insertions(+), 7 deletions(-) diff --git a/ur_robot_driver/src/hardware_interface.cpp b/ur_robot_driver/src/hardware_interface.cpp index 2cccb1426..749baba84 100644 --- a/ur_robot_driver/src/hardware_interface.cpp +++ b/ur_robot_driver/src/hardware_interface.cpp @@ -155,6 +155,13 @@ URPositionHardwareInterface::on_init(const hardware_interface::HardwareInfo& sys trajectory_joint_positions_.reserve(32768); trajectory_joint_velocities_.reserve(32768); trajectory_joint_accelerations_.reserve(32768); + for (size_t i = 0; i < 6; i++) { + force_mode_task_frame_[i] = NO_NEW_CMD_; + force_mode_selection_vector_[i] = static_cast(NO_NEW_CMD_); + force_mode_wrench_[i] = NO_NEW_CMD_; + force_mode_limits_[i] = NO_NEW_CMD_; + } + force_mode_type_ = static_cast(NO_NEW_CMD_); for (const hardware_interface::ComponentInfo& joint : info_.joints) { auto has_cmd_interface = [](const hardware_interface::ComponentInfo& joint, const std::string& interface_name) { @@ -679,13 +686,6 @@ URPositionHardwareInterface::on_activate(const rclcpp_lifecycle::State& previous { RCLCPP_INFO(rclcpp::get_logger("URPositionHardwareInterface"), "Activating HW interface"); - for (size_t i = 0; i < 6; i++) { - force_mode_task_frame_[i] = NO_NEW_CMD_; - force_mode_selection_vector_[i] = static_cast(NO_NEW_CMD_); - force_mode_wrench_[i] = NO_NEW_CMD_; - force_mode_limits_[i] = NO_NEW_CMD_; - } - force_mode_type_ = static_cast(NO_NEW_CMD_); return hardware_interface::CallbackReturn::SUCCESS; } From e919a0324f045a1c6cdc264e94f4adb30a4a5aaa Mon Sep 17 00:00:00 2001 From: "mergify[bot]" <37929162+mergify[bot]@users.noreply.github.com> Date: Tue, 13 Jan 2026 07:22:15 +0100 Subject: [PATCH 127/133] Auto-update pre-commit hooks (backport #1622) (#1629) --- .pre-commit-config.yaml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml index 7a1f2e796..0786f1c1a 100644 --- a/.pre-commit-config.yaml +++ b/.pre-commit-config.yaml @@ -87,7 +87,7 @@ repos: args: ["--linelength=120"] - repo: https://github.com/pre-commit/mirrors-clang-format - rev: 'v21.1.7' + rev: 'v21.1.8' hooks: - id: clang-format From b884934c681b6fca2d0af1c5e04aeb5281e0697b Mon Sep 17 00:00:00 2001 From: "mergify[bot]" <37929162+mergify[bot]@users.noreply.github.com> Date: Tue, 13 Jan 2026 13:53:49 +0100 Subject: [PATCH 128/133] Ensure latched qos is reliable (backport #1594) (#1632) --- ur_controllers/src/gpio_controller.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/ur_controllers/src/gpio_controller.cpp b/ur_controllers/src/gpio_controller.cpp index 3fadbd3db..4dfaadf76 100644 --- a/ur_controllers/src/gpio_controller.cpp +++ b/ur_controllers/src/gpio_controller.cpp @@ -282,6 +282,7 @@ ur_controllers::GPIOController::on_activate(const rclcpp_lifecycle::State& /*pre try { auto qos_latched = rclcpp::SystemDefaultsQoS(); qos_latched.transient_local(); + qos_latched.reliable(); // register publisher io_pub_ = get_node()->create_publisher("~/io_states", rclcpp::SystemDefaultsQoS()); From b01ae53650790c43b6554c8cd7d04308ce8d5126 Mon Sep 17 00:00:00 2001 From: "mergify[bot]" <37929162+mergify[bot]@users.noreply.github.com> Date: Tue, 13 Jan 2026 14:50:49 +0100 Subject: [PATCH 129/133] Fix flange-to-TCP wrench transformation (backport #1615) (#1635) --- ur_robot_driver/src/hardware_interface.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ur_robot_driver/src/hardware_interface.cpp b/ur_robot_driver/src/hardware_interface.cpp index 749baba84..e2b9273b8 100644 --- a/ur_robot_driver/src/hardware_interface.cpp +++ b/ur_robot_driver/src/hardware_interface.cpp @@ -1105,7 +1105,7 @@ void URPositionHardwareInterface::transformForceTorque() ft = base_to_flange.M.Inverse() * ft; // Transform the wrench to the tcp frame - ft = flange_to_tcp * ft; + ft = flange_to_tcp.Inverse() * ft; } else { // CB3 KDL::Vector vec = KDL::Vector(urcl_target_tcp_pose_[3], urcl_target_tcp_pose_[4], urcl_target_tcp_pose_[5]); double angle = vec.Normalize(); From 9d7b7193537cf45231864d347f787462e51ad1ba Mon Sep 17 00:00:00 2001 From: Felix Exner Date: Tue, 13 Jan 2026 15:14:10 +0100 Subject: [PATCH 130/133] Update changelogs for upcoming release --- ur/CHANGELOG.rst | 3 +++ ur_bringup/CHANGELOG.rst | 3 +++ ur_calibration/CHANGELOG.rst | 3 +++ ur_controllers/CHANGELOG.rst | 5 +++++ ur_dashboard_msgs/CHANGELOG.rst | 3 +++ ur_moveit_config/CHANGELOG.rst | 3 +++ ur_robot_driver/CHANGELOG.rst | 8 ++++++++ 7 files changed, 28 insertions(+) diff --git a/ur/CHANGELOG.rst b/ur/CHANGELOG.rst index 6a23f0a1e..b28932700 100644 --- a/ur/CHANGELOG.rst +++ b/ur/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package ur ^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 2.11.0 (2025-12-01) ------------------- diff --git a/ur_bringup/CHANGELOG.rst b/ur_bringup/CHANGELOG.rst index e22339c80..4d186d334 100644 --- a/ur_bringup/CHANGELOG.rst +++ b/ur_bringup/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package ur_bringup ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 2.11.0 (2025-12-01) ------------------- diff --git a/ur_calibration/CHANGELOG.rst b/ur_calibration/CHANGELOG.rst index 2feb6b8b1..8b3ade6c1 100644 --- a/ur_calibration/CHANGELOG.rst +++ b/ur_calibration/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package ur_calibration ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 2.11.0 (2025-12-01) ------------------- diff --git a/ur_controllers/CHANGELOG.rst b/ur_controllers/CHANGELOG.rst index dbcd42f18..b5f530322 100644 --- a/ur_controllers/CHANGELOG.rst +++ b/ur_controllers/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package ur_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Ensure latched qos is reliable (backport `#1594 `_) (`#1632 `_) +* Contributors: mergify[bot] + 2.11.0 (2025-12-01) ------------------- diff --git a/ur_dashboard_msgs/CHANGELOG.rst b/ur_dashboard_msgs/CHANGELOG.rst index b2a41551c..d40c2f0c5 100644 --- a/ur_dashboard_msgs/CHANGELOG.rst +++ b/ur_dashboard_msgs/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package ur_dashboard_msgs ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 2.11.0 (2025-12-01) ------------------- diff --git a/ur_moveit_config/CHANGELOG.rst b/ur_moveit_config/CHANGELOG.rst index 2fab0168d..6dd08b592 100644 --- a/ur_moveit_config/CHANGELOG.rst +++ b/ur_moveit_config/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package ur_moveit_config ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 2.11.0 (2025-12-01) ------------------- diff --git a/ur_robot_driver/CHANGELOG.rst b/ur_robot_driver/CHANGELOG.rst index 7e4bd5439..7a2d2dc74 100644 --- a/ur_robot_driver/CHANGELOG.rst +++ b/ur_robot_driver/CHANGELOG.rst @@ -1,3 +1,11 @@ +Forthcoming +----------- +* Fix flange-to-TCP wrench transformation (backport `#1615 `_) (`#1635 `_) +* Initialize force mode interfaces to NaN on init (backport `#1625 `_) (`#1627 `_) +* [DashboardClient] Add a parameter callback (backport `#1598 `_) (`#1604 `_) +* Replace dashboard client on PolyScope X warning (backport of `#1599 `_) (`#1602 `_) +* Add ros2run as a test_depend (backport of `#1597 `_) (`#1600 `_) + 2.11.0 (2025-12-01) ------------------- * Dashboard client polyscopex (backport `#1546 `_) (`#1562 `_) From 9091458e6990b4e6954ebc94c1bbd18a00c24cd9 Mon Sep 17 00:00:00 2001 From: Felix Exner Date: Tue, 13 Jan 2026 15:14:33 +0100 Subject: [PATCH 131/133] 2.12.0 --- ur/CHANGELOG.rst | 4 ++-- ur/package.xml | 2 +- ur_bringup/CHANGELOG.rst | 4 ++-- ur_bringup/package.xml | 2 +- ur_calibration/CHANGELOG.rst | 4 ++-- ur_calibration/package.xml | 2 +- ur_controllers/CHANGELOG.rst | 4 ++-- ur_controllers/package.xml | 2 +- ur_dashboard_msgs/CHANGELOG.rst | 4 ++-- ur_dashboard_msgs/package.xml | 2 +- ur_moveit_config/CHANGELOG.rst | 4 ++-- ur_moveit_config/package.xml | 2 +- ur_robot_driver/CHANGELOG.rst | 4 ++-- ur_robot_driver/package.xml | 2 +- 14 files changed, 21 insertions(+), 21 deletions(-) diff --git a/ur/CHANGELOG.rst b/ur/CHANGELOG.rst index b28932700..0ecc1eb3a 100644 --- a/ur/CHANGELOG.rst +++ b/ur/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package ur ^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.12.0 (2026-01-13) +------------------- 2.11.0 (2025-12-01) ------------------- diff --git a/ur/package.xml b/ur/package.xml index 9da1ebfdb..c4fb5c5e8 100644 --- a/ur/package.xml +++ b/ur/package.xml @@ -2,7 +2,7 @@ ur - 2.11.0 + 2.12.0 Metapackage for universal robots Felix Exner diff --git a/ur_bringup/CHANGELOG.rst b/ur_bringup/CHANGELOG.rst index 4d186d334..3caf835f3 100644 --- a/ur_bringup/CHANGELOG.rst +++ b/ur_bringup/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package ur_bringup ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.12.0 (2026-01-13) +------------------- 2.11.0 (2025-12-01) ------------------- diff --git a/ur_bringup/package.xml b/ur_bringup/package.xml index d632bc0fd..da3a8a3e7 100644 --- a/ur_bringup/package.xml +++ b/ur_bringup/package.xml @@ -2,7 +2,7 @@ ur_bringup - 2.11.0 + 2.12.0 Launch file and run-time configurations, e.g. controllers. Felix Exner diff --git a/ur_calibration/CHANGELOG.rst b/ur_calibration/CHANGELOG.rst index 8b3ade6c1..7dee9f8d2 100644 --- a/ur_calibration/CHANGELOG.rst +++ b/ur_calibration/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package ur_calibration ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.12.0 (2026-01-13) +------------------- 2.11.0 (2025-12-01) ------------------- diff --git a/ur_calibration/package.xml b/ur_calibration/package.xml index 3a10da09d..149aefa66 100644 --- a/ur_calibration/package.xml +++ b/ur_calibration/package.xml @@ -1,7 +1,7 @@ ur_calibration - 2.11.0 + 2.12.0 Package for extracting the factory calibration from a UR robot and change it such that it can be used by ur_description to gain a correct URDF Felix Exner diff --git a/ur_controllers/CHANGELOG.rst b/ur_controllers/CHANGELOG.rst index b5f530322..eb84fabe2 100644 --- a/ur_controllers/CHANGELOG.rst +++ b/ur_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package ur_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.12.0 (2026-01-13) +------------------- * Ensure latched qos is reliable (backport `#1594 `_) (`#1632 `_) * Contributors: mergify[bot] diff --git a/ur_controllers/package.xml b/ur_controllers/package.xml index 2feabb802..fc8473f73 100644 --- a/ur_controllers/package.xml +++ b/ur_controllers/package.xml @@ -2,7 +2,7 @@ ur_controllers - 2.11.0 + 2.12.0 Provides controllers that use the speed scaling interface of Universal Robots. Felix Exner diff --git a/ur_dashboard_msgs/CHANGELOG.rst b/ur_dashboard_msgs/CHANGELOG.rst index d40c2f0c5..6a69efdc3 100644 --- a/ur_dashboard_msgs/CHANGELOG.rst +++ b/ur_dashboard_msgs/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package ur_dashboard_msgs ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.12.0 (2026-01-13) +------------------- 2.11.0 (2025-12-01) ------------------- diff --git a/ur_dashboard_msgs/package.xml b/ur_dashboard_msgs/package.xml index 3ff5debf7..06142bd6f 100644 --- a/ur_dashboard_msgs/package.xml +++ b/ur_dashboard_msgs/package.xml @@ -2,7 +2,7 @@ ur_dashboard_msgs - 2.11.0 + 2.12.0 Messages around the UR Dashboard server. Felix Exner diff --git a/ur_moveit_config/CHANGELOG.rst b/ur_moveit_config/CHANGELOG.rst index 6dd08b592..4d88dc2c8 100644 --- a/ur_moveit_config/CHANGELOG.rst +++ b/ur_moveit_config/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package ur_moveit_config ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.12.0 (2026-01-13) +------------------- 2.11.0 (2025-12-01) ------------------- diff --git a/ur_moveit_config/package.xml b/ur_moveit_config/package.xml index f2f2e3e8a..f06cd7b5c 100644 --- a/ur_moveit_config/package.xml +++ b/ur_moveit_config/package.xml @@ -2,7 +2,7 @@ ur_moveit_config - 2.11.0 + 2.12.0 An example package with MoveIt2 configurations for UR robots. diff --git a/ur_robot_driver/CHANGELOG.rst b/ur_robot_driver/CHANGELOG.rst index 7a2d2dc74..53f4f7023 100644 --- a/ur_robot_driver/CHANGELOG.rst +++ b/ur_robot_driver/CHANGELOG.rst @@ -1,5 +1,5 @@ -Forthcoming ------------ +2.12.0 (2026-01-13) +------------------- * Fix flange-to-TCP wrench transformation (backport `#1615 `_) (`#1635 `_) * Initialize force mode interfaces to NaN on init (backport `#1625 `_) (`#1627 `_) * [DashboardClient] Add a parameter callback (backport `#1598 `_) (`#1604 `_) diff --git a/ur_robot_driver/package.xml b/ur_robot_driver/package.xml index 59587a26a..53197b43a 100644 --- a/ur_robot_driver/package.xml +++ b/ur_robot_driver/package.xml @@ -2,7 +2,7 @@ ur_robot_driver - 2.11.0 + 2.12.0 The new driver for Universal Robots UR3, UR5 and UR10 robots with CB3 controllers and the e-series. Felix Exner From 61bbb93e49b611a0dfb9eaf2bdb75dbbf5dd84a2 Mon Sep 17 00:00:00 2001 From: "mergify[bot]" <37929162+mergify[bot]@users.noreply.github.com> Date: Mon, 19 Jan 2026 11:39:42 +0100 Subject: [PATCH 132/133] Auto-update pre-commit hooks (backport #1643) (#1644) --- .pre-commit-config.yaml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml index 0786f1c1a..423232483 100644 --- a/.pre-commit-config.yaml +++ b/.pre-commit-config.yaml @@ -39,7 +39,7 @@ repos: args: [--py36-plus] - repo: https://github.com/psf/black - rev: 25.12.0 + rev: 26.1.0 hooks: - id: black args: ["--line-length=100"] From 5e600c38843ee0a8eeda349ff607028087c9b6fe Mon Sep 17 00:00:00 2001 From: "mergify[bot]" <37929162+mergify[bot]@users.noreply.github.com> Date: Tue, 20 Jan 2026 09:21:37 +0100 Subject: [PATCH 133/133] [Driver Tests] Unlock protective stop during test case setup (backport #1641) (#1646) * Use custom logging for ListControllersResponse * Unlock protective stop * Use a while loop instead of a fixed wait --- ur_robot_driver/test/test_common.py | 24 ++++++++++++++++-------- 1 file changed, 16 insertions(+), 8 deletions(-) diff --git a/ur_robot_driver/test/test_common.py b/ur_robot_driver/test/test_common.py index f96624538..46220cf90 100644 --- a/ur_robot_driver/test/test_common.py +++ b/ur_robot_driver/test/test_common.py @@ -112,7 +112,11 @@ def _call_service(node, client, request): rclpy.spin_until_future_complete(node, future) if future.result() is not None: - logging.info(" Received result: %s", future.result()) + response_str = str(future.result()) + if type(future.result()).__name__ == "ListControllers_Response": + controllers_str_list = [f"{c.name}: {c.state}" for c in future.result().controller] + response_str = f"controllers: [{', '.join(controllers_str_list)}]" + logging.info(" Received result: %s", response_str) return future.result() raise Exception(f"Error while calling service '{client.srv_name}': {future.exception()}") @@ -238,17 +242,21 @@ def start_robot(self): self._check_call(self.power_off()) self._check_call(self.power_on()) self._check_call(self.brake_release()) + self._check_call(self.unlock_protective_stop()) time.sleep(1) robot_mode = self.get_robot_mode() - self._check_call(robot_mode) - if robot_mode.robot_mode.mode != RobotMode.RUNNING: - raise Exception( - f"Incorrect robot mode: Expected {RobotMode.RUNNING}, got {robot_mode.robot_mode.mode}" - ) - - self._check_call(self.stop()) + start_time = time.time() + while time.time() - start_time < TIMEOUT_WAIT_SERVICE: + self._check_call(robot_mode) + if robot_mode.robot_mode.mode == RobotMode.RUNNING: + self._check_call(self.stop()) + return + time.sleep(0.1) + raise Exception( + f"Incorrect robot mode: Expected {RobotMode.RUNNING}, got {robot_mode.robot_mode.mode}" + ) def _check_call(self, result): if not result.success: