diff --git a/ur_controllers/include/ur_controllers/gpio_controller.hpp b/ur_controllers/include/ur_controllers/gpio_controller.hpp index 2b785932d..4abcbc322 100644 --- a/ur_controllers/include/ur_controllers/gpio_controller.hpp +++ b/ur_controllers/include/ur_controllers/gpio_controller.hpp @@ -82,6 +82,13 @@ enum CommandInterfaces HAND_BACK_CONTROL_CMD = 33, HAND_BACK_CONTROL_ASYNC_SUCCESS = 34, ANALOG_OUTPUTS_DOMAIN = 35, + PAYLOAD_INERTIA_IXX = 36, + PAYLOAD_INERTIA_IYY = 37, + PAYLOAD_INERTIA_IZZ = 38, + PAYLOAD_INERTIA_IXY = 39, + PAYLOAD_INERTIA_IXZ = 40, + PAYLOAD_INERTIA_IYZ = 41, + PAYLOAD_TRANSITION_TIME = 42, }; enum StateInterfaces @@ -107,6 +114,12 @@ enum StateInterfaces PAYLOAD_STATE_COG_X = 72, PAYLOAD_STATE_COG_Y = 73, PAYLOAD_STATE_COG_Z = 74, + PAYLOAD_STATE_INERTIA_IXX = 75, + PAYLOAD_STATE_INERTIA_IYY = 76, + PAYLOAD_STATE_INERTIA_IZZ = 77, + PAYLOAD_STATE_INERTIA_IXY = 78, + PAYLOAD_STATE_INERTIA_IXZ = 79, + PAYLOAD_STATE_INERTIA_IYZ = 80, }; class GPIOController : public controller_interface::ControllerInterface @@ -210,7 +223,8 @@ class GPIOController : public controller_interface::ControllerInterface */ bool waitForAsyncCommand(std::function get_value); - bool waitForPayloadRtdeMatch(double mass, double cx, double cy, double cz); + bool waitForPayloadRtdeMatch(double mass, double cx, double cy, double cz, double ixx, double iyy, double izz, + double ixy, double ixz, double iyz, double transition_time); }; } // namespace ur_controllers diff --git a/ur_controllers/src/gpio_controller.cpp b/ur_controllers/src/gpio_controller.cpp index 989223c80..d94fb8c69 100644 --- a/ur_controllers/src/gpio_controller.cpp +++ b/ur_controllers/src/gpio_controller.cpp @@ -135,6 +135,14 @@ controller_interface::InterfaceConfiguration GPIOController::command_interface_c config.names.emplace_back(tf_prefix + "gpio/analog_output_domain_cmd"); + config.names.emplace_back(tf_prefix + "payload/inertia.ixx"); + config.names.emplace_back(tf_prefix + "payload/inertia.iyy"); + config.names.emplace_back(tf_prefix + "payload/inertia.izz"); + config.names.emplace_back(tf_prefix + "payload/inertia.ixy"); + config.names.emplace_back(tf_prefix + "payload/inertia.ixz"); + config.names.emplace_back(tf_prefix + "payload/inertia.iyz"); + config.names.emplace_back(tf_prefix + "payload/transition_time"); + return config; } @@ -200,6 +208,12 @@ controller_interface::InterfaceConfiguration ur_controllers::GPIOController::sta config.names.emplace_back(tf_prefix + "payload/cog.x"); config.names.emplace_back(tf_prefix + "payload/cog.y"); config.names.emplace_back(tf_prefix + "payload/cog.z"); + config.names.emplace_back(tf_prefix + "payload/inertia.ixx"); + config.names.emplace_back(tf_prefix + "payload/inertia.iyy"); + config.names.emplace_back(tf_prefix + "payload/inertia.izz"); + config.names.emplace_back(tf_prefix + "payload/inertia.ixy"); + config.names.emplace_back(tf_prefix + "payload/inertia.ixz"); + config.names.emplace_back(tf_prefix + "payload/inertia.iyz"); return config; } @@ -618,6 +632,15 @@ bool GPIOController::setPayload(const ur_msgs::srv::SetPayload::Request::SharedP std::ignore = command_interfaces_[CommandInterfaces::PAYLOAD_COG_X].set_value(req->center_of_gravity.x); std::ignore = command_interfaces_[CommandInterfaces::PAYLOAD_COG_Y].set_value(req->center_of_gravity.y); std::ignore = command_interfaces_[CommandInterfaces::PAYLOAD_COG_Z].set_value(req->center_of_gravity.z); + std::ignore = command_interfaces_[CommandInterfaces::PAYLOAD_INERTIA_IXX].set_value(req->ixx); + std::ignore = command_interfaces_[CommandInterfaces::PAYLOAD_INERTIA_IYY].set_value(req->iyy); + std::ignore = command_interfaces_[CommandInterfaces::PAYLOAD_INERTIA_IZZ].set_value(req->izz); + std::ignore = command_interfaces_[CommandInterfaces::PAYLOAD_INERTIA_IXY].set_value(req->ixy); + std::ignore = command_interfaces_[CommandInterfaces::PAYLOAD_INERTIA_IXZ].set_value(req->ixz); + std::ignore = command_interfaces_[CommandInterfaces::PAYLOAD_INERTIA_IYZ].set_value(req->iyz); + double transition_time_seconds = + static_cast(req->transition_time.sec) + (static_cast(req->transition_time.nanosec) * 1e-9); + std::ignore = command_interfaces_[CommandInterfaces::PAYLOAD_TRANSITION_TIME].set_value(transition_time_seconds); if (!waitForAsyncCommand([&]() { return command_interfaces_[CommandInterfaces::PAYLOAD_ASYNC_SUCCESS].get_optional().value_or(ASYNC_WAITING); @@ -636,8 +659,10 @@ bool GPIOController::setPayload(const ur_msgs::srv::SetPayload::Request::SharedP if (params_.verify_payload_on_set) { if (!waitForPayloadRtdeMatch(static_cast(req->mass), req->center_of_gravity.x, req->center_of_gravity.y, - req->center_of_gravity.z)) { - RCLCPP_WARN(get_node()->get_logger(), "setPayload reported success but RTDE payload / payload_cog do not match " + req->center_of_gravity.z, req->ixx, req->iyy, req->izz, req->ixy, req->ixz, req->iyz, + transition_time_seconds)) { + RCLCPP_WARN(get_node()->get_logger(), "setPayload reported success but RTDE payload / payload_cog / " + "payload_inertia do not match " "the " "request yet. (This might " "happen when using the mocked interface.)"); @@ -706,20 +731,35 @@ bool GPIOController::waitForAsyncCommand(std::function get_value) return true; } -bool GPIOController::waitForPayloadRtdeMatch(double mass, double cx, double cy, double cz) +bool GPIOController::waitForPayloadRtdeMatch(double mass, double cx, double cy, double cz, double ixx, double iyy, + double izz, double ixy, double ixz, double iyz, double transition_time) { constexpr double tol_mass = 1e-3; constexpr double tol_cog = 1e-4; + constexpr double tol_inertia = 1e-4; const auto maximum_retries = params_.check_io_successfull_retries; + if (transition_time > 0.0) { + std::this_thread::sleep_for(std::chrono::milliseconds(static_cast(transition_time * 1000))); + } + for (int retries = 0; retries <= maximum_retries; ++retries) { const auto m = state_interfaces_[StateInterfaces::PAYLOAD_STATE_MASS].get_optional(); const auto sx = state_interfaces_[StateInterfaces::PAYLOAD_STATE_COG_X].get_optional(); const auto sy = state_interfaces_[StateInterfaces::PAYLOAD_STATE_COG_Y].get_optional(); const auto sz = state_interfaces_[StateInterfaces::PAYLOAD_STATE_COG_Z].get_optional(); - if (m && sx && sy && sz) { + const auto sixx = state_interfaces_[StateInterfaces::PAYLOAD_STATE_INERTIA_IXX].get_optional(); + const auto siyy = state_interfaces_[StateInterfaces::PAYLOAD_STATE_INERTIA_IYY].get_optional(); + const auto sizz = state_interfaces_[StateInterfaces::PAYLOAD_STATE_INERTIA_IZZ].get_optional(); + const auto sixy = state_interfaces_[StateInterfaces::PAYLOAD_STATE_INERTIA_IXY].get_optional(); + const auto sixz = state_interfaces_[StateInterfaces::PAYLOAD_STATE_INERTIA_IXZ].get_optional(); + const auto siyz = state_interfaces_[StateInterfaces::PAYLOAD_STATE_INERTIA_IYZ].get_optional(); + if (m && sx && sy && sz && sixx && siyy && sizz && sixy && sixz && siyz) { if (std::abs(*m - mass) <= tol_mass && std::abs(*sx - cx) <= tol_cog && std::abs(*sy - cy) <= tol_cog && - std::abs(*sz - cz) <= tol_cog) { + std::abs(*sz - cz) <= tol_cog && std::abs(*sixx - ixx) <= tol_inertia && + std::abs(*siyy - iyy) <= tol_inertia && std::abs(*sizz - izz) <= tol_inertia && + std::abs(*sixy - ixy) <= tol_inertia && std::abs(*sixz - ixz) <= tol_inertia && + std::abs(*siyz - iyz) <= tol_inertia) { return true; } } diff --git a/ur_robot_driver/CMakeLists.txt b/ur_robot_driver/CMakeLists.txt index 57e4f8cc3..6222bb2bd 100644 --- a/ur_robot_driver/CMakeLists.txt +++ b/ur_robot_driver/CMakeLists.txt @@ -286,6 +286,10 @@ if(BUILD_TESTING) TIMEOUT 800 ) + add_launch_test(test/integration_test_payload.py + TIMEOUT + 800 + ) add_launch_test(test/integration_test_force_mode.py TIMEOUT 800 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 885660117..ef42314fa 100644 --- a/ur_robot_driver/include/ur_robot_driver/hardware_interface.hpp +++ b/ur_robot_driver/include/ur_robot_driver/hardware_interface.hpp @@ -284,10 +284,13 @@ class URPositionHardwareInterface : public hardware_interface::SystemInterface // Payload stuff urcl::vector3d_t payload_center_of_gravity_; + urcl::vector6d_t payload_inertia_; double payload_mass_; + double payload_transition_time_; double payload_async_success_; double rtde_payload_mass_ = 0.0; urcl::vector3d_t rtde_payload_cog_{ 0.0, 0.0, 0.0 }; + urcl::vector6d_t rtde_payload_inertia_{ 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 }; // Friction model parameters urcl::vector6d_t friction_model_viscous_; diff --git a/ur_robot_driver/resources/rtde_output_recipe.txt b/ur_robot_driver/resources/rtde_output_recipe.txt index b322592c2..964446500 100644 --- a/ur_robot_driver/resources/rtde_output_recipe.txt +++ b/ur_robot_driver/resources/rtde_output_recipe.txt @@ -29,3 +29,4 @@ actual_current tcp_offset payload payload_cog +payload_inertia diff --git a/ur_robot_driver/src/hardware_interface.cpp b/ur_robot_driver/src/hardware_interface.cpp index c658e82e5..3e8508fae 100644 --- a/ur_robot_driver/src/hardware_interface.cpp +++ b/ur_robot_driver/src/hardware_interface.cpp @@ -415,7 +415,18 @@ std::vector URPositionHardwareInterface::exp hardware_interface::StateInterface(tf_prefix + "payload", "cog.y", &rtde_payload_cog_[1])); state_interfaces.emplace_back( hardware_interface::StateInterface(tf_prefix + "payload", "cog.z", &rtde_payload_cog_[2])); - + state_interfaces.emplace_back( + hardware_interface::StateInterface(tf_prefix + "payload", "inertia.ixx", &rtde_payload_inertia_[0])); + state_interfaces.emplace_back( + hardware_interface::StateInterface(tf_prefix + "payload", "inertia.iyy", &rtde_payload_inertia_[1])); + state_interfaces.emplace_back( + hardware_interface::StateInterface(tf_prefix + "payload", "inertia.izz", &rtde_payload_inertia_[2])); + state_interfaces.emplace_back( + hardware_interface::StateInterface(tf_prefix + "payload", "inertia.ixy", &rtde_payload_inertia_[3])); + state_interfaces.emplace_back( + hardware_interface::StateInterface(tf_prefix + "payload", "inertia.ixz", &rtde_payload_inertia_[4])); + state_interfaces.emplace_back( + hardware_interface::StateInterface(tf_prefix + "payload", "inertia.iyz", &rtde_payload_inertia_[5])); // Motion primitives stuff state_interfaces.emplace_back(hardware_interface::StateInterface(tf_prefix + HW_IF_MOTION_PRIMITIVES, "execution_status", &hw_moprim_states_[0])); @@ -478,6 +489,20 @@ std::vector URPositionHardwareInterface::e hardware_interface::CommandInterface(tf_prefix + "payload", "cog.y", &payload_center_of_gravity_[1])); command_interfaces.emplace_back( hardware_interface::CommandInterface(tf_prefix + "payload", "cog.z", &payload_center_of_gravity_[2])); + command_interfaces.emplace_back( + hardware_interface::CommandInterface(tf_prefix + "payload", "inertia.ixx", &payload_inertia_[0])); + command_interfaces.emplace_back( + hardware_interface::CommandInterface(tf_prefix + "payload", "inertia.iyy", &payload_inertia_[1])); + command_interfaces.emplace_back( + hardware_interface::CommandInterface(tf_prefix + "payload", "inertia.izz", &payload_inertia_[2])); + command_interfaces.emplace_back( + hardware_interface::CommandInterface(tf_prefix + "payload", "inertia.ixy", &payload_inertia_[3])); + command_interfaces.emplace_back( + hardware_interface::CommandInterface(tf_prefix + "payload", "inertia.ixz", &payload_inertia_[4])); + command_interfaces.emplace_back( + hardware_interface::CommandInterface(tf_prefix + "payload", "inertia.iyz", &payload_inertia_[5])); + command_interfaces.emplace_back( + hardware_interface::CommandInterface(tf_prefix + "payload", "transition_time", &payload_transition_time_)); command_interfaces.emplace_back( hardware_interface::CommandInterface(tf_prefix + "payload", "payload_async_success", &payload_async_success_)); @@ -999,6 +1024,7 @@ hardware_interface::return_type URPositionHardwareInterface::read(const rclcpp:: readData(data_package_buffer_, "tcp_offset", tcp_offset_); readData(data_package_buffer_, "payload", rtde_payload_mass_); readData(data_package_buffer_, "payload_cog", rtde_payload_cog_); + readData(data_package_buffer_, "payload_inertia", rtde_payload_inertia_); // required transforms extractToolPose(); @@ -1138,6 +1164,8 @@ void URPositionHardwareInterface::initAsyncIO() payload_mass_ = NO_NEW_CMD_; payload_center_of_gravity_ = { NO_NEW_CMD_, NO_NEW_CMD_, NO_NEW_CMD_ }; + payload_inertia_ = { NO_NEW_CMD_, NO_NEW_CMD_, NO_NEW_CMD_, NO_NEW_CMD_, NO_NEW_CMD_, NO_NEW_CMD_ }; + payload_transition_time_ = NO_NEW_CMD_; gravity_vector_ = { NO_NEW_CMD_, NO_NEW_CMD_, NO_NEW_CMD_ }; friction_model_viscous_.fill(NO_NEW_CMD_); @@ -1205,10 +1233,16 @@ void URPositionHardwareInterface::checkAsyncIO() if (!std::isnan(payload_mass_) && !std::isnan(payload_center_of_gravity_[0]) && !std::isnan(payload_center_of_gravity_[1]) && !std::isnan(payload_center_of_gravity_[2]) && - ur_driver_ != nullptr) { - payload_async_success_ = ur_driver_->setPayload(payload_mass_, payload_center_of_gravity_); + !std::isnan(payload_inertia_[0]) && !std::isnan(payload_inertia_[1]) && !std::isnan(payload_inertia_[2]) && + !std::isnan(payload_inertia_[3]) && !std::isnan(payload_inertia_[4]) && !std::isnan(payload_inertia_[5]) && + !std::isnan(payload_transition_time_) && ur_driver_ != nullptr) { + payload_async_success_ = ur_driver_->setTargetPayload(payload_mass_, payload_center_of_gravity_, payload_inertia_, + payload_transition_time_); + payload_mass_ = NO_NEW_CMD_; payload_center_of_gravity_ = { NO_NEW_CMD_, NO_NEW_CMD_, NO_NEW_CMD_ }; + payload_inertia_ = { NO_NEW_CMD_, NO_NEW_CMD_, NO_NEW_CMD_, NO_NEW_CMD_, NO_NEW_CMD_, NO_NEW_CMD_ }; + payload_transition_time_ = NO_NEW_CMD_; } if (!std::isnan(gravity_vector_[0]) && !std::isnan(gravity_vector_[1]) && !std::isnan(gravity_vector_[2]) && diff --git a/ur_robot_driver/test/integration_test_payload.py b/ur_robot_driver/test/integration_test_payload.py new file mode 100644 index 000000000..7ed19d853 --- /dev/null +++ b/ur_robot_driver/test/integration_test_payload.py @@ -0,0 +1,189 @@ +#!/usr/bin/env python +# Copyright 2026, 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 geometry_msgs.msg import Vector3 +from builtin_interfaces.msg import Duration +from ur_msgs.srv import SetPayload + +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) + self._set_payload_client = self.node.create_client( + SetPayload, "/io_and_status_controller/set_payload" + ) + + def setUp(self): + self._dashboard_interface.start_robot() + time.sleep(1) + self.assertTrue(self._io_status_controller_interface.resend_robot_program().success) + # Reset payload to a known zero state before each test + self._call_set_payload(mass=0.0) + + def _call_set_payload( + self, + mass=0.0, + cx=0.0, + cy=0.0, + cz=0.0, + ixx=0.0, + ixy=0.0, + ixz=0.0, + iyy=0.0, + iyz=0.0, + izz=0.0, + transition_time=0.0, + ): + """Call the set_payload service and return the response.""" + req = SetPayload.Request() + req.mass = float(mass) + req.center_of_gravity = Vector3(x=float(cx), y=float(cy), z=float(cz)) + req.ixx = float(ixx) + req.ixy = float(ixy) + req.ixz = float(ixz) + req.iyy = float(iyy) + req.iyz = float(iyz) + req.izz = float(izz) + seconds = int(transition_time) + nanoseconds = int(round((transition_time - seconds) * 1e9)) + req.transition_time = Duration(sec=seconds, nanosec=nanoseconds) + + future = self._set_payload_client.call_async(req) + rclpy.spin_until_future_complete(self.node, future) + if future.result() is None: + raise Exception(f"set_payload service call failed: {future.exception()}") + return future.result() + + def test_set_payload_with_inertia(self, tf_prefix): + """Setting mass, COG and full inertia matrix should succeed.""" + res = self._call_set_payload( + mass=1.0, + cx=0.1, + cy=0.0, + cz=0.2, + ixx=0.01, + iyy=0.01, + izz=0.02, + ixy=0.0, + ixz=0.0, + iyz=0.0, + transition_time=0.0, + ) + self.assertTrue(res.success) + + def test_set_payload_with_transition_time(self, tf_prefix): + """ + Setting payload with transition_time > 0 should succeed. + + The service should wait for the transition to complete before verifying. + """ + res = self._call_set_payload( + mass=1.0, + cx=0.0, + cy=0.0, + cz=0.1, + ixx=0.01, + iyy=0.01, + izz=0.02, + transition_time=1.0, + ) + self.assertTrue(res.success) + + def test_set_payload_updates_sequentially(self, tf_prefix): + """Multiple sequential set_payload calls should all succeed.""" + payloads = [ + {"mass": 0.5, "cx": 0.0, "cy": 0.0, "cz": 0.1, "ixx": 0.005, "iyy": 0.005, "izz": 0.01}, + {"mass": 1.0, "cx": 0.1, "cy": 0.0, "cz": 0.2, "ixx": 0.01, "iyy": 0.01, "izz": 0.02}, + { + "mass": 2.0, + "cx": 0.05, + "cy": 0.05, + "cz": 0.15, + "ixx": 0.02, + "iyy": 0.02, + "izz": 0.04, + }, + {"mass": 0.0, "cx": 0.0, "cy": 0.0, "cz": 0.0, "ixx": 0.0, "iyy": 0.0, "izz": 0.0}, + ] + for p in payloads: + res = self._call_set_payload( + mass=p["mass"], + cx=p["cx"], + cy=p["cy"], + cz=p["cz"], + ixx=p["ixx"], + iyy=p["iyy"], + izz=p["izz"], + transition_time=0.0, + ) + self.assertTrue(res.success, f"set_payload failed for m={p['mass']}") diff --git a/ur_robot_driver/urdf/ur.ros2_control.xacro b/ur_robot_driver/urdf/ur.ros2_control.xacro index e8404741d..6d6e11cf6 100644 --- a/ur_robot_driver/urdf/ur.ros2_control.xacro +++ b/ur_robot_driver/urdf/ur.ros2_control.xacro @@ -213,12 +213,25 @@ + + + + + + + + + + + + +