Check payload state in gpio_controller (backport #1770) (#1775)

Before, we were returning success, as soon as the hardware interface
attempted to setup the payload.

This change reads the payload information from the robot and only
returns success if the read payload matches with what the user
requested.

This is active by default, but will be inactive when using mock
hardware.

Add tests verifying setPayload works with URSim and mock hardware

Author: mergify[bot]

ur_controllers/include/ur_controllers/gpio_controller.hpp

@@ -102,6 +102,10 @@ enum StateInterfaces
   SAFETY_STATUS_BITS = 58,
   INITIALIZED_FLAG = 69,
   PROGRAM_RUNNING = 70,
+  PAYLOAD_STATE_MASS = 71,
+  PAYLOAD_STATE_COG_X = 72,
+  PAYLOAD_STATE_COG_Y = 73,
+  PAYLOAD_STATE_COG_Z = 74,
 };
 
 class GPIOController : public controller_interface::ControllerInterface
@@ -196,6 +200,8 @@ class GPIOController : public controller_interface::ControllerInterface
    * have been reached
    */
   bool waitForAsyncCommand(std::function<double(void)> get_value);
+
+  bool waitForPayloadRtdeMatch(double mass, double cx, double cy, double cz);
 };
 }  // namespace ur_controllers
 

ur_controllers/src/gpio_controller.cpp

@@ -37,6 +37,7 @@
 
 #include "ur_controllers/gpio_controller.hpp"
 
+#include <cmath>
 #include <string>
 
 namespace ur_controllers
@@ -160,6 +161,11 @@ controller_interface::InterfaceConfiguration ur_controllers::GPIOController::sta
   // program running
   config.names.emplace_back(tf_prefix + "gpio/program_running");
 
+  config.names.emplace_back(tf_prefix + "payload/mass");
+  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");
+
   return config;
 }
 
@@ -530,13 +536,25 @@ bool GPIOController::setPayload(const ur_msgs::srv::SetPayload::Request::SharedP
 
   resp->success = static_cast<bool>(command_interfaces_[CommandInterfaces::PAYLOAD_ASYNC_SUCCESS].get_value());
 
-  if (resp->success) {
-    RCLCPP_INFO(get_node()->get_logger(), "Payload has been set successfully");
-  } else {
+  if (!resp->success) {
     RCLCPP_ERROR(get_node()->get_logger(), "Could not set the payload");
     return false;
   }
 
+  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 "
+                                            "the "
+                                            "request yet. (This might "
+                                            "happen when using the mocked interface.)");
+      resp->success = false;
+      RCLCPP_ERROR(get_node()->get_logger(), "Payload RTDE verification failed");
+      return false;
+    }
+
+    RCLCPP_INFO(get_node()->get_logger(), "Payload has been set and verified against RTDE feedback");
+  }
   return true;
 }
 
@@ -588,6 +606,26 @@ bool GPIOController::waitForAsyncCommand(std::function<double(void)> get_value)
   return true;
 }
 
+bool GPIOController::waitForPayloadRtdeMatch(double mass, double cx, double cy, double cz)
+{
+  constexpr double tol_mass = 1e-3;
+  constexpr double tol_cog = 1e-4;
+  const auto maximum_retries = params_.check_io_successfull_retries;
+
+  for (int retries = 0; retries <= maximum_retries; ++retries) {
+    const auto m = state_interfaces_[StateInterfaces::PAYLOAD_STATE_MASS].get_value();
+    const auto sx = state_interfaces_[StateInterfaces::PAYLOAD_STATE_COG_X].get_value();
+    const auto sy = state_interfaces_[StateInterfaces::PAYLOAD_STATE_COG_Y].get_value();
+    const auto sz = state_interfaces_[StateInterfaces::PAYLOAD_STATE_COG_Z].get_value();
+    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) {
+      return true;
+    }
+    std::this_thread::sleep_for(std::chrono::milliseconds(50));
+  }
+  return false;
+}
+
 }  // namespace ur_controllers
 
 #include "pluginlib/class_list_macros.hpp"

ur_controllers/src/gpio_controller_parameters.yaml

@@ -9,3 +9,8 @@ gpio_controller:
       default_value: 10,
       description: "Amount of retries for checking if the selected gpio was set successfully"
    }
+   verify_payload_on_set: {
+      type: bool,
+      default_value: true,
+      description: "Whether to check if the payload of the set command is correct. If false, the controller will only check if the command was successful, but not if the correct value was set. Setting this to false can be useful if the controller is used on a robot with mock hardware or GZ simulation."
+   }

ur_robot_driver/include/ur_robot_driver/hardware_interface.hpp

@@ -264,6 +264,8 @@ class URPositionHardwareInterface : public hardware_interface::SystemInterface
   urcl::vector3d_t payload_center_of_gravity_;
   double payload_mass_;
   double payload_async_success_;
+  double rtde_payload_mass_ = 0.0;
+  urcl::vector3d_t rtde_payload_cog_{ 0.0, 0.0, 0.0 };
 
   // Friction model parameters
   urcl::vector6d_t friction_model_viscous_;

ur_robot_driver/launch/ur_control.launch.py

@@ -235,6 +235,7 @@ def launch_setup(context, *args, **kwargs):
             robot_description,
             update_rate_config_file,
             ParameterFile(initial_joint_controllers, allow_substs=True),
+            {"verify_payload_on_set": NotSubstitution(use_fake_hardware)},
         ],
         output="screen",
         condition=IfCondition(use_fake_hardware),
