feat: 记录夹爪逻辑执行状态

xr_rm_teleop/test/test_initial_joint_pose.py

@@ -6,13 +6,14 @@ from types import ModuleType, SimpleNamespace
 import pytest
 import yaml
 
-from xr_rm_teleop import realman_adapter
+from xr_rm_teleop import fun_peripheral, realman_adapter
 from xr_rm_teleop.realman_adapter import RealManAdapter
 from xr_rm_teleop.realman_adapter import MockRealManAdapter
 from xr_rm_teleop.fun_peripheral import (
     PeripheralConfig,
     _configure_tool_frame,
     load_peripheral_config,
+    peripheral_cfg,
 )
 
 
@@ -82,6 +83,78 @@ def test_deployed_peripheral_config_selects_left_and_right_tools() -> None:
     assert right.tool_pose == [0.0, 0.0, 0.14, 0.0, 0.0, 0.0, 1.0]
 
 
+def test_right_tool_initializes_open_only_for_right_arm() -> None:
+    config_file = CONFIG_DIR / "peripherals_rm75.yaml"
+    left = load_peripheral_config(str(config_file), "left")
+    right = load_peripheral_config(str(config_file), "right")
+
+    assert not left.set_initial_tool_state
+    assert right.set_initial_tool_state
+
+
+def test_omnipic_initial_state_opens_fully(monkeypatch) -> None:
+    calls = []
+
+    class FakeArm:
+        def rm_set_voltage(self, *args):
+            del args
+
+        def rm_set_io_mode(self, *args):
+            del args
+
+        def rm_algo_quaternion2euler(self, quaternion):
+            del quaternion
+            return [0.0, 0.0, 0.0]
+
+        def rm_get_total_tool_frame(self):
+            return {"return_code": 0, "tool_names": []}
+
+        def rm_set_manual_tool_frame(self, *, frame):
+            del frame
+            return 0
+
+        def rm_change_tool_frame(self, tool_name):
+            del tool_name
+            return 0
+
+        def rm_set_modbus_mode(self, **kwargs):
+            del kwargs
+            return 0
+
+        def rm_write_single_register(self, params, value):
+            del params, value
+            return 0
+
+    sdk = ModuleType("Robotic_Arm.rm_robot_interface")
+    sdk.rm_frame_t = lambda *args: object()
+    sdk.rm_peripheral_read_write_params_t = lambda *args: object()
+    package = ModuleType("Robotic_Arm")
+    package.rm_robot_interface = sdk
+    monkeypatch.setitem(sys.modules, "Robotic_Arm", package)
+    monkeypatch.setitem(sys.modules, "Robotic_Arm.rm_robot_interface", sdk)
+    monkeypatch.setattr(fun_peripheral.time, "sleep", lambda seconds: None)
+    monkeypatch.setattr(
+        fun_peripheral,
+        "set_tool_position",
+        lambda robot, percent, device, scissorgripper: calls.append(
+            (percent, device, scissorgripper)
+        ),
+    )
+    tools = {
+        "scissor": [[0.0] * 7, [0.0] * 7],
+        "omnipic": [[0.0] * 7, [0.0] * 7],
+    }
+
+    peripheral_cfg(
+        FakeArm(),
+        1,
+        tools,
+        set_initial_tool_state=True,
+    )
+
+    assert calls == [(1.0, 1, 1)]
+
+
 @pytest.mark.parametrize(
     ("config_name", "node_names"),
     [