From 3c86dd68f169fc29de2a8b2002d9b667513a178d Mon Sep 17 00:00:00 2001 From: Felix Exner Date: Mon, 3 Nov 2025 15:39:45 +0000 Subject: [PATCH 1/3] Fix EOL in ur_controllers.yaml --- ur_robot_driver/config/ur_controllers.yaml | 1 - 1 file changed, 1 deletion(-) diff --git a/ur_robot_driver/config/ur_controllers.yaml b/ur_robot_driver/config/ur_controllers.yaml index 00d9185ed..16d986509 100644 --- a/ur_robot_driver/config/ur_controllers.yaml +++ b/ur_robot_driver/config/ur_controllers.yaml @@ -214,4 +214,3 @@ tool_contact_controller: motion_primitive_forward_controller: ros__parameters: tf_prefix: "$(var tf_prefix)" - From 29fb9e9b883c5d61160123e66decb0b921806570 Mon Sep 17 00:00:00 2001 From: Felix Exner Date: Mon, 3 Nov 2025 15:41:23 +0000 Subject: [PATCH 2/3] Remove logging statements from perform_switch method --- ur_robot_driver/src/hardware_interface.cpp | 3 --- 1 file changed, 3 deletions(-) diff --git a/ur_robot_driver/src/hardware_interface.cpp b/ur_robot_driver/src/hardware_interface.cpp index 414032a39..ec904d4aa 100644 --- a/ur_robot_driver/src/hardware_interface.cpp +++ b/ur_robot_driver/src/hardware_interface.cpp @@ -1398,7 +1398,6 @@ hardware_interface::return_type URPositionHardwareInterface::perform_command_mod } 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(get_logger(), "Stopping passthrough trajectory controller."); passthrough_trajectory_controller_running_ = false; passthrough_trajectory_abort_ = 1.0; trajectory_joint_positions_.clear(); @@ -1417,8 +1416,6 @@ hardware_interface::return_type URPositionHardwareInterface::perform_command_mod resetMoprimCmdInterfaces(); current_moprim_execution_status_ = MoprimExecutionState::IDLE; ready_for_new_moprim_ = false; - - RCLCPP_INFO(get_logger(), "Motion primitives mode stopped."); } if (stop_modes_.size() != 0 && std::find(stop_modes_[0].begin(), stop_modes_[0].end(), StoppingInterface::STOP_TOOL_CONTACT) != stop_modes_[0].end()) { From bf586b414d2d8f76c3ebc471a7f29881b7271ee0 Mon Sep 17 00:00:00 2001 From: Felix Exner Date: Mon, 3 Nov 2025 15:55:54 +0000 Subject: [PATCH 3/3] Add cross-controller compatibility checks --- ur_robot_driver/src/hardware_interface.cpp | 16 +++ .../integration_test_controller_switch.py | 107 ++++++++++++++++++ 2 files changed, 123 insertions(+) diff --git a/ur_robot_driver/src/hardware_interface.cpp b/ur_robot_driver/src/hardware_interface.cpp index ec904d4aa..376f20409 100644 --- a/ur_robot_driver/src/hardware_interface.cpp +++ b/ur_robot_driver/src/hardware_interface.cpp @@ -68,6 +68,7 @@ URPositionHardwareInterface::URPositionHardwareInterface() 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_POSITION][HW_IF_MOTION_PRIMITIVES] = false; 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; @@ -75,6 +76,7 @@ URPositionHardwareInterface::URPositionHardwareInterface() 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_VELOCITY][HW_IF_MOTION_PRIMITIVES] = false; 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; @@ -82,6 +84,7 @@ URPositionHardwareInterface::URPositionHardwareInterface() 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_[hardware_interface::HW_IF_EFFORT][HW_IF_MOTION_PRIMITIVES] = false; mode_compatibility_[FORCE_MODE_GPIO][hardware_interface::HW_IF_POSITION] = false; mode_compatibility_[FORCE_MODE_GPIO][hardware_interface::HW_IF_VELOCITY] = false; @@ -89,6 +92,7 @@ URPositionHardwareInterface::URPositionHardwareInterface() 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_[FORCE_MODE_GPIO][HW_IF_MOTION_PRIMITIVES] = true; mode_compatibility_[PASSTHROUGH_GPIO][hardware_interface::HW_IF_POSITION] = false; mode_compatibility_[PASSTHROUGH_GPIO][hardware_interface::HW_IF_VELOCITY] = false; @@ -96,6 +100,7 @@ URPositionHardwareInterface::URPositionHardwareInterface() 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_[PASSTHROUGH_GPIO][HW_IF_MOTION_PRIMITIVES] = false; mode_compatibility_[FREEDRIVE_MODE_GPIO][hardware_interface::HW_IF_POSITION] = false; mode_compatibility_[FREEDRIVE_MODE_GPIO][hardware_interface::HW_IF_VELOCITY] = false; @@ -103,6 +108,8 @@ URPositionHardwareInterface::URPositionHardwareInterface() 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_[FREEDRIVE_MODE_GPIO][TOOL_CONTACT_GPIO] = false; + mode_compatibility_[FREEDRIVE_MODE_GPIO][HW_IF_MOTION_PRIMITIVES] = false; mode_compatibility_[TOOL_CONTACT_GPIO][hardware_interface::HW_IF_POSITION] = true; mode_compatibility_[TOOL_CONTACT_GPIO][hardware_interface::HW_IF_VELOCITY] = true; @@ -110,6 +117,15 @@ URPositionHardwareInterface::URPositionHardwareInterface() 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; + mode_compatibility_[TOOL_CONTACT_GPIO][HW_IF_MOTION_PRIMITIVES] = true; + + mode_compatibility_[HW_IF_MOTION_PRIMITIVES][hardware_interface::HW_IF_POSITION] = false; + mode_compatibility_[HW_IF_MOTION_PRIMITIVES][hardware_interface::HW_IF_VELOCITY] = false; + mode_compatibility_[HW_IF_MOTION_PRIMITIVES][hardware_interface::HW_IF_EFFORT] = false; + mode_compatibility_[HW_IF_MOTION_PRIMITIVES][FORCE_MODE_GPIO] = true; + mode_compatibility_[HW_IF_MOTION_PRIMITIVES][PASSTHROUGH_GPIO] = false; + mode_compatibility_[HW_IF_MOTION_PRIMITIVES][FREEDRIVE_MODE_GPIO] = false; + mode_compatibility_[HW_IF_MOTION_PRIMITIVES][TOOL_CONTACT_GPIO] = true; } URPositionHardwareInterface::~URPositionHardwareInterface() diff --git a/ur_robot_driver/test/integration_test_controller_switch.py b/ur_robot_driver/test/integration_test_controller_switch.py index 7365607db..2f4f46f9a 100644 --- a/ur_robot_driver/test/integration_test_controller_switch.py +++ b/ur_robot_driver/test/integration_test_controller_switch.py @@ -56,6 +56,7 @@ "passthrough_trajectory_controller", "force_mode_controller", "freedrive_mode_controller", + "motion_primitive_forward_controller", ] @@ -483,3 +484,109 @@ def test_tool_contact_compatibility(self): ], ).ok ) + + self.assertTrue( + self._controller_manager_interface.switch_controller( + strictness=SwitchController.Request.STRICT, + activate_controllers=[ + "motion_primitive_forward_controller", + "tool_contact_controller", + ], + ).ok + ) + self.assertTrue( + self._controller_manager_interface.switch_controller( + strictness=SwitchController.Request.STRICT, + deactivate_controllers=[ + "motion_primitive_forward_controller", + "tool_contact_controller", + ], + ).ok + ) + + def test_moprim_compatibility(self): + # Deactivate all writing controllers + self.assertTrue( + self._controller_manager_interface.switch_controller( + strictness=SwitchController.Request.BEST_EFFORT, + deactivate_controllers=ALL_CONTROLLERS, + ).ok + ) + + time.sleep(3) + + # moprim controller should not start with any other joint controller + self.assertFalse( + self._controller_manager_interface.switch_controller( + strictness=SwitchController.Request.STRICT, + activate_controllers=[ + "motion_primitive_forward_controller", + "joint_trajectory_controller", + ], + ).ok + ) + self.assertFalse( + self._controller_manager_interface.switch_controller( + strictness=SwitchController.Request.STRICT, + activate_controllers=[ + "motion_primitive_forward_controller", + "forward_effort_controller", + ], + ).ok + ) + self.assertFalse( + self._controller_manager_interface.switch_controller( + strictness=SwitchController.Request.STRICT, + activate_controllers=[ + "motion_primitive_forward_controller", + "forward_velocity_controller", + ], + ).ok + ) + self.assertFalse( + self._controller_manager_interface.switch_controller( + strictness=SwitchController.Request.STRICT, + activate_controllers=[ + "motion_primitive_forward_controller", + "forward_position_controller", + ], + ).ok + ) + self.assertFalse( + self._controller_manager_interface.switch_controller( + strictness=SwitchController.Request.STRICT, + activate_controllers=[ + "motion_primitive_forward_controller", + "passthrough_trajectory_controller", + ], + ).ok + ) + self.assertFalse( + self._controller_manager_interface.switch_controller( + strictness=SwitchController.Request.STRICT, + activate_controllers=[ + "motion_primitive_forward_controller", + "freedrive_mode_controller", + ], + ).ok + ) + + # MoPrim controller and force_mode should be possible to combine + self.assertTrue( + self._controller_manager_interface.switch_controller( + strictness=SwitchController.Request.STRICT, + activate_controllers=[ + "motion_primitive_forward_controller", + "force_mode_controller", + ], + ).ok + ) + self.assertTrue( + self._controller_manager_interface.switch_controller( + strictness=SwitchController.Request.STRICT, + deactivate_controllers=[ + "motion_primitive_forward_controller", + "force_mode_controller", + ], + ).ok + )