Allow setting payload inertia matrix via set_payload service backwards-compatible (#1811)

Extend set_payload service with inertia matrix and transition_time

(cherry picked from commit 3d18755)

# Conflicts:
#	ur_robot_driver/src/hardware_interface.cpp

Author: srvald

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<double(void)> 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
 

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<double>(req->transition_time.sec) + (static_cast<double>(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<double>(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<double(void)> 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<int>(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;
       }
     }

ur_robot_driver/include/ur_robot_driver/hardware_interface.hpp

@@ -270,10 +270,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_;

ur_robot_driver/resources/rtde_output_recipe.txt

@@ -29,3 +29,4 @@ actual_current
 tcp_offset
 payload
 payload_cog
+payload_inertia

ur_robot_driver/src/hardware_interface.cpp

@@ -385,6 +385,26 @@ std::vector<hardware_interface::StateInterface> 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]));
+<<<<<<< HEAD
+=======
+  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]));
+  state_interfaces.emplace_back(hardware_interface::StateInterface(tf_prefix + HW_IF_MOTION_PRIMITIVES,
+                                                                   "ready_for_new_primitive", &hw_moprim_states_[1]));
+>>>>>>> 3d18755 (Allow setting payload inertia matrix via set_payload service backwards-compatible (#1811))
 
   return state_interfaces;
 }
@@ -442,6 +462,20 @@ std::vector<hardware_interface::CommandInterface> 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_));
 
@@ -896,6 +930,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();
@@ -1027,6 +1062,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_);
@@ -1094,10 +1131,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]) &&

ur_robot_driver/test/integration_test_io_controller.py

@@ -33,13 +33,14 @@
 import time
 import unittest
 
-import launch_testing
 import pytest
 import rclpy
 from geometry_msgs.msg import Vector3
 from rclpy.node import Node
 from ur_msgs.msg import IOStates
 
+from builtin_interfaces.msg import Duration as DurationMsg
+
 sys.path.append(os.path.dirname(__file__))
 from test_common import (  # noqa: E402
     ControllerManagerInterface,
@@ -50,9 +51,8 @@
 
 
 @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)
+def generate_test_description():
+    return generate_driver_test_description()
 
 
 class IOControllerTest(unittest.TestCase):
@@ -159,3 +159,61 @@ def test_set_payload(self):
             mass=0.0, center_of_gravity=Vector3(x=0.0, y=0.0, z=0.0)
         )
         self.assertTrue(result.success, "Resetting payload via set_payload failed")
+
+    def test_set_payload_with_inertia(self):
+        """Setting mass, COG and full inertia matrix should succeed."""
+        res = self._io_status_controller_interface.set_payload(
+            mass=1.0,
+            center_of_gravity=Vector3(x=0.1, y=0.0, z=0.2),
+            ixx=0.01,
+            iyy=0.01,
+            izz=0.02,
+            ixy=0.0,
+            ixz=0.0,
+            iyz=0.0,
+            transition_time=DurationMsg(),
+        )
+        self.assertTrue(res.success)
+
+    def test_set_payload_with_transition_time(self):
+        """
+        Setting payload with transition_time > 0 should succeed.
+
+        The service should wait for the transition to complete before verifying.
+        """
+        res = self._io_status_controller_interface.set_payload(
+            mass=1.0,
+            center_of_gravity=Vector3(x=0.0, y=0.0, z=0.1),
+            ixx=0.01,
+            iyy=0.01,
+            izz=0.02,
+            transition_time=DurationMsg(sec=1, nanosec=0),
+        )
+        self.assertTrue(res.success)
+
+    def test_set_payload_updates_sequentially(self):
+        """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._io_status_controller_interface.set_payload(
+                mass=p["mass"],
+                center_of_gravity=Vector3(x=p["cx"], y=p["cy"], z=p["cz"]),
+                ixx=p["ixx"],
+                iyy=p["iyy"],
+                izz=p["izz"],
+                transition_time=DurationMsg(),
+            )
+            self.assertTrue(res.success, f"set_payload failed for m={p['mass']}")

ur_robot_driver/urdf/ur.ros2_control.xacro

@@ -213,12 +213,25 @@
         <command_interface name="cog.x"/>
         <command_interface name="cog.y"/>
         <command_interface name="cog.z"/>
+        <command_interface name="inertia.ixx"/>
+        <command_interface name="inertia.iyy"/>
+        <command_interface name="inertia.izz"/>
+        <command_interface name="inertia.ixy"/>
+        <command_interface name="inertia.ixz"/>
+        <command_interface name="inertia.iyz"/>
+        <command_interface name="transition_time"/>
         <command_interface name="payload_async_success"/>
 
         <state_interface name="mass"/>
         <state_interface name="cog.x"/>
         <state_interface name="cog.y"/>
         <state_interface name="cog.z"/>
+        <state_interface name="inertia.ixx"/>
+        <state_interface name="inertia.iyy"/>
+        <state_interface name="inertia.izz"/>
+        <state_interface name="inertia.ixy"/>
+        <state_interface name="inertia.ixz"/>
+        <state_interface name="inertia.iyz"/>
       </gpio>
 
       <gpio name="${tf_prefix}gravity">