@@ -247,6 +248,7 @@ def launch_setup(context, *args, **kwargs):
             robot_description,
             update_rate_config_file,
             ParameterFile(initial_joint_controllers, allow_substs=True),
+            {"verify_payload_on_set": NotSubstitution(use_fake_hardware)},
         ],
         output="screen",
         condition=UnlessCondition(use_fake_hardware),

ur_robot_driver/resources/rtde_output_recipe.txt

@@ -27,3 +27,5 @@ robot_status_bits
 safety_status_bits
 actual_current
 tcp_offset
+payload
+payload_cog

ur_robot_driver/src/hardware_interface.cpp

@@ -341,6 +341,14 @@ std::vector<hardware_interface::StateInterface> URPositionHardwareInterface::exp
   state_interfaces.emplace_back(
       hardware_interface::StateInterface(tf_prefix + TOOL_CONTACT_GPIO, "tool_contact_state", &tool_contact_state_));
 
+  state_interfaces.emplace_back(hardware_interface::StateInterface(tf_prefix + "payload", "mass", &rtde_payload_mass_));
+  state_interfaces.emplace_back(
+      hardware_interface::StateInterface(tf_prefix + "payload", "cog.x", &rtde_payload_cog_[0]));
+  state_interfaces.emplace_back(
+      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]));
+
   return state_interfaces;
 }
 
@@ -813,6 +821,8 @@ hardware_interface::return_type URPositionHardwareInterface::read(const rclcpp::
     readBitsetData<uint32_t>(data_package_buffer_, "analog_io_types", analog_io_types_);
     readBitsetData<uint32_t>(data_package_buffer_, "tool_analog_input_types", tool_analog_input_types_);
     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_);
 
     // required transforms
     extractToolPose();

ur_robot_driver/test/integration_test_io_controller.py

@@ -36,6 +36,7 @@
 import launch_testing
 import pytest
 import rclpy
+from geometry_msgs.msg import Vector3
 from rclpy.node import Node
 from ur_msgs.msg import IOStates
 
@@ -130,3 +131,31 @@ def io_msg_cb(msg):
 
         # Clean up io subscription
         self.node.destroy_subscription(io_states_sub)
+
+    def test_set_payload(self):
+        """
+        Test that setting a payload succeeds and the value is verified against RTDE feedback.
+
+        With ``verify_payload_on_set`` enabled (default for the real driver), the
+        controller only reports success after the requested payload mass and
+        center-of-gravity have been confirmed via the RTDE state interfaces.
+        """
+        # Set a non-default payload so we can detect the change reliably
+        mass = 1.5
+        cog = Vector3(x=0.01, y=0.02, z=0.03)
+
+        logging.info("Setting payload to mass=%f, cog=(%f, %f, %f)", mass, cog.x, cog.y, cog.z)
+        result = self._io_status_controller_interface.set_payload(mass=mass, center_of_gravity=cog)
+        self.assertTrue(
+            result.success,
+            "set_payload returned success=False. With verify_payload_on_set=true the "
+            "controller only returns success once the RTDE feedback matches the request.",
+        )
+
+        # Reset the payload to zero and verify again. This makes sure the verification
+        # logic also detects subsequent changes and is not just matching the initial state.
+        logging.info("Resetting payload to zero")
+        result = self._io_status_controller_interface.set_payload(
+            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")

ur_robot_driver/test/test_common.py

@@ -73,7 +73,13 @@
     SetOperationalMode,
     SetUserRole,
 )
-from ur_msgs.srv import SetIO, GetRobotSoftwareVersion, SetForceMode, SetFrictionModelParameters
+from ur_msgs.srv import (
+    SetIO,
+    SetPayload,
+    GetRobotSoftwareVersion,
+    SetForceMode,
+    SetFrictionModelParameters,
+)
 from builtin_interfaces.msg import Duration
 from control_msgs.action import FollowJointTrajectory
 from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint
@@ -326,6 +332,7 @@ class IoStatusInterface(
     initial_services={"set_io": SetIO},
     services={
         "resend_robot_program": Trigger,
+        "set_payload": SetPayload,
     },
 ):
     pass

ur_robot_driver/test/test_mock_hardware.py

@@ -35,6 +35,7 @@
 import launch_testing
 import pytest
 import rclpy
+from geometry_msgs.msg import Vector3
 from rclpy.node import Node
 from control_msgs.action import FollowJointTrajectory
 from controller_manager_msgs.srv import SwitchController
@@ -120,3 +121,26 @@ def test_trajectory(self, tf_prefix):
 
     def test_illegal_trajectory(self, tf_prefix):
         sjtc_illegal_trajectory_test(self, tf_prefix)
+
+    def test_set_payload(self):
+        """
+        Test that ``set_payload`` succeeds with mock hardware.
+
+        Mock hardware does not feed back the payload via RTDE, so the controller
+        is launched with ``verify_payload_on_set:=false`` (set automatically by
+        ``ur_control.launch.py`` when ``use_mock_hardware:=true``). The service
+        should therefore return success without performing the RTDE verification.
+        """
+        result = self._io_status_controller_interface.set_payload(
+            mass=1.5, center_of_gravity=Vector3(x=0.01, y=0.02, z=0.03)
+        )
+        self.assertTrue(
+            result.success,
+            "set_payload returned success=False on mock hardware. The controller "
+            "should be launched with verify_payload_on_set=false in this case.",
+        )
+
+        result = self._io_status_controller_interface.set_payload(
+            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 on mock hardware")