feat: add peripherals control

xr_rm_teleop/xr_rm_teleop/realman_adapter.py

@@ -32,6 +32,7 @@ class MockRealManAdapter:
         self._pose = ArmPose(*initial_pose[:6])
         self._dt = dt
         self.last_velocity = [0.0] * 6
+        self.last_tool_open: bool | None = None
 
     def connect(self) -> None:
         return
@@ -56,6 +57,12 @@ class MockRealManAdapter:
     def close(self) -> None:
         self.stop()
 
+    def configure_peripheral(self, config_file: str, peripheral_arm: str) -> None:
+        del config_file, peripheral_arm
+
+    def set_tool_enabled(self, open_tool: bool) -> None:
+        self.last_tool_open = open_tool
+
 
 class RealManAdapter:
     """睿尔曼 Python API2 的笛卡尔速度透传适配层。"""
@@ -103,6 +110,7 @@ class RealManAdapter:
         self._command_mode = command_mode
         self._canfd_trajectory_mode = canfd_trajectory_mode
         self._canfd_radio = canfd_radio
+        self._scissorgripper: int | None = None
         self._arm: Any | None = None
         if self._command_mode not in ("velocity", "pose_canfd"):
             raise ValueError("command_mode must be one of: velocity, pose_canfd")
@@ -164,6 +172,35 @@ class RealManAdapter:
     def uses_pose_targets(self) -> bool:
         return self._command_mode == "pose_canfd"
 
+    def configure_peripheral(self, config_file: str, peripheral_arm: str) -> None:
+        self._require_arm()
+        from .fun_peripheral import load_peripheral_config, peripheral_cfg
+
+        config = load_peripheral_config(config_file, peripheral_arm)
+        self._scissorgripper = config.scissorgripper
+        tool_name = list(config.tools_in_ee.keys())[config.scissorgripper]
+        self._log_info(
+            "开始配置 RealMan 末端外设："
+            f"arm={peripheral_arm}, scissorgripper={config.scissorgripper}, "
+            f"tool={tool_name}, set_initial_tool_state={config.set_initial_tool_state}"
+        )
+        peripheral_cfg(
+            self._arm,
+            config.scissorgripper,
+            config.tools_in_ee,
+            set_initial_tool_state=config.set_initial_tool_state,
+        )
+        self._log_info(f"RealMan 末端外设配置完成：arm={peripheral_arm}, tool={tool_name}")
+
+    def set_tool_enabled(self, open_tool: bool) -> None:
+        self._require_arm()
+        if self._scissorgripper is None:
+            raise RuntimeError("末端外设尚未初始化，无法执行开合命令")
+
+        from .fun_peripheral import tool_exe
+
+        tool_exe(self._arm, self._scissorgripper, open_tool)
+
     def stop(self) -> None:
         if self._arm is None:
             return