From d46556167c3d26359adadde2a89f837b1a8b0798 Mon Sep 17 00:00:00 2001
From: Felix Exner <feex@universal-robots.com>
Date: Wed, 18 Dec 2024 08:18:04 +0100
Subject: [PATCH] Fix running force_mode controller alongside passthrough
 trajectory controller (#1210)

This probably got mixed up when merging the two together. This allows
using force_mode together with other controllers.

Starting of the force_mode_controller is still only allowed for the
passthrough trajectory controller using the prepareSwitch mechanism.
---
 ur_robot_driver/src/hardware_interface.cpp    | 14 ++---
 .../test/integration_test_force_mode.py       | 62 ++++++++++++++-----
 2 files changed, 55 insertions(+), 21 deletions(-)

diff --git a/ur_robot_driver/src/hardware_interface.cpp b/ur_robot_driver/src/hardware_interface.cpp
index e06dc537..6ec54929 100644
--- a/ur_robot_driver/src/hardware_interface.cpp
+++ b/ur_robot_driver/src/hardware_interface.cpp
@@ -749,14 +749,14 @@ hardware_interface::return_type URPositionHardwareInterface::write(const rclcpp:
       check_passthrough_trajectory_controller();
     } else {
       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();
-      }
+    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;
diff --git a/ur_robot_driver/test/integration_test_force_mode.py b/ur_robot_driver/test/integration_test_force_mode.py
index d7462f0c..f4b555cf 100644
--- a/ur_robot_driver/test/integration_test_force_mode.py
+++ b/ur_robot_driver/test/integration_test_force_mode.py
@@ -116,19 +116,9 @@ 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
@@ -237,6 +227,49 @@ 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=[
+                    "force_mode_controller",
+                ],
+                deactivate_controllers=[
+                    "scaled_joint_trajectory_controller",
+                    "joint_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(
@@ -443,7 +476,8 @@ 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,
         )
 
     def test_params_out_of_range_fails(self, tf_prefix):