config: 同步双臂 TCP 与前方工作空间

xr_rm_bringup/config/dual_arm_rm75.yaml

@@ -29,7 +29,7 @@ left_arm_teleop:
     orientation_filter_alpha: 0.65
     max_orientation_speed: 0.5
     workspace_min: [-0.70, -0.70, 0.10]
-    workspace_max: [0.70, 0.70, 0.75]
+    workspace_max: [0.70, 0.10, 0.75]
     cyl_radius_limit: [0.10, 0.80]
     low_z_threshold: 0.1
     low_z_min_radius: 0.1
@@ -86,7 +86,7 @@ right_arm_teleop:
     orientation_filter_alpha: 0.65
     max_orientation_speed: 0.5
     workspace_min: [-0.70, -0.70, 0.10]
-    workspace_max: [0.70, 0.70, 0.75]
+    workspace_max: [0.70, 0.10, 0.75]
     cyl_radius_limit: [0.10, 0.80]
     low_z_threshold: 0.1
     low_z_min_radius: 0.1

xr_rm_bringup/config/left_arm_rm75.yaml

@@ -23,7 +23,7 @@ single_arm_velocity_teleop:
     orientation_filter_alpha: 0.65
     max_orientation_speed: 0.5
     workspace_min: [-0.70, -0.70, 0.10]
-    workspace_max: [0.70, 0.70, 0.75]
+    workspace_max: [0.70, 0.10, 0.75]
     cyl_radius_limit: [0.10, 0.80]
     low_z_threshold: 0.1
     low_z_min_radius: 0.1

xr_rm_bringup/config/peripherals_rm75.yaml

@@ -13,10 +13,10 @@ tools_in_ee:
     # mass, center_x, center_y, center_z, reserved...
     load: [0.66, 0.0, 0.0, 0.06, 0.0, 0.0, 0.0]
   omnipic:
-    pose: [0.0, 0.0, 0.16, 0.0, 0.0, 0.0, 1.0]
+    pose: [0.0, 0.0, 0.14, 0.0, 0.0, 0.0, 1.0]
     load: [0.43, 0.0, 0.0, 0.06, 0.0, 0.0, 0.0]
   minisci:
-    pose: [0.0, 0.0, 0.19, 0.0, 0.0, 0.0, 1.0]
+    pose: [0.0, 0.0, 0.165, 0.0, 0.0, 0.0, 1.0]
     load: [0.46, 0.0, 0.0, 0.06, 0.0, 0.0, 0.0]
   no_tool:
     pose: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0]

xr_rm_bringup/config/right_arm_rm75.yaml

@@ -22,7 +22,7 @@ single_arm_velocity_teleop:
     orientation_filter_alpha: 0.65
     max_orientation_speed: 0.5
     workspace_min: [-0.70, -0.70, 0.10]
-    workspace_max: [0.70, 0.70, 0.75]
+    workspace_max: [0.70, 0.10, 0.75]
     cyl_radius_limit: [0.10, 0.80]
     low_z_threshold: 0.1
     low_z_min_radius: 0.1

xr_rm_teleop/test/test_initial_joint_pose.py

@@ -1,13 +1,22 @@
 import math
 import sys
+from pathlib import Path
 from types import ModuleType, SimpleNamespace
 
 import pytest
+import yaml
 
 from xr_rm_teleop import 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
+from xr_rm_teleop.fun_peripheral import (
+    PeripheralConfig,
+    _configure_tool_frame,
+    load_peripheral_config,
+)
+
+
+CONFIG_DIR = Path(__file__).resolve().parents[2] / "xr_rm_bringup" / "config"
 
 
 def test_initial_pose_uses_joint_move_only() -> None:
@@ -60,6 +69,37 @@ def test_peripheral_config_exposes_selected_tool() -> None:
     assert config.tool_pose == [0.0, 0.0, 0.16, 0.0, 0.0, 0.0, 1.0]
 
 
+def test_deployed_peripheral_config_selects_left_and_right_tools() -> 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 left.scissorgripper == 2
+    assert left.tool_name == "minisci"
+    assert left.tool_pose == [0.0, 0.0, 0.165, 0.0, 0.0, 0.0, 1.0]
+    assert right.scissorgripper == 1
+    assert right.tool_name == "omnipic"
+    assert right.tool_pose == [0.0, 0.0, 0.14, 0.0, 0.0, 0.0, 1.0]
+
+
+@pytest.mark.parametrize(
+    ("config_name", "node_names"),
+    [
+        ("left_arm_rm75.yaml", ("single_arm_velocity_teleop",)),
+        ("right_arm_rm75.yaml", ("single_arm_velocity_teleop",)),
+        ("dual_arm_rm75.yaml", ("left_arm_teleop", "right_arm_teleop")),
+    ],
+)
+def test_deployed_workspace_is_in_front_of_robot(config_name, node_names) -> None:
+    with (CONFIG_DIR / config_name).open(encoding="utf-8") as stream:
+        config = yaml.safe_load(stream)
+
+    for node_name in node_names:
+        parameters = config[node_name]["ros__parameters"]
+        assert parameters["workspace_min"] == [-0.70, -0.70, 0.10]
+        assert parameters["workspace_max"] == [0.70, 0.10, 0.75]
+
+
 @pytest.mark.parametrize(
     ("existing", "expected_operation"),
     [(False, "create"), (True, "update")],