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)" - diff --git a/ur_robot_driver/src/hardware_interface.cpp b/ur_robot_driver/src/hardware_interface.cpp index 414032a39..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() @@ -1398,7 +1414,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 +1432,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()) { 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 + )