docs: 更新双 RM75 逆解说明

README.md

@@ -65,7 +65,8 @@ src/
 └── xr_rm_teleop/
     ├── models/
     │   ├── rm75/                   # 旧 RM75 模型资源（launch 不再选用）
-    │   └── rm75_omnipicker/        # RM75 + OmniPicker fixed URDF 与网格
+    │   ├── rm75_omnipicker/        # 旧单臂 OmniPicker 模型资源
+    │   └── dual_rm75/              # 当前左右臂统一使用的双 RM75 URDF 与网格
     └── xr_rm_teleop/
         ├── placo_ik_solver.py      # Placo 0.9.4 单步 QP 逆解
         ├── single_arm_velocity_teleop.py
@@ -244,7 +245,9 @@ ros2 launch xr_rm_bringup arm_debug.launch.py arm:=both use_mock:=false
 
 `left_arm_rm75.yaml` 和 `right_arm_rm75.yaml` 用于 `arm_debug.launch.py arm:=left/right` 的单臂调试，因为单臂节点名是 `single_arm_velocity_teleop`。
 
-`xr_rm_bringup/config/peripherals_rm75.yaml` 保存真实控制器使用的末端工具坐标、负载和左右臂外设选择，文件内容保持原状。Placo 使用 `xr_rm_teleop/models/rm75_omnipicker` 中的一体化 fixed URDF，直接控制相对 `omnipicker_base_link` 沿 `+Z` 偏移 `0.16 m` 的 `omnipicker_tcp`，不再把外设 YAML 的工具位姿重复转换到 QP。真机连接阶段仍会初始化外设，关节反馈、关节指令、慢停和开合命令复用该单臂节点的同一个 RealMan 连接。
+`xr_rm_bringup/config/peripherals_rm75.yaml` 保存真实控制器使用的末端工具坐标、负载和左右臂外设选择。左臂 `scissorgripper: 2` 是外设选择值，选择 `minisci`，TCP 的 Z 向偏移为 `0.165 m`；右臂 `scissorgripper: 1`，选择 `omnipic`，TCP 的 Z 向偏移为 `0.14 m`。真机连接阶段仍会初始化外设，关节反馈、关节指令、慢停和开合命令复用该单臂节点的同一个 RealMan 连接。
+
+Placo 使用 `xr_rm_teleop/models/dual_rm75/Dual_arm.urdf`。左右 ROS 节点分别创建独立 solver：左臂从 `scissor_base_link` 到 `scissor_scissor_tcp`，并 mask 右臂；右臂从 `omnipic_base_link` 到 `omnipic_OmniPic_tcp`，并 mask 左臂。节点目标仍在各自局部基坐标系中，现有 PICO 映射不改为公共坐标系。
 
 重点控制参数：
 
@@ -274,6 +277,8 @@ ros2 launch xr_rm_bringup arm_debug.launch.py arm:=both use_mock:=false
 - receiver 默认按 `xyzw` 解析四元数，也可通过 `quat_order:=wxyz` 切换。
 - 左臂映射：机器人位移增量 = `[-手柄y, 手柄z, -手柄x]`。
 - 右臂映射：机器人位移增量 = `[手柄y, 手柄z, 手柄x]`。
+- 两侧局部 `-Y` 均指向机器人前方；局部 `+Y` 指向后方，后方工作空间仅保留 `0.10 m`。
+- 左臂局部 `+X/+Y/+Z` 分别指向下/后/左外侧；右臂分别指向上/后/右外侧。
 
 如果某个机械臂方向相反，只改对应臂的 `xr_to_robot_matrix` 符号，不要同时改多个控制参数。
 
@@ -440,7 +445,7 @@ ros2 topic echo /xr/right_controller --field trigger
 8. 确认松开 `grip` 后机械臂慢停，`/xr_rm/<arm>/cmd_vel` 回到零；trigger 仍只影响夹爪，不影响机械臂运动门控。
 9. 左右臂都确认后，再进入双臂模式。
 
-当前项目没有双臂碰撞检测。双臂首次联调时，请让两个工作区在物理上分开，低速验证，不要让两臂末端互相靠近。
+当前项目没有双臂碰撞检测/避障。双臂首次联调时，请让两个工作区在物理上分开，低速验证，不要让两臂末端互相靠近。
 
 ## 后续优化路线
 

docs/superpowers/plans/2026-08-03-dual-rm75-ik-model.md

@@ -0,0 +1,870 @@
+# 双 RM75 逆解模型替换实施计划
+
+> **面向执行代理：** 必须逐项执行本计划，并使用 `superpowers:test-driven-development`；可选择 `superpowers:subagent-driven-development`（推荐）或 `superpowers:executing-plans`。
+
+**目标：** 让单臂和双臂遥操作统一加载 `dual_rm75`，左右节点分别使用本侧局部 base→TCP 相对任务求解 7 个关节，并同步前方工作空间与真机 TCP 配置。
+
+**架构：** 保留 `left_arm_teleop`、`right_arm_teleop` 两个独立节点和 RealMan 连接。每个节点创建独立 `PlacoIkSolver`，加载同一双臂 URDF，固定浮动基座、mask 另一臂关节，并通过当前侧关节名查询 q/v offset。节点继续在各自局部基坐标系生成目标，现有 PICO 映射与安全链路不变。
+
+**技术栈：** Ubuntu 22.04、ROS2 Humble、Python 3.10、ament_python、Placo 0.9.4、NumPy、pytest、colcon。
+
+---
+
+## 执行约束
+
+- 所有构建、测试和启动命令均在 `/home/robot/WS_xr` 执行，并先运行：
+
+  ```bash
+  source /opt/ros/humble/setup.bash
+  ```
+
+- 真实 Placo 测试使用 `/home/robot/miniconda3/envs/xr/bin/python`，不能把跳过测试当作通过。
+- 启动验收只允许 `use_mock:=true`，不得连接真机、移动机械臂或操作夹爪。
+- 不修改 `configure_safety_limits: true`、`move_to_initial_pose_on_connect: false`、左右节点名或现有限速/超时/安全停止逻辑。
+- 不增加碰撞约束、新依赖、第三个控制节点或公共坐标系控制路径。
+- 每个实现任务只提交列出的文件，不提交无关工作树内容。
+- `setup.py` 和 launch 路径属于配置集成；按已确认的测试设计使用完整构建、安装
+  资源检查和 mock 启动验收，不增加读取源码字符串的脆弱测试。
+
+## 文件结构
+
+**修改：**
+
+- `xr_rm_teleop/xr_rm_teleop/placo_ik_solver.py`：选择左右运动链、查询 offset、建立相对位姿任务。
+- `xr_rm_teleop/xr_rm_teleop/single_arm_velocity_teleop.py`：把当前侧名称传给求解器。
+- `xr_rm_teleop/test/test_placo_transforms.py`：双臂 URDF、左右 offset、局部位姿和真实 Placo 收敛回归。
+- `xr_rm_teleop/test/placo_ik_smoke.py`：左右分支手工性能冒烟脚本。
+- `xr_rm_teleop/test/test_initial_joint_pose.py`：真机外设选择与三份工作空间配置回归。
+- `xr_rm_teleop/setup.py`：安装双臂 URDF 和混合大小写 STL。
+- `xr_rm_bringup/launch/arm_debug.launch.py`：单臂/双臂统一选择双臂 URDF。
+- `xr_rm_bringup/config/dual_arm_rm75.yaml`：左右局部 Y 上界改为 `0.10`。
+- `xr_rm_bringup/config/left_arm_rm75.yaml`：左臂局部 Y 上界改为 `0.10`。
+- `xr_rm_bringup/config/right_arm_rm75.yaml`：右臂局部 Y 上界改为 `0.10`。
+- `xr_rm_bringup/config/peripherals_rm75.yaml`：同步右臂 omnipic 和左臂编号 2 实际工具的 TCP。
+- `README.md`：更新模型、局部坐标与配置说明。
+
+**不创建新的生产模块或依赖。**
+
+### 任务一：用回归测试锁定外设 TCP 与前方工作空间
+
+**文件：**
+
+- 修改：`xr_rm_teleop/test/test_initial_joint_pose.py`
+- 修改：`xr_rm_bringup/config/peripherals_rm75.yaml`
+- 修改：`xr_rm_bringup/config/dual_arm_rm75.yaml`
+- 修改：`xr_rm_bringup/config/left_arm_rm75.yaml`
+- 修改：`xr_rm_bringup/config/right_arm_rm75.yaml`
+
+- [ ] **步骤 1：先写失败的真实配置测试**
+
+在 `test_initial_joint_pose.py` 顶部补充导入：
+
+```python
+from pathlib import Path
+
+import yaml
+
+from xr_rm_teleop.fun_peripheral import (
+    PeripheralConfig,
+    _configure_tool_frame,
+    load_peripheral_config,
+)
+```
+
+删除原来单行的 `PeripheralConfig, _configure_tool_frame` 导入，随后在
+`test_peripheral_config_exposes_selected_tool()` 后加入：
+
+```python
+CONFIG_DIR = Path(__file__).resolve().parents[2] / "xr_rm_bringup" / "config"
+
+
+def test_deployed_peripheral_config_matches_dual_urdf_tcps() -> None:
+    path = CONFIG_DIR / "peripherals_rm75.yaml"
+    left = load_peripheral_config(str(path), "left")
+    right = load_peripheral_config(str(path), "right")
+
+    assert left.scissorgripper == 2
+    assert left.tool_name == "minisci"
+    assert left.tool_pose == pytest.approx(
+        [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 == pytest.approx(
+        [0.0, 0.0, 0.14, 0.0, 0.0, 0.0, 1.0]
+    )
+
+
+@pytest.mark.parametrize(
+    ("filename", "node_name"),
+    [
+        ("left_arm_rm75.yaml", "single_arm_velocity_teleop"),
+        ("right_arm_rm75.yaml", "single_arm_velocity_teleop"),
+        ("dual_arm_rm75.yaml", "left_arm_teleop"),
+        ("dual_arm_rm75.yaml", "right_arm_teleop"),
+    ],
+)
+def test_deployed_workspaces_keep_only_ten_centimeters_behind(
+    filename: str,
+    node_name: str,
+) -> None:
+    with (CONFIG_DIR / filename).open("r", encoding="utf-8") as stream:
+        parameters = yaml.safe_load(stream)[node_name]["ros__parameters"]
+
+    assert parameters["workspace_min"] == [-0.70, -0.70, 0.10]
+    assert parameters["workspace_max"] == [0.70, 0.10, 0.75]
+```
+
+- [ ] **步骤 2：运行测试并确认按预期失败**
+
+运行：
+
+```bash
+cd /home/robot/WS_xr
+source /opt/ros/humble/setup.bash
+pytest \
+  src/xr_rm_teleop/test/test_initial_joint_pose.py::test_deployed_peripheral_config_matches_dual_urdf_tcps \
+  src/xr_rm_teleop/test/test_initial_joint_pose.py::test_deployed_workspaces_keep_only_ten_centimeters_behind \
+  -v
+```
+
+预期：FAIL；当前左臂 `minisci.pose.z` 为 `0.19`、右臂 `omnipic.pose.z` 为
+`0.16`，三份配置的 `workspace_max[1]` 为 `0.70`。
+
+- [ ] **步骤 3：做最小配置修改**
+
+在 `peripherals_rm75.yaml` 中只修改：
+
+```yaml
+  omnipic:
+    pose: [0.0, 0.0, 0.14, 0.0, 0.0, 0.0, 1.0]
+  minisci:
+    pose: [0.0, 0.0, 0.165, 0.0, 0.0, 0.0, 1.0]
+```
+
+保持以下内容不变：
+
+```yaml
+  scissor:
+    pose: [0.0, 0.0, 0.19, 0.0, 0.0, 0.0, 1.0]
+arms:
+  left:
+    scissorgripper: 2
+  right:
+    scissorgripper: 1
+```
+
+在 `left_arm_rm75.yaml`、`right_arm_rm75.yaml` 以及 `dual_arm_rm75.yaml` 的左右
+节点参数中只把：
+
+```yaml
+workspace_max: [0.70, 0.70, 0.75]
+```
+
+改为：
+
+```yaml
+workspace_max: [0.70, 0.10, 0.75]
+```
+
+- [ ] **步骤 4：运行配置测试并确认通过**
+
+运行：
+
+```bash
+cd /home/robot/WS_xr
+source /opt/ros/humble/setup.bash
+pytest src/xr_rm_teleop/test/test_initial_joint_pose.py -v
+```
+
+预期：该文件全部通过，左臂索引仍为 `2`。
+
+- [ ] **步骤 5：提交配置与测试**
+
+```bash
+git add \
+  src/xr_rm_teleop/test/test_initial_joint_pose.py \
+  src/xr_rm_bringup/config/peripherals_rm75.yaml \
+  src/xr_rm_bringup/config/dual_arm_rm75.yaml \
+  src/xr_rm_bringup/config/left_arm_rm75.yaml \
+  src/xr_rm_bringup/config/right_arm_rm75.yaml
+git commit -m "config: 同步双臂 TCP 与前方工作空间"
+```
+
+### 任务二：为双臂局部相对逆解建立失败测试
+
+**文件：**
+
+- 修改：`xr_rm_teleop/test/test_placo_transforms.py`
+
+- [ ] **步骤 1：把旧单臂 URDF 结构测试替换为双臂结构测试**
+
+在测试文件导入中加入 `QP_ORIENTATION_TOLERANCE_RAD`，并定义模型路径：
+
+```python
+from xr_rm_teleop.placo_ik_solver import (
+    QP_ORIENTATION_TOLERANCE_RAD,
+    QP_POSITION_TOLERANCE_M,
+    PlacoIkSolver,
+    _validated_transform,
+)
+
+
+DUAL_URDF_PATH = (
+    Path(__file__).resolve().parents[1]
+    / "models"
+    / "dual_rm75"
+    / "Dual_arm.urdf"
+)
+```
+
+用下面测试替换 `test_fixed_urdf_has_seven_moving_joints_and_omnipicker_tcp()`：
+
+```python
+def test_dual_urdf_has_two_rm75_chains_and_tool_tcps() -> None:
+    root = ElementTree.parse(DUAL_URDF_PATH).getroot()
+    moving_joint_names = [
+        joint.attrib["name"]
+        for joint in root.findall("joint")
+        if joint.attrib["type"] != "fixed"
+    ]
+
+    assert moving_joint_names == [
+        *[f"omnipic_joint_{index}" for index in range(1, 8)],
+        *[f"scissor_joint_{index}" for index in range(1, 8)],
+    ]
+    assert all(
+        mesh.attrib["filename"].startswith("meshes/")
+        for mesh in root.findall(".//mesh")
+    )
+
+    expected_fixed_joints = {
+        "omnipic_base_mount_joint": (
+            "dual_arm_base_link",
+            "omnipic_base_link",
+            None,
+        ),
+        "scissor_base_mount_joint": (
+            "dual_arm_base_link",
+            "scissor_base_link",
+            None,
+        ),
+        "omnipic_OmniPic_tcp_fixed": (
+            "omnipic_gripper_link",
+            "omnipic_OmniPic_tcp",
+            "0 0 0.14",
+        ),
+        "scissor_scissor_tcp_fixed": (
+            "scissor_scissor_link",
+            "scissor_scissor_tcp",
+            "0 0 0",
+        ),
+        "scissor_scissor_fixed_joint": (
+            "scissor_link_7",
+            "scissor_scissor_link",
+            "0 0 0.165",
+        ),
+    }
+    for name, (parent, child, xyz) in expected_fixed_joints.items():
+        joint = root.find(f"joint[@name='{name}']")
+        assert joint is not None
+        assert joint.attrib["type"] == "fixed"
+        assert joint.find("parent").attrib["link"] == parent
+        assert joint.find("child").attrib["link"] == child
+        if xyz is not None:
+            assert joint.find("origin").attrib["xyz"] == xyz
+```
+
+- [ ] **步骤 2：增加左右求解器、offset 与相对位姿测试**
+
+用下面代码替换 `_rm75_placo_solver()` 和旧的单臂收敛测试：
+
+```python
+ARM_CASES = [
+    pytest.param(
+        "left",
+        [-78.81, 3.22, 67.96, 97.12, 95.08, -81.11, -74.55],
+        list(range(14, 21)),
+        list(range(13, 20)),
+        "omnipic",
+        id="left",
+    ),
+    pytest.param(
+        "right",
+        [-86.10, 22.80, -89.57, 93.98, -91.82, -87.32, -89.35],
+        list(range(7, 14)),
+        list(range(6, 13)),
+        "scissor",
+        id="right",
+    ),
+]
+
+
+def _dual_placo_solver(
+    arm: str,
+    joint_degrees: list[float],
+) -> tuple[PlacoIkSolver, list[float]]:
+    pytest.importorskip("placo")
+    joints = [math.radians(value) for value in joint_degrees]
+    return PlacoIkSolver(str(DUAL_URDF_PATH), 1.0 / 90.0, arm), joints
+
+
+@pytest.mark.parametrize(
+    ("arm", "joint_degrees", "q_offsets", "v_offsets", "inactive_prefix"),
+    ARM_CASES,
+)
+def test_solver_uses_arm_specific_offsets(
+    arm: str,
+    joint_degrees: list[float],
+    q_offsets: list[int],
+    v_offsets: list[int],
+    inactive_prefix: str,
+) -> None:
+    del inactive_prefix
+    solver, _ = _dual_placo_solver(arm, joint_degrees)
+
+    assert solver._q_offsets.tolist() == q_offsets
+    assert solver._v_offsets.tolist() == v_offsets
+
+
+@pytest.mark.parametrize(
+    ("arm", "joint_degrees", "q_offsets", "v_offsets", "inactive_prefix"),
+    ARM_CASES,
+)
+def test_joint_state_pose_is_relative_to_selected_arm_base(
+    arm: str,
+    joint_degrees: list[float],
+    q_offsets: list[int],
+    v_offsets: list[int],
+    inactive_prefix: str,
+) -> None:
+    del q_offsets, v_offsets, inactive_prefix
+    solver, joints = _dual_placo_solver(arm, joint_degrees)
+
+    actual = solver.update_joint_state(joints)
+    expected = (
+        np.linalg.inv(solver._robot.get_T_world_frame(solver._base_frame))
+        @ solver._robot.get_T_world_frame(solver._tcp_frame)
+    )
+
+    assert actual == pytest.approx(expected)
+
+
+@pytest.mark.parametrize(
+    ("arm", "joint_degrees", "q_offsets", "v_offsets", "inactive_prefix"),
+    ARM_CASES,
+)
+def test_qp_solve_converges_without_moving_inactive_arm(
+    arm: str,
+    joint_degrees: list[float],
+    q_offsets: list[int],
+    v_offsets: list[int],
+    inactive_prefix: str,
+) -> None:
+    del q_offsets, v_offsets
+    solver, joints = _dual_placo_solver(arm, joint_degrees)
+    inactive_offsets = [
+        solver._robot.get_joint_offset(f"{inactive_prefix}_joint_{index}")
+        for index in range(1, 8)
+    ]
+    inactive_before = solver._robot.state.q[inactive_offsets].copy()
+    start_pose = solver.update_joint_state(joints)
+    target_pose = start_pose.copy()
+    target_pose[0, 3] += 0.01
+
+    result = solver.solve(target_pose)
+    reached_pose = solver.update_joint_state(result)
+    rotation_delta = target_pose[:3, :3] @ reached_pose[:3, :3].T
+    orientation_error = math.acos(
+        float(
+            np.clip(
+                (np.trace(rotation_delta) - 1.0) * 0.5,
+                -1.0,
+                1.0,
+            )
+        )
+    )
+
+    assert len(result) == 7
+    assert np.isfinite(result).all()
+    assert np.linalg.norm(
+        target_pose[:3, 3] - reached_pose[:3, 3]
+    ) <= QP_POSITION_TOLERANCE_M
+    assert orientation_error <= QP_ORIENTATION_TOLERANCE_RAD
+    assert solver._robot.state.q[inactive_offsets] == pytest.approx(
+        inactive_before
+    )
+
+
+def test_solver_rejects_unknown_arm() -> None:
+    pytest.importorskip("placo")
+
+    with pytest.raises(ValueError, match="arm must be left or right"):
+        PlacoIkSolver(str(DUAL_URDF_PATH), 1.0 / 90.0, "middle")
+```
+
+- [ ] **步骤 3：运行新测试并确认按预期失败**
+
+运行：
+
+```bash
+cd /home/robot/WS_xr
+source /opt/ros/humble/setup.bash
+/home/robot/miniconda3/envs/xr/bin/python -m pytest \
+  src/xr_rm_teleop/test/test_placo_transforms.py -v
+```
+
+预期：FAIL；当前 `PlacoIkSolver` 不接受 `arm` 参数，仍要求单臂 q shape 和
+`joint_1～7`。
+
+### 任务三：实现最小双臂分支相对求解器
+
+**文件：**
+
+- 修改：`xr_rm_teleop/xr_rm_teleop/placo_ik_solver.py`
+- 修改：`xr_rm_teleop/test/test_placo_transforms.py`
+- 修改：`xr_rm_teleop/test/placo_ik_smoke.py`
+
+- [ ] **步骤 1：替换单臂固定常量**
+
+把 `RM75_JOINT_NAMES` 和 `RM75_Q_SLICE` 替换为：
+
+```python
+ARM_CHAINS = {
+    "left": (
+        "scissor_base_link",
+        "scissor_scissor_tcp",
+        "scissor",
+        "omnipic",
+    ),
+    "right": (
+        "omnipic_base_link",
+        "omnipic_OmniPic_tcp",
+        "omnipic",
+        "scissor",
+    ),
+}
+DUAL_RM75_JOINT_NAMES = [
+    *[f"omnipic_joint_{index}" for index in range(1, 8)],
+    *[f"scissor_joint_{index}" for index in range(1, 8)],
+]
+```
+
+- [ ] **步骤 2：按名称选择当前分支并建立相对任务**
+
+将 `PlacoIkSolver.__init__()` 签名改为：
+
+```python
+def __init__(
+    self,
+    urdf_path: str,
+    dt: float,
+    arm: str,
+) -> None:
+```
+
+在 `dt` 校验后先选择固定分支：
+
+```python
+if arm not in ARM_CHAINS:
+    raise ValueError("arm must be left or right")
+self._base_frame, self._tcp_frame, prefix, inactive_prefix = ARM_CHAINS[arm]
+self._joint_names = [f"{prefix}_joint_{index}" for index in range(1, 8)]
+inactive_joint_names = [
+    f"{inactive_prefix}_joint_{index}" for index in range(1, 8)
+]
+```
+
+加载 `RobotWrapper` 后，用下面代码替换单臂 q shape、关节顺序、offset 和限位初始化：
+
+```python
+if self._robot.state.q.shape != (21,):
+    raise RuntimeError(
+        f"expected Placo q shape (21,), got {self._robot.state.q.shape}"
+    )
+if list(self._robot.joint_names()) != DUAL_RM75_JOINT_NAMES:
+    raise RuntimeError(
+        "unexpected dual RM75 joint order: "
+        f"{list(self._robot.joint_names())}"
+    )
+
+self._q_offsets = np.asarray(
+    [self._robot.get_joint_offset(name) for name in self._joint_names],
+    dtype=int,
+)
+self._v_offsets = np.asarray(
+    [self._robot.get_joint_v_offset(name) for name in self._joint_names],
+    dtype=int,
+)
+if len(set(self._q_offsets.tolist())) != 7:
+    raise RuntimeError(f"invalid RM75 q offsets: {self._q_offsets.tolist()}")
+if len(set(self._v_offsets.tolist())) != 7:
+    raise RuntimeError(f"invalid RM75 v offsets: {self._v_offsets.tolist()}")
+
+self._joint_limits = np.asarray(
+    [self._robot.get_joint_limits(name) for name in self._joint_names]
+)
+self._velocity_limits = np.asarray(
+    [self._robot.model.velocityLimit[index] for index in self._v_offsets]
+)
+self._actual_joints: np.ndarray | None = None
+```
+
+用下面代码替换任务创建：
+
+```python
+self._solver = placo.KinematicsSolver(self._robot)
+self._solver.dt = dt
+self._solver.mask_fbase(True)
+for name in inactive_joint_names:
+    self._solver.mask_dof(name)
+self._solver.enable_velocity_limits(True)
+self._frame_task = self._solver.add_relative_frame_task(
+    self._base_frame,
+    self._tcp_frame,
+    np.eye(4),
+)
+self._frame_task.configure("rm75_relative_frame", "soft", 1.0)
+self._solver.add_kinetic_energy_regularization_task(1e-6)
+```
+
+- [ ] **步骤 3：让反馈和结果使用当前侧 offset 与局部位姿**
+
+在 `update_joint_state()` 中用下面逻辑替换固定切片和绝对 TCP 查询：
+
+```python
+self._robot.state.q[self._q_offsets] = values
+self._robot.update_kinematics()
+base_to_tool = (
+    np.linalg.inv(self._robot.get_T_world_frame(self._base_frame))
+    @ self._robot.get_T_world_frame(self._tcp_frame)
+)
+if is_first_feedback:
+    self._frame_task.T_a_b = base_to_tool.copy()
+return base_to_tool.copy()
+```
+
+在 `solve()` 中把任务目标与两处结果读取分别改为：
+
+```python
+self._frame_task.T_a_b = _validated_transform(target_tool_pose)
+result = np.asarray(
+    self._robot.state.q[self._q_offsets],
+    dtype=float,
+).copy()
+```
+
+迭代后的结果读取使用同一段 `self._q_offsets` 代码。`base_configuration`、目标误差、
+结果校验和收敛循环保持不变。
+
+- [ ] **步骤 4：更新无真实 Placo 的小型求解测试桩**
+
+在 `test_qp_solve_accepts_position_error_within_two_millimeters()` 和
+`test_qp_solve_rejects_position_error_above_two_millimeters()` 中设置：
+
+```python
+solver._q_offsets = np.arange(7, 14)
+solver._robot = SimpleNamespace(
+    state=SimpleNamespace(q=np.zeros(21)),
+)
+solver._frame_task = SimpleNamespace(T_a_b=None)
+```
+
+第二个测试继续给 `_robot` 增加原有 `update_kinematics=lambda: None`，其他桩保持
+原样。这样测试仍只覆盖 2 mm 收敛边界，不伪造 Placo 相对任务。
+
+- [ ] **步骤 5：运行真实 Placo 测试并确认转绿**
+
+运行：
+
+```bash
+cd /home/robot/WS_xr
+source /opt/ros/humble/setup.bash
+/home/robot/miniconda3/envs/xr/bin/python -m pytest \
+  src/xr_rm_teleop/test/test_placo_transforms.py -v
+```
+
+预期：全部通过；左右真实 Placo 用例均执行，不能显示 skipped。
+
+- [ ] **步骤 6：更新手工 Placo 冒烟脚本**
+
+把 `placo_ik_smoke.py` 的 `CASES` 更新为当前左右初始角：
+
+```python
+CASES = {
+    "left": [-78.81, 3.22, 67.96, 97.12, 95.08, -81.11, -74.55],
+    "right": [-86.10, 22.80, -89.57, 93.98, -91.82, -87.32, -89.35],
+}
+
+TOOL_CHAINS = {
+    "left": ("scissor_base_link", "scissor_link_7", 0.165),
+    "right": ("omnipic_base_link", "omnipic_link_7", 0.14),
+}
+```
+
+两处求解器构造都改为：
+
+```python
+PlacoIkSolver(str(urdf_path), 1.0 / 125.0, arm)
+```
+
+把固定 `link_7`/`0.16` 检查替换为：
+
+```python
+base_frame, flange_frame, tcp_length = TOOL_CHAINS[arm]
+world_to_base = drift_solver._robot.get_T_world_frame(base_frame)
+world_to_flange = drift_solver._robot.get_T_world_frame(flange_frame)
+base_to_flange = np.linalg.inv(world_to_base) @ world_to_flange
+flange_to_tcp = np.linalg.inv(base_to_flange) @ stationary_target
+assert np.allclose(flange_to_tcp[:3, 3], [0.0, 0.0, tcp_length])
+assert np.allclose(flange_to_tcp[:3, :3], np.eye(3), atol=1e-5)
+```
+
+- [ ] **步骤 7：运行冒烟脚本**
+
+运行：
+
+```bash
+cd /home/robot/WS_xr
+source /opt/ros/humble/setup.bash
+PYTHONPATH=src/xr_rm_teleop \
+  /home/robot/miniconda3/envs/xr/bin/python \
+  src/xr_rm_teleop/test/placo_ik_smoke.py \
+  src/xr_rm_teleop/models/dual_rm75/Dual_arm.urdf
+```
+
+预期：左右各输出一行有限误差与耗时统计；位置误差不超过 `0.005 m`、姿态误差
+不超过 `2°`、静止漂移不超过 `0.05°`。
+
+- [ ] **步骤 8：提交求解器与测试**
+
+```bash
+git add \
+  src/xr_rm_teleop/xr_rm_teleop/placo_ik_solver.py \
+  src/xr_rm_teleop/test/test_placo_transforms.py \
+  src/xr_rm_teleop/test/placo_ik_smoke.py
+git commit -m "feat: 使用双 RM75 局部相对逆解"
+```
+
+### 任务四：接入节点、安装空间与统一 launch
+
+**文件：**
+
+- 修改：`xr_rm_teleop/xr_rm_teleop/single_arm_velocity_teleop.py`
+- 修改：`xr_rm_teleop/setup.py`
+- 修改：`xr_rm_bringup/launch/arm_debug.launch.py`
+
+- [ ] **步骤 1：把节点当前侧传给求解器**
+
+将节点中的求解器构造改为：
+
+```python
+self._ik_solver = PlacoIkSolver(
+    str(self.get_parameter("robot_urdf_path").value),
+    self._dt,
+    peripheral_arm,
+)
+```
+
+复用已经用于外设加载的 `peripheral_arm`，不增加新的 ROS 参数。
+
+- [ ] **步骤 2：安装双臂模型资源**
+
+在 `xr_rm_teleop/setup.py` 的 `data_files` 中增加：
+
+```python
+(
+    f"share/{package_name}/models/dual_rm75",
+    ["models/dual_rm75/Dual_arm.urdf"],
+),
+(
+    f"share/{package_name}/models/dual_rm75/meshes",
+    glob("models/dual_rm75/meshes/*.STL")
+    + glob("models/dual_rm75/meshes/*.stl"),
+),
+```
+
+保留旧模型安装项，避免破坏仓库中其他手工路径；不修改锁文件或依赖。
+
+- [ ] **步骤 3：让所有 launch 模式选择双臂 URDF**
+
+将 `_rm75_urdf()` 改名并替换为：
+
+```python
+def _dual_rm75_urdf() -> PathJoinSubstitution:
+    return PathJoinSubstitution([
+        FindPackageShare("xr_rm_teleop"),
+        "models",
+        "dual_rm75",
+        "Dual_arm.urdf",
+    ])
+```
+
+把单臂节点和两个双臂节点中的：
+
+```python
+"robot_urdf_path": _rm75_urdf(),
+```
+
+全部替换为：
+
+```python
+"robot_urdf_path": _dual_rm75_urdf(),
+```
+
+- [ ] **步骤 4：构建完整工作空间**
+
+运行：
+
+```bash
+cd /home/robot/WS_xr
+source /opt/ros/humble/setup.bash
+colcon build --symlink-install
+```
+
+预期：退出码 `0`，四个 ROS2 包构建成功。
+
+- [ ] **步骤 5：验证安装空间包含完整模型**
+
+运行：
+
+```bash
+cd /home/robot/WS_xr
+test -f install/xr_rm_teleop/share/xr_rm_teleop/models/dual_rm75/Dual_arm.urdf
+find install/xr_rm_teleop/share/xr_rm_teleop/models/dual_rm75/meshes \
+  -maxdepth 1 -type f | sort
+```
+
+预期：`test` 退出码 `0`；列表包含 `base_link.STL`、`OmniPic.stl`、
+`scissor.stl`、`dual_arm_base.stl` 和 7 个 link 网格等现有资源。
+
+- [ ] **步骤 6：运行双臂 mock 启动验收**
+
+运行：
+
+```bash
+cd /home/robot/WS_xr
+source /opt/ros/humble/setup.bash
+source install/setup.bash
+timeout 15s ros2 launch xr_rm_bringup arm_debug.launch.py \
+  arm:=both use_mock:=true
+```
+
+预期：日志显示 `left_rm75`、`right_rm75` 两个 Placo QP 节点启动，无模型路径、
+q shape、关节名、frame 或 traceback 错误。`timeout` 到期的退出码 `124` 属于预期；
+不得改用 `use_mock:=false`。
+
+- [ ] **步骤 7：提交接入修改**
+
+```bash
+git add \
+  src/xr_rm_teleop/xr_rm_teleop/single_arm_velocity_teleop.py \
+  src/xr_rm_teleop/setup.py \
+  src/xr_rm_bringup/launch/arm_debug.launch.py
+git commit -m "feat: 接入双 RM75 逆解模型"
+```
+
+### 任务五：更新文档并完成全量验证
+
+**文件：**
+
+- 修改：`README.md`
+
+- [ ] **步骤 1：更新项目结构和模型说明**
+
+在 README 的模型树中保留旧模型并增加：
+
+```text
+│   ├── rm75/                   # 旧 RM75 模型资源（launch 不再选用）
+│   ├── rm75_omnipicker/        # 旧单臂 OmniPicker 模型资源
+│   └── dual_rm75/              # 当前左右臂统一使用的双 RM75 URDF 与网格
+```
+
+把“Placo 使用 `rm75_omnipicker` 和统一 `omnipicker_tcp`”段落替换为：
+
+```markdown
+Placo 使用 `xr_rm_teleop/models/dual_rm75/Dual_arm.urdf`。左右控制节点分别创建
+独立求解器：左臂控制 `scissor_base_link` 到 `scissor_scissor_tcp`，右臂控制
+`omnipic_base_link` 到 `omnipic_OmniPic_tcp`，并 mask 另一侧关节。节点目标仍在
+各自局部基坐标系表达，不把现有 PICO 映射改为公共坐标系。
+
+两侧局部 `-Y` 都指向机器人前方，工作空间在局部 `+Y` 后方只保留 `0.10 m`。
+左臂局部 `+X/+Y/+Z` 分别向下/向后/向左外侧；右臂分别向上/向后/向右外侧。
+真机工具坐标使用 URDF TCP：左臂硬件编号保持 `2`，实际选择的 `minisci` 工具
+长度为 `0.165 m`；右臂编号保持 `1`，`omnipic` 工具长度为 `0.14 m`。
+```
+
+不要把“当前没有双臂碰撞检测”的安全提示改成已完成。
+
+- [ ] **步骤 2：运行相关 Python 测试**
+
+运行：
+
+```bash
+cd /home/robot/WS_xr
+source /opt/ros/humble/setup.bash
+pytest src/xr_rm_teleop/test/test_initial_joint_pose.py -v
+pytest src/xr_rm_teleop/test/test_orientation_control.py -v
+/home/robot/miniconda3/envs/xr/bin/python -m pytest \
+  src/xr_rm_teleop/test/test_placo_transforms.py -v
+```
+
+预期：三个测试文件全部通过；真实 Placo 左右用例均执行。
+
+- [ ] **步骤 3：重新构建工作空间**
+
+运行：
+
+```bash
+cd /home/robot/WS_xr
+source /opt/ros/humble/setup.bash
+colcon build --symlink-install
+```
+
+预期：退出码 `0`。
+
+- [ ] **步骤 4：重新运行最终 mock 验收**
+
+运行：
+
+```bash
+cd /home/robot/WS_xr
+source /opt/ros/humble/setup.bash
+source install/setup.bash
+timeout 15s ros2 launch xr_rm_bringup arm_debug.launch.py \
+  arm:=both use_mock:=true
+```
+
+预期：两个节点均启动且没有 traceback；退出码 `124` 仅由 `timeout` 产生。
+
+- [ ] **步骤 5：检查最终范围和格式**
+
+运行：
+
+```bash
+cd /home/robot/WS_xr/src
+git diff --check
+git status --short
+git diff --stat
+```
+
+预期：无空白错误；变更仅包含本计划列出的求解器、测试、launch、安装、四份配置、
+README 和 Superpowers 文档。
+
+- [ ] **步骤 6：提交 README**
+
+```bash
+git add README.md
+git commit -m "docs: 更新双 RM75 逆解说明"
+```
+
+## 完成标准
+
+- 单臂和双臂 launch 均只选择安装空间中的 `dual_rm75/Dual_arm.urdf`。
+- 左右节点是独立求解器实例，各自使用正确 base、TCP、q/v offset 和相对位姿任务。
+- 当前侧小幅可达目标收敛，另一侧关节不漂移。
+- 左臂硬件编号保持 `2`，实际工具 TCP 为 `0.165 m`；右臂编号保持 `1`，TCP 为
+  `0.14 m`。
+- 三份控制配置的局部 Y 范围为 `[-0.70, 0.10]`，其他安全参数不变。
+- 相关测试、完整构建和 `arm:=both use_mock:=true` 启动验收取得新鲜证据。
+- 未连接真机，未增加碰撞控制、依赖或无关重构。

docs/superpowers/specs/2026-08-03-dual-rm75-ik-model-design.md

@@ -0,0 +1,180 @@
+# 双 RM75 逆解模型替换设计
+
+## 背景与目标
+
+当前左右遥操作节点都加载单臂 `rm75_omnipicker` URDF，求解器将 7 个关节名、
+`q[7:14]` 和 `omnipicker_tcp` 写死。项目新增的
+`xr_rm_teleop/models/dual_rm75/Dual_arm.urdf` 包含真实双臂布局：物理左臂为
+scissor 分支，物理右臂为 omnipic 分支，主要活动区域位于机器人前方。
+
+本次变更目标是：
+
+- 单臂和双臂调试都加载同一份 `dual_rm75` 模型；
+- 左右节点继续独立控制各自的 RM75，只求解当前侧 7 个关节；
+- 保留左右臂各自的局部控制坐标系和现有 PICO 映射；
+- 使用 URDF 中的 TCP 长度，并同步真机外设工具坐标；
+- 把局部后方工作空间余量限制为 `0.10 m`；
+- 保留现有速度、工作空间、圆柱、超时和安全停止逻辑。
+
+本次不增加双臂碰撞规避、公共坐标系目标、双臂协同任务，不合并左右控制节点，
+也不连接或移动真机。
+
+## 方案选择
+
+采用“完整双臂 URDF + 两个独立局部相对位姿任务”。
+
+未采用以下方案：
+
+1. 公共坐标系绝对位姿任务：需要重写 PICO 映射和现有安全限位，改动范围过大。
+2. 从双臂模型拆出两份单臂 URDF：会产生重复模型和后续同步风险。
+
+## 坐标系与控制语义
+
+`dual_arm_base_link` 是完整模型的公共根坐标系。左右控制节点仍以各自机械臂基座
+作为控制和安全坐标系：
+
+| 机械臂 | 局部基坐标系 | TCP | 活动关节 |
+|---|---|---|---|
+| 左臂 | `scissor_base_link` | `scissor_scissor_tcp` | `scissor_joint_1`～`scissor_joint_7` |
+| 右臂 | `omnipic_base_link` | `omnipic_OmniPic_tcp` | `omnipic_joint_1`～`omnipic_joint_7` |
+
+以公共坐标系 `+X` 向机器人右侧、`+Y` 向前、`+Z` 向上为参照，URDF 中局部轴
+朝向如下：
+
+| 局部轴 | 左臂 `scissor_base_link` | 右臂 `omnipic_base_link` |
+|---|---|---|
+| `+X` | 向下 | 向上 |
+| `+Y` | 向后 | 向后 |
+| `+Z` | 向左、远离机身 | 向右、远离机身 |
+| `-Y` | 向前 | 向前 |
+
+现有左右 `xr_to_robot_matrix` 继续把 PICO 相对位置和相对旋转映射到对应局部基
+坐标系。节点产生的目标仍是 `T_base_tcp`，不显式转换成
+`dual_arm_base_link` 下的绝对目标。
+
+## 求解器设计
+
+左右节点使用同一个 `PlacoIkSolver` 类，但每个节点创建自己的求解器实例、机器人
+状态和 QP 任务。两个实例都加载完整 `Dual_arm.urdf`，不共享可变状态。
+
+求解器构造时接收 `arm=left|right`，按固定映射选择局部基坐标系、TCP、当前侧
+关节和另一侧关节。每个实例执行以下设置：
+
+1. 使用 `mask_fbase(True)` 固定 Placo 浮动基座；
+2. mask 另一侧全部 7 个关节；
+3. 使用 Placo 原生
+   `add_relative_frame_task(base_frame, tcp_frame, target)` 创建局部 TCP 任务；
+4. 保留速度限制、动能正则化、最多 30 次有界迭代和现有收敛阈值。
+
+双臂模型的 Placo 状态为 21 个 q 分量：7 个浮动基座分量、右臂 7 个关节、
+左臂 7 个关节。求解器不再使用固定 `q[7:14]`，而是通过当前侧关节名查询：
+
+- `get_joint_offset()`：定位实际关节反馈和逆解结果在 q 中的位置；
+- `get_joint_v_offset()`：定位对应的 URDF 关节速度上限。
+
+当前 URDF 中右臂 q/v offset 分别为 `7～13`/`6～12`，左臂分别为
+`14～20`/`13～19`；实现仍通过名称查询并对这些预期结果做回归测试。
+
+查询 offset 不放宽模型校验。求解器仍检查完整左右关节集合、当前侧恰好 7 个关节、
+offset 唯一有效，以及所需 base 和 TCP 均存在。
+
+`update_joint_state()` 只写入当前侧 7 个关节反馈，并返回当前 TCP 相对当前侧基座的
+`T_base_tcp`。`solve()` 接受相同坐标语义的目标，设置相对位姿任务并只返回当前侧
+7 个关节结果。另一侧关节保持 mask，不参与本实例求解。
+
+## 启动、安装与配置
+
+`arm_debug.launch.py` 的 `arm:=left|right|both` 全部使用：
+
+```text
+xr_rm_teleop/models/dual_rm75/Dual_arm.urdf
+```
+
+双臂模式继续保留 `left_arm_teleop`、`right_arm_teleop` 节点名，`use_mock` 默认
+保持 `true`。`setup.py` 安装 `Dual_arm.urdf` 以及 `dual_rm75/meshes` 中现有的
+`.STL` 和 `.stl` 文件，不新增依赖。
+
+三份控制配置的局部工作空间统一为：
+
+```yaml
+workspace_min: [-0.70, -0.70, 0.10]
+workspace_max: [0.70, 0.10, 0.75]
+```
+
+其中两侧局部 `-Y` 都是机器人前方，`+Y` 后方最多保留 `0.10 m` 余量。其他工作
+空间轴、圆柱限位、线速度、角速度、关节速度、关节加速度和指令超时参数不变。
+
+真机外设配置采用 URDF TCP 长度，但保留当前硬件选择编号：
+
+```yaml
+tools_in_ee:
+  scissor:
+    pose: [0.0, 0.0, 0.19, 0.0, 0.0, 0.0, 1.0]
+  omnipic:
+    pose: [0.0, 0.0, 0.14, 0.0, 0.0, 0.0, 1.0]
+  minisci:
+    pose: [0.0, 0.0, 0.165, 0.0, 0.0, 0.0, 1.0]
+
+arms:
+  left:
+    scissorgripper: 2
+  right:
+    scissorgripper: 1
+```
+
+左臂保留编号 `2`，继续使用控制器 DO3/DO4；该编号按当前配置顺序选中
+`minisci` 工具坐标，因此更新 `minisci.pose.z`。右臂编号 `1` 继续选中
+`omnipic`。URDF 的左分支名 `scissor_*` 与真机外设编号/配置键是两套既有命名，
+不据此改写硬件编号。两侧负载参数和未选中 `scissor.pose` 保持不变。
+
+README 同步说明新模型路径、左右分支/TCP、局部坐标轴和前方工作区。
+
+## 校验与故障处理
+
+模型路径、arm、关节、frame 或 offset 校验失败时，节点在创建 RealMan 适配器前
+终止启动，不连接真机。
+
+运行期间保留现有行为：
+
+- 关节反馈必须包含 7 个有限数值；
+- TCP 目标必须是有限、合法的齐次变换和旋转矩阵；
+- QP 结果必须满足当前侧 URDF 关节位置和单周期速度限制；
+- QP 不收敛时保持上一组有效关节目标；
+- 反馈异常、反馈超时、XR 超时、Grip 松开和节点退出时执行现有安全停止；
+- `configure_safety_limits` 保持 `true`；
+- `move_to_initial_pose_on_connect` 默认保持 `false`。
+
+完整 URDF 虽包含两臂碰撞几何，本次不启用碰撞约束。真机验证不在本次执行范围；
+后续首次真机验证必须分别验证两臂并保持物理隔离。
+
+## 测试与验收
+
+采用现有 pytest、Placo 0.9.4 和 ROS2 构建流程，不新增测试框架。
+
+自动化测试覆盖：
+
+- 双臂 URDF 的 14 个活动关节、base、TCP、固定挂载和 TCP 长度；
+- 左右实例选择正确的关节、q/v offset 和相对任务 frame；
+- 当前实例只更新和返回本侧 7 个关节，另一侧保持不动；
+- 左右初始关节反馈能得到有限的局部 `T_base_tcp`；
+- 左右小幅可达目标能够收敛，结果满足位置、姿态和关节限制；
+- 非法目标、未初始化求解和不收敛故障路径；
+- 左臂编号 `2` 实际选择 `minisci` 且 TCP 为 `0.165 m`；
+- 右臂编号 `1` 实际选择 `omnipic` 且 TCP 为 `0.14 m`；
+- 三份配置的局部 Y 上界均为 `0.10 m`。
+
+所有命令在 `/home/robot/WS_xr` 执行，并先加载 ROS2 Humble：
+
+```bash
+source /opt/ros/humble/setup.bash
+/home/robot/miniconda3/envs/xr/bin/python -m pytest \
+  src/xr_rm_teleop/test/test_placo_transforms.py -v
+pytest src/xr_rm_teleop/test/test_orientation_control.py
+colcon build --symlink-install
+source install/setup.bash
+timeout 15s ros2 launch xr_rm_bringup arm_debug.launch.py \
+  arm:=both use_mock:=true
+```
+
+最后一条命令只验证安装空间中的新模型能被两个 mock 节点加载；`timeout` 到期退出
+属于预期。整个验收过程不得使用 `use_mock:=false`。

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_bringup/launch/arm_debug.launch.py

@@ -31,13 +31,12 @@ def _config_file(name: str) -> PathJoinSubstitution:
     ])
 
 
-def _rm75_urdf() -> PathJoinSubstitution:
+def _dual_rm75_urdf() -> PathJoinSubstitution:
     return PathJoinSubstitution([
         FindPackageShare("xr_rm_teleop"),
         "models",
-        "rm75_omnipicker",
+        "dual_rm75",
-        "urdf",
+        "Dual_arm.urdf",
-        "RM75-B_OmniPicker_fixed.urdf",
     ])
 
 
@@ -75,7 +74,7 @@ def _single_arm_node(
             _config_file(config_name),
             {
                 "use_mock": use_mock,
-                "robot_urdf_path": _rm75_urdf(),
+                "robot_urdf_path": _dual_rm75_urdf(),
                 "peripheral_config_file": _config_file("peripherals_rm75.yaml"),
                 "peripheral_arm": arm,
                 "tool_command_topic": f"/xr_rm/{arm_name}/tool_enable",
@@ -102,7 +101,7 @@ def _dual_arm_nodes(use_mock: bool) -> list[Node]:
                 config_file,
                 {
                     "use_mock": use_mock,
-                    "robot_urdf_path": _rm75_urdf(),
+                    "robot_urdf_path": _dual_rm75_urdf(),
                     "peripheral_config_file": _config_file("peripherals_rm75.yaml"),
                     "peripheral_arm": "left",
                     "tool_command_topic": "/xr_rm/left_rm75/tool_enable",
@@ -119,7 +118,7 @@ def _dual_arm_nodes(use_mock: bool) -> list[Node]:
                 config_file,
                 {
                     "use_mock": use_mock,
-                    "robot_urdf_path": _rm75_urdf(),
+                    "robot_urdf_path": _dual_rm75_urdf(),
                     "peripheral_config_file": _config_file("peripherals_rm75.yaml"),
                     "peripheral_arm": "right",
                     "tool_command_topic": "/xr_rm/right_rm75/tool_enable",

xr_rm_teleop/models/dual_rm75/Dual_arm.urdf

@@ -0,0 +1,555 @@
+<?xml version='1.0' encoding='utf-8'?>
+<robot name="rm75_dual_arm">
+  <!--Shared supporting base. Adjust the two mount joint origins to match the CAD mounting frames.-->
+  <link name="dual_arm_base_link">
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0" />
+      <geometry>
+        <mesh filename="meshes/dual_arm_base.stl" scale="1 1 1" />
+      </geometry>
+      <material name="dual_arm_base_material">
+        <color rgba="0.5 0.5 0.5 1" />
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0" />
+      <geometry>
+        <mesh filename="meshes/dual_arm_base.stl" scale="1 1 1" />
+      </geometry>
+    </collision>
+  </link>
+  <link name="omnipic_base_link">
+    <inertial>
+      <origin xyz="0.00049987 5.2709E-05 0.060019" rpy="0 0 0" />
+      <mass value="1.862" />
+      <inertia ixx="0.0017232" ixy="-3.1058E-06" ixz="-3.7924E-05" iyy="0.0017051" iyz="1.3691E-06" izz="0.00090158" />
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0" />
+      <geometry>
+        <mesh filename="meshes/base_link.STL" />
+      </geometry>
+      <material name="">
+        <color rgba="1 1 1 1" />
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0" />
+      <geometry>
+        <mesh filename="meshes/base_link.STL" />
+      </geometry>
+    </collision>
+  </link>
+  <link name="omnipic_link_1">
+    <inertial>
+      <origin xyz="0.000241 -0.013273 -0.00995" rpy="0 0 0" />
+      <mass value="1.574" />
+      <inertia ixx="0.002487573" ixy="0.000009663" ixz="-0.000007909" iyy="0.002321038" iyz="0.000179393" izz="0.001450554" />
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0" />
+      <geometry>
+        <mesh filename="meshes/link_1.STL" />
+      </geometry>
+      <material name="">
+        <color rgba="1 1 1 1" />
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0" />
+      <geometry>
+        <mesh filename="meshes/link_1.STL" />
+      </geometry>
+    </collision>
+  </link>
+  <joint name="omnipic_joint_1" type="revolute">
+    <origin xyz="0 0 0.2405" rpy="0 0 0" />
+    <parent link="omnipic_base_link" />
+    <child link="omnipic_link_1" />
+    <axis xyz="0 0 1" />
+    <limit lower="-3.106" upper="3.106" effort="60" velocity="3.14" />
+  </joint>
+  <link name="omnipic_link_2">
+    <inertial>
+      <origin xyz="-0.000357 -0.106789 0.005329" rpy="0 0 0" />
+      <mass value="1.217" />
+      <inertia ixx="0.003494121" ixy="0.000002921" ixz="-0.000005613" iyy="0.000892721" iyz="-0.000583884" izz="0.003444080" />
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0" />
+      <geometry>
+        <mesh filename="meshes/link_2.STL" />
+      </geometry>
+      <material name="">
+        <color rgba="1 1 1 1" />
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0" />
+      <geometry>
+        <mesh filename="meshes/link_2.STL" />
+      </geometry>
+    </collision>
+  </link>
+  <joint name="omnipic_joint_2" type="revolute">
+    <origin xyz="0 0 0" rpy="-1.5708 0 0" />
+    <parent link="omnipic_link_1" />
+    <child link="omnipic_link_2" />
+    <axis xyz="0 0 1" />
+    <limit lower="-2.2689" upper="2.2689" effort="60" velocity="3.14" />
+  </joint>
+  <link name="omnipic_link_3">
+    <inertial>
+      <origin xyz="0.000003 -0.01398 -0.011324" rpy="0 0 0" />
+      <mass value="1.11" />
+      <inertia ixx="0.001836663" ixy="0.000002259" ixz="-0.000004216" iyy="0.001498875" iyz="0.000037167" izz="0.001062545" />
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0" />
+      <geometry>
+        <mesh filename="meshes/link_3.STL" />
+      </geometry>
+      <material name="">
+        <color rgba="1 1 1 1" />
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0" />
+      <geometry>
+        <mesh filename="meshes/link_3.STL" />
+      </geometry>
+    </collision>
+  </link>
+  <joint name="omnipic_joint_3" type="revolute">
+    <origin xyz="0 -0.256 0" rpy="1.5708 0 0" />
+    <parent link="omnipic_link_2" />
+    <child link="omnipic_link_3" />
+    <axis xyz="0 0 1" />
+    <limit lower="-3.106" upper="3.106" effort="30" velocity="3.14" />
+  </joint>
+  <link name="omnipic_link_4">
+    <inertial>
+      <origin xyz="-0.000005 -0.084658 0.004747" rpy="0 0 0" />
+      <mass value="0.685" />
+      <inertia ixx="0.001282444" ixy="-0.000000551" ixz="-0.000000630" iyy="0.000373013" iyz="-0.000232084" izz="0.001256177" />
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0" />
+      <geometry>
+        <mesh filename="meshes/link_4.STL" />
+      </geometry>
+      <material name="">
+        <color rgba="1 1 1 1" />
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0" />
+      <geometry>
+        <mesh filename="meshes/link_4.STL" />
+      </geometry>
+    </collision>
+  </link>
+  <joint name="omnipic_joint_4" type="revolute">
+    <origin xyz="0 0 0" rpy="-1.5708 0 0" />
+    <parent link="omnipic_link_3" />
+    <child link="omnipic_link_4" />
+    <axis xyz="0 0 1" />
+    <limit lower="-2.356" upper="2.356" effort="30" velocity="3.14" />
+  </joint>
+  <link name="omnipic_link_5">
+    <inertial>
+      <origin xyz="0.000078 -0.012937 -0.008781" rpy="0 0 0" />
+      <mass value="0.619" />
+      <inertia ixx="0.000627336" ixy="0.000001636" ixz="-0.000001345" iyy="0.000542455" iyz="0.000034970" izz="0.000370291" />
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0" />
+      <geometry>
+        <mesh filename="meshes/link_5.STL" />
+      </geometry>
+      <material name="">
+        <color rgba="1 1 1 1" />
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0" />
+      <geometry>
+        <mesh filename="meshes/link_5.STL" />
+      </geometry>
+    </collision>
+  </link>
+  <joint name="omnipic_joint_5" type="revolute">
+    <origin xyz="0 -0.21 0" rpy="1.5708 0 0" />
+    <parent link="omnipic_link_4" />
+    <child link="omnipic_link_5" />
+    <axis xyz="0 0 1" />
+    <limit lower="-3.106" upper="3.106" effort="10" velocity="3.14" />
+  </joint>
+  <link name="omnipic_link_6">
+    <inertial>
+      <origin xyz="-0.000014 -0.078524 0.002819" rpy="0 0 0" />
+      <mass value="0.602" />
+      <inertia ixx="0.000780774" ixy="-0.000000121" ixz="-0.000000469" iyy="0.000289973" iyz="-0.000120513" izz="0.000763955" />
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0" />
+      <geometry>
+        <mesh filename="meshes/link_6.STL" />
+      </geometry>
+      <material name="">
+        <color rgba="1 1 1 1" />
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0" />
+      <geometry>
+        <mesh filename="meshes/link_6.STL" />
+      </geometry>
+    </collision>
+  </link>
+  <joint name="omnipic_joint_6" type="revolute">
+    <origin xyz="0 0 0" rpy="-1.5708 0 0" />
+    <parent link="omnipic_link_5" />
+    <child link="omnipic_link_6" />
+    <axis xyz="0 0 1" />
+    <limit lower="-2.234" upper="2.234" effort="10" velocity="3.14" />
+  </joint>
+  <link name="omnipic_link_7">
+    <inertial>
+      <origin xyz="0.001094 -0.000077 -0.010119" rpy="0 0 0" />
+      <mass value="0.107" />
+      <inertia ixx="0.000044123" ixy="-0.000000064" ixz="0.0000003" iyy="0.000035078" iyz="-0.000000029" izz="0.000065445" />
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0" />
+      <geometry>
+        <mesh filename="meshes/link_7.STL" />
+      </geometry>
+      <material name="">
+        <color rgba="1 1 1 1" />
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0" />
+      <geometry>
+        <mesh filename="meshes/link_7.STL" />
+      </geometry>
+    </collision>
+  </link>
+  <joint name="omnipic_joint_7" type="revolute">
+    <origin xyz="0 -0.144 0" rpy="1.5708 0 0" />
+    <parent link="omnipic_link_6" />
+    <child link="omnipic_link_7" />
+    <axis xyz="0 0 1" />
+    <limit lower="-6.28" upper="6.28" effort="10" velocity="3.14" />
+  </joint>
+  <link name="omnipic_gripper_link">
+    <inertial>
+      <origin xyz="0 0 0" rpy="0 0 0" />
+      <mass value="0.1" />
+      <inertia ixx="0.0001" ixy="0" ixz="0" iyy="0.0001" iyz="0" izz="0.0001" />
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0" />
+      <geometry>
+        <mesh filename="meshes/OmniPic.stl" scale="1 1 1" />
+      </geometry>
+      <material name="omnipic_OmniPic_material">
+        <color rgba="0.7 0.7 0.7 1" />
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0" />
+      <geometry>
+        <mesh filename="meshes/OmniPic.stl" scale="1 1 1" />
+      </geometry>
+    </collision>
+  </link>
+  <joint name="omnipic_OmniPic_fixed_joint" type="fixed">
+    <parent link="omnipic_link_7" />
+    <child link="omnipic_gripper_link" />
+    <origin xyz="0 0 0" rpy="0 0 0" />
+  </joint>
+  <link name="omnipic_OmniPic_tcp" />
+  <joint name="omnipic_OmniPic_tcp_fixed" type="fixed">
+    <parent link="omnipic_gripper_link" />
+    <child link="omnipic_OmniPic_tcp" />
+    <origin xyz="0 0 0.14" rpy="0 0 0" />
+  </joint>
+  <!--Omnipic arm mount (physical right): edit xyz/rpy to match dual_arm_base.stl.-->
+  <joint name="omnipic_base_mount_joint" type="fixed">
+    <parent link="dual_arm_base_link" />
+    <child link="omnipic_base_link" />
+    <origin xyz="0.03 0 0" rpy="3.1416 -1.5708 0" />
+  </joint>
+  <link name="scissor_base_link">
+    <inertial>
+      <origin xyz="0.00049987 5.2709E-05 0.060019" rpy="0 0 0" />
+      <mass value="1.862" />
+      <inertia ixx="0.0017232" ixy="-3.1058E-06" ixz="-3.7924E-05" iyy="0.0017051" iyz="1.3691E-06" izz="0.00090158" />
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0" />
+      <geometry>
+        <mesh filename="meshes/base_link.STL" />
+      </geometry>
+      <material name="">
+        <color rgba="1 1 1 1" />
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0" />
+      <geometry>
+        <mesh filename="meshes/base_link.STL" />
+      </geometry>
+    </collision>
+  </link>
+  <link name="scissor_link_1">
+    <inertial>
+      <origin xyz="0.000241 -0.013273 -0.00995" rpy="0 0 0" />
+      <mass value="1.574" />
+      <inertia ixx="0.002487573" ixy="0.000009663" ixz="-0.000007909" iyy="0.002321038" iyz="0.000179393" izz="0.001450554" />
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0" />
+      <geometry>
+        <mesh filename="meshes/link_1.STL" />
+      </geometry>
+      <material name="">
+        <color rgba="1 1 1 1" />
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0" />
+      <geometry>
+        <mesh filename="meshes/link_1.STL" />
+      </geometry>
+    </collision>
+  </link>
+  <joint name="scissor_joint_1" type="revolute">
+    <origin xyz="0 0 0.2405" rpy="0 0 0" />
+    <parent link="scissor_base_link" />
+    <child link="scissor_link_1" />
+    <axis xyz="0 0 1" />
+    <limit lower="-3.106" upper="3.106" effort="60" velocity="3.14" />
+  </joint>
+  <link name="scissor_link_2">
+    <inertial>
+      <origin xyz="-0.000357 -0.106789 0.005329" rpy="0 0 0" />
+      <mass value="1.217" />
+      <inertia ixx="0.003494121" ixy="0.000002921" ixz="-0.000005613" iyy="0.000892721" iyz="-0.000583884" izz="0.003444080" />
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0" />
+      <geometry>
+        <mesh filename="meshes/link_2.STL" />
+      </geometry>
+      <material name="">
+        <color rgba="1 1 1 1" />
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0" />
+      <geometry>
+        <mesh filename="meshes/link_2.STL" />
+      </geometry>
+    </collision>
+  </link>
+  <joint name="scissor_joint_2" type="revolute">
+    <origin xyz="0 0 0" rpy="-1.5708 0 0" />
+    <parent link="scissor_link_1" />
+    <child link="scissor_link_2" />
+    <axis xyz="0 0 1" />
+    <limit lower="-2.2689" upper="2.2689" effort="60" velocity="3.14" />
+  </joint>
+  <link name="scissor_link_3">
+    <inertial>
+      <origin xyz="0.000003 -0.01398 -0.011324" rpy="0 0 0" />
+      <mass value="1.11" />
+      <inertia ixx="0.001836663" ixy="0.000002259" ixz="-0.000004216" iyy="0.001498875" iyz="0.000037167" izz="0.001062545" />
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0" />
+      <geometry>
+        <mesh filename="meshes/link_3.STL" />
+      </geometry>
+      <material name="">
+        <color rgba="1 1 1 1" />
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0" />
+      <geometry>
+        <mesh filename="meshes/link_3.STL" />
+      </geometry>
+    </collision>
+  </link>
+  <joint name="scissor_joint_3" type="revolute">
+    <origin xyz="0 -0.256 0" rpy="1.5708 0 0" />
+    <parent link="scissor_link_2" />
+    <child link="scissor_link_3" />
+    <axis xyz="0 0 1" />
+    <limit lower="-3.106" upper="3.106" effort="30" velocity="3.14" />
+  </joint>
+  <link name="scissor_link_4">
+    <inertial>
+      <origin xyz="-0.000005 -0.084658 0.004747" rpy="0 0 0" />
+      <mass value="0.685" />
+      <inertia ixx="0.001282444" ixy="-0.000000551" ixz="-0.000000630" iyy="0.000373013" iyz="-0.000232084" izz="0.001256177" />
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0" />
+      <geometry>
+        <mesh filename="meshes/link_4.STL" />
+      </geometry>
+      <material name="">
+        <color rgba="1 1 1 1" />
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0" />
+      <geometry>
+        <mesh filename="meshes/link_4.STL" />
+      </geometry>
+    </collision>
+  </link>
+  <joint name="scissor_joint_4" type="revolute">
+    <origin xyz="0 0 0" rpy="-1.5708 0 0" />
+    <parent link="scissor_link_3" />
+    <child link="scissor_link_4" />
+    <axis xyz="0 0 1" />
+    <limit lower="-2.356" upper="2.356" effort="30" velocity="3.14" />
+  </joint>
+  <link name="scissor_link_5">
+    <inertial>
+      <origin xyz="0.000078 -0.012937 -0.008781" rpy="0 0 0" />
+      <mass value="0.619" />
+      <inertia ixx="0.000627336" ixy="0.000001636" ixz="-0.000001345" iyy="0.000542455" iyz="0.000034970" izz="0.000370291" />
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0" />
+      <geometry>
+        <mesh filename="meshes/link_5.STL" />
+      </geometry>
+      <material name="">
+        <color rgba="1 1 1 1" />
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0" />
+      <geometry>
+        <mesh filename="meshes/link_5.STL" />
+      </geometry>
+    </collision>
+  </link>
+  <joint name="scissor_joint_5" type="revolute">
+    <origin xyz="0 -0.21 0" rpy="1.5708 0 0" />
+    <parent link="scissor_link_4" />
+    <child link="scissor_link_5" />
+    <axis xyz="0 0 1" />
+    <limit lower="-3.106" upper="3.106" effort="10" velocity="3.14" />
+  </joint>
+  <link name="scissor_link_6">
+    <inertial>
+      <origin xyz="-0.000014 -0.078524 0.002819" rpy="0 0 0" />
+      <mass value="0.602" />
+      <inertia ixx="0.000780774" ixy="-0.000000121" ixz="-0.000000469" iyy="0.000289973" iyz="-0.000120513" izz="0.000763955" />
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0" />
+      <geometry>
+        <mesh filename="meshes/link_6.STL" />
+      </geometry>
+      <material name="">
+        <color rgba="1 1 1 1" />
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0" />
+      <geometry>
+        <mesh filename="meshes/link_6.STL" />
+      </geometry>
+    </collision>
+  </link>
+  <joint name="scissor_joint_6" type="revolute">
+    <origin xyz="0 0 0" rpy="-1.5708 0 0" />
+    <parent link="scissor_link_5" />
+    <child link="scissor_link_6" />
+    <axis xyz="0 0 1" />
+    <limit lower="-2.234" upper="2.234" effort="10" velocity="3.14" />
+  </joint>
+  <link name="scissor_link_7">
+    <inertial>
+      <origin xyz="0.001094 -0.000077 -0.010119" rpy="0 0 0" />
+      <mass value="0.107" />
+      <inertia ixx="0.000044123" ixy="-0.000000064" ixz="0.0000003" iyy="0.000035078" iyz="-0.000000029" izz="0.000065445" />
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0" />
+      <geometry>
+        <mesh filename="meshes/link_7.STL" />
+      </geometry>
+      <material name="">
+        <color rgba="1 1 1 1" />
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0" />
+      <geometry>
+        <mesh filename="meshes/link_7.STL" />
+      </geometry>
+    </collision>
+  </link>
+  <joint name="scissor_joint_7" type="revolute">
+    <origin xyz="0 -0.144 0" rpy="1.5708 0 0" />
+    <parent link="scissor_link_6" />
+    <child link="scissor_link_7" />
+    <axis xyz="0 0 1" />
+    <limit lower="-6.28" upper="6.28" effort="10" velocity="3.14" />
+  </joint>
+  <link name="scissor_scissor_link">
+    <inertial>
+      <origin xyz="0 0 0" rpy="0 0 0" />
+      <mass value="0.1" />
+      <inertia ixx="0.0001" ixy="0" ixz="0" iyy="0.0001" iyz="0" izz="0.0001" />
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0" />
+      <geometry>
+        <mesh filename="meshes/scissor.stl" scale="1 1 1" />
+      </geometry>
+      <material name="scissor_scissor_material">
+        <color rgba="0.7 0.7 0.7 1" />
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0" />
+      <geometry>
+        <mesh filename="meshes/scissor.stl" scale="1 1 1" />
+      </geometry>
+    </collision>
+  </link>
+  <joint name="scissor_scissor_fixed_joint" type="fixed">
+    <parent link="scissor_link_7" />
+    <child link="scissor_scissor_link" />
+    <origin xyz="0 0 0.165" rpy="0 0 0" />
+  </joint>
+  <link name="scissor_scissor_tcp" />
+  <joint name="scissor_scissor_tcp_fixed" type="fixed">
+    <parent link="scissor_scissor_link" />
+    <child link="scissor_scissor_tcp" />
+    <origin xyz="0 0 0" rpy="0 0 0" />
+  </joint>
+  <link name="scissor_camera_tcp" />
+  <joint name="scissor_camera_tcp_fixed" type="fixed">
+    <parent link="scissor_scissor_link" />
+    <child link="scissor_camera_tcp" />
+    <origin xyz="0.042 0 -0.0755" rpy="0 0 1.57" />
+  </joint>
+  <!--Scissor arm mount (physical left): edit xyz/rpy to match dual_arm_base.stl.-->
+  <joint name="scissor_base_mount_joint" type="fixed">
+    <parent link="dual_arm_base_link" />
+    <child link="scissor_base_link" />
+    <origin xyz="-0.030 0 0" rpy="-3.1416 1.5708 0" />
+  </joint>
+</robot>

xr_rm_teleop/setup.py

@@ -24,6 +24,15 @@ setup(
             f"share/{package_name}/models/rm75/meshes",
             glob("models/rm75/meshes/*.STL"),
         ),
+        (
+            f"share/{package_name}/models/dual_rm75",
+            ["models/dual_rm75/Dual_arm.urdf"],
+        ),
+        (
+            f"share/{package_name}/models/dual_rm75/meshes",
+            glob("models/dual_rm75/meshes/*.STL")
+            + glob("models/dual_rm75/meshes/*.stl"),
+        ),
         (
             f"share/{package_name}/models/rm75_omnipicker/urdf",
             glob("models/rm75_omnipicker/urdf/*.urdf"),

xr_rm_teleop/test/placo_ik_smoke.py

@@ -11,8 +11,13 @@ from xr_rm_teleop.placo_ik_solver import PlacoIkSolver
 
 
 CASES = {
-    "left": [-79.55, -9.99, 71.01, 101.45, 95.07, -84.47, -74.52],
+    "left": [-78.81, 3.22, 67.96, 97.12, 95.08, -81.11, -74.55],
-    "right": [-90.14, 3.76, -86.89, 87.89, -96.53, -79.62, -90.04],
+    "right": [-86.10, 22.80, -89.57, 93.98, -91.82, -87.32, -89.35],
+}
+
+TOOL_CHAINS = {
+    "left": ("scissor_base_link", "scissor_link_7", 0.165),
+    "right": ("omnipic_base_link", "omnipic_link_7", 0.14),
 }
 
 
@@ -37,13 +42,16 @@ def main() -> None:
     urdf_path = Path(sys.argv[1]).resolve()
     for arm, joint_degrees in CASES.items():
         initial_joints = np.deg2rad(joint_degrees)
-        drift_solver = PlacoIkSolver(str(urdf_path), 1.0 / 125.0)
+        drift_solver = PlacoIkSolver(str(urdf_path), 1.0 / 125.0, arm)
         joints = initial_joints.tolist()
         stationary_target = drift_solver.update_joint_state(joints)
-        flange = drift_solver._robot.get_T_world_frame("link_7")
+        base_frame, flange_frame, tcp_length = TOOL_CHAINS[arm]
-        flange_to_tcp = np.linalg.inv(flange) @ stationary_target
+        world_to_base = drift_solver._robot.get_T_world_frame(base_frame)
-        assert np.allclose(flange_to_tcp[:3, 3], [0.0, 0.0, 0.16])
+        world_to_flange = drift_solver._robot.get_T_world_frame(flange_frame)
-        assert np.allclose(flange_to_tcp[:3, :3], np.eye(3))
+        base_to_flange = np.linalg.inv(world_to_base) @ world_to_flange
+        flange_to_tcp = np.linalg.inv(base_to_flange) @ stationary_target
+        assert np.allclose(flange_to_tcp[:3, 3], [0.0, 0.0, tcp_length])
+        assert np.allclose(flange_to_tcp[:3, :3], np.eye(3), atol=1e-5)
         for _ in range(250):
             drift_solver.update_joint_state(joints)
             joints = drift_solver.solve(stationary_target)
@@ -54,7 +62,7 @@ def main() -> None:
             f"{arm} stationary target drifted {drift_degrees:.3f}deg"
         )
 
-        solver = PlacoIkSolver(str(urdf_path), 1.0 / 125.0)
+        solver = PlacoIkSolver(str(urdf_path), 1.0 / 125.0, arm)
         joints = initial_joints.tolist()
         current = solver.update_joint_state(joints)
         assert current.shape == (4, 4)

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")],

xr_rm_teleop/test/test_placo_transforms.py

@@ -7,76 +7,171 @@ import numpy as np
 import pytest
 
 from xr_rm_teleop.placo_ik_solver import (
+    QP_ORIENTATION_TOLERANCE_RAD,
     QP_POSITION_TOLERANCE_M,
     PlacoIkSolver,
     _validated_transform,
 )
 
+DUAL_URDF_PATH = (
+    Path(__file__).resolve().parents[1]
+    / "models"
+    / "dual_rm75"
+    / "Dual_arm.urdf"
+)
+ARM_CASES = (
+    (
+        "left",
+        [-78.81, 3.22, 67.96, 97.12, 95.08, -81.11, -74.55],
+        list(range(14, 21)),
+        list(range(13, 20)),
+        "omnipic",
+        "scissor_base_link",
+        "scissor_scissor_tcp",
+    ),
+    (
+        "right",
+        [-86.10, 22.80, -89.57, 93.98, -91.82, -87.32, -89.35],
+        list(range(7, 14)),
+        list(range(6, 13)),
+        "scissor",
+        "omnipic_base_link",
+        "omnipic_OmniPic_tcp",
+    ),
+)
 
-def test_fixed_urdf_has_seven_moving_joints_and_omnipicker_tcp() -> None:
+
-    urdf_path = (
+def test_dual_urdf_has_expected_joints_meshes_and_tcp_frames() -> None:
-        Path(__file__).resolve().parents[1]
+    root = ElementTree.parse(DUAL_URDF_PATH).getroot()
-        / "models"
-        / "rm75_omnipicker"
-        / "urdf"
-        / "RM75-B_OmniPicker_fixed.urdf"
-    )
-    root = ElementTree.parse(urdf_path).getroot()
     moving_joint_names = [
         joint.attrib["name"]
         for joint in root.findall("joint")
         if joint.attrib["type"] != "fixed"
     ]
-    tcp_joint = root.find("joint[@name='omnipicker_tcp_joint']")
     mesh_filenames = [
         mesh.attrib["filename"]
         for mesh in root.findall(".//mesh")
     ]
+    fixed_joints = {
+        "omnipic_base_mount_joint": (
+            "dual_arm_base_link",
+            "omnipic_base_link",
+            None,
+        ),
+        "scissor_base_mount_joint": (
+            "dual_arm_base_link",
+            "scissor_base_link",
+            None,
+        ),
+        "omnipic_OmniPic_tcp_fixed": (
+            "omnipic_gripper_link",
+            "omnipic_OmniPic_tcp",
+            "0 0 0.14",
+        ),
+        "scissor_scissor_tcp_fixed": (
+            "scissor_scissor_link",
+            "scissor_scissor_tcp",
+            "0 0 0",
+        ),
+        "scissor_scissor_fixed_joint": (
+            "scissor_link_7",
+            "scissor_scissor_link",
+            "0 0 0.165",
+        ),
+    }
 
-    assert moving_joint_names == [f"joint_{index}" for index in range(1, 8)]
+    assert moving_joint_names == [
-    assert all(
+        *[f"omnipic_joint_{index}" for index in range(1, 8)],
-        filename.startswith(
+        *[f"scissor_joint_{index}" for index in range(1, 8)],
-            "package://xr_rm_teleop/models/rm75_omnipicker/meshes/"
-        )
-        for filename in mesh_filenames
-    )
-    assert tcp_joint is not None
-    assert tcp_joint.attrib["type"] == "fixed"
-    assert tcp_joint.find("parent").attrib["link"] == "omnipicker_base_link"
-    assert tcp_joint.find("child").attrib["link"] == "omnipicker_tcp"
-    assert tcp_joint.find("origin").attrib["xyz"] == "0 0 0.16"
-    assert tcp_joint.find("origin").attrib["rpy"] == "0 0 0"
-
-
-def _rm75_placo_solver() -> tuple[PlacoIkSolver, list[float]]:
-    pytest.importorskip("placo")
-    urdf_path = (
-        Path(__file__).resolve().parents[1]
-        / "models"
-        / "rm75_omnipicker"
-        / "urdf"
-        / "RM75-B_OmniPicker_fixed.urdf"
-    )
-    joints = [
-        math.radians(value)
-        for value in [
-            -90.14,
-            3.76,
-            -86.89,
-            87.89,
-            -96.53,
-            -79.62,
-            -90.04,
-        ]
     ]
-    return PlacoIkSolver(str(urdf_path), 1.0 / 90.0), joints
+    assert all(filename.startswith("meshes/") for filename in mesh_filenames)
+    for name, (parent, child, xyz) in fixed_joints.items():
+        joint = root.find(f"joint[@name='{name}']")
+        assert joint is not None
+        assert joint.attrib["type"] == "fixed"
+        assert joint.find("parent").attrib["link"] == parent
+        assert joint.find("child").attrib["link"] == child
+        if xyz is not None:
+            assert joint.find("origin").attrib["xyz"] == xyz
 
 
-def test_qp_solve_converges_to_reachable_tcp_target() -> None:
+def _dual_placo_solver(
-    solver, joints = _rm75_placo_solver()
+    arm: str,
+    joint_degrees: list[float],
+) -> tuple[PlacoIkSolver, list[float]]:
+    pytest.importorskip("placo")
+    joints = [math.radians(value) for value in joint_degrees]
+    return PlacoIkSolver(str(DUAL_URDF_PATH), 1.0 / 90.0, arm), joints
+
+
+@pytest.mark.parametrize(
+    "arm,joint_degrees,q_offsets,v_offsets,inactive_prefix,"
+    "expected_base_frame,expected_tcp_frame",
+    ARM_CASES,
+)
+def test_solver_uses_arm_specific_offsets(
+    arm: str,
+    joint_degrees: list[float],
+    q_offsets: list[int],
+    v_offsets: list[int],
+    inactive_prefix: str,
+    expected_base_frame: str,
+    expected_tcp_frame: str,
+) -> None:
+    solver, _ = _dual_placo_solver(arm, joint_degrees)
+
+    assert solver._q_offsets.tolist() == q_offsets
+    assert solver._v_offsets.tolist() == v_offsets
+
+
+@pytest.mark.parametrize(
+    "arm,joint_degrees,q_offsets,v_offsets,inactive_prefix,"
+    "expected_base_frame,expected_tcp_frame",
+    ARM_CASES,
+)
+def test_joint_state_pose_is_relative_to_selected_arm_base(
+    arm: str,
+    joint_degrees: list[float],
+    q_offsets: list[int],
+    v_offsets: list[int],
+    inactive_prefix: str,
+    expected_base_frame: str,
+    expected_tcp_frame: str,
+) -> None:
+    solver, joints = _dual_placo_solver(arm, joint_degrees)
+
+    actual_pose = solver.update_joint_state(joints)
+    assert solver._base_frame == expected_base_frame
+    assert solver._tcp_frame == expected_tcp_frame
+    world_base = solver._robot.get_T_world_frame(expected_base_frame)
+    world_tcp = solver._robot.get_T_world_frame(expected_tcp_frame)
+
+    assert actual_pose == pytest.approx(np.linalg.inv(world_base) @ world_tcp)
+
+
+@pytest.mark.parametrize(
+    "arm,joint_degrees,q_offsets,v_offsets,inactive_prefix,"
+    "expected_base_frame,expected_tcp_frame",
+    ARM_CASES,
+)
+def test_qp_solve_converges_without_moving_inactive_arm(
+    arm: str,
+    joint_degrees: list[float],
+    q_offsets: list[int],
+    v_offsets: list[int],
+    inactive_prefix: str,
+    expected_base_frame: str,
+    expected_tcp_frame: str,
+) -> None:
+    solver, joints = _dual_placo_solver(arm, joint_degrees)
     start_pose = solver.update_joint_state(joints)
+    inactive_q_offsets = [
+        solver._robot.get_joint_offset(f"{inactive_prefix}_joint_{index}")
+        for index in range(1, 8)
+    ]
+    inactive_before = solver._robot.state.q[inactive_q_offsets].copy()
     target_pose = start_pose.copy()
-    target_pose[0, 3] += 0.07
+    target_pose[0, 3] += 0.01
 
     result = solver.solve(target_pose)
     reached_pose = solver.update_joint_state(result)
@@ -96,17 +191,28 @@ def test_qp_solve_converges_to_reachable_tcp_target() -> None:
         )
     )
 
+    assert np.asarray(result).shape == (7,)
+    assert np.isfinite(result).all()
     assert position_error <= QP_POSITION_TOLERANCE_M
-    assert orientation_error <= 5e-3
+    assert orientation_error <= QP_ORIENTATION_TOLERANCE_RAD
+    assert solver._robot.state.q[inactive_q_offsets] == pytest.approx(
+        inactive_before
+    )
+
+
+def test_solver_rejects_unknown_arm() -> None:
+    with pytest.raises(ValueError, match="arm must be left or right"):
+        PlacoIkSolver(str(DUAL_URDF_PATH), 1.0 / 90.0, "middle")
 
 
 def test_qp_solve_accepts_position_error_within_two_millimeters() -> None:
     solver = object.__new__(PlacoIkSolver)
     solver._actual_joints = np.zeros(7)
+    solver._q_offsets = np.arange(7, 14)
     solver._robot = SimpleNamespace(
-        state=SimpleNamespace(q=np.zeros(14))
+        state=SimpleNamespace(q=np.zeros(21))
     )
-    solver._frame_task = SimpleNamespace(T_world_frame=None)
+    solver._frame_task = SimpleNamespace(T_a_b=None)
     solver._target_errors = lambda: (1.5e-3, 0.0)
 
     result = solver.solve(np.eye(4))
@@ -117,11 +223,12 @@ def test_qp_solve_accepts_position_error_within_two_millimeters() -> None:
 def test_qp_solve_rejects_position_error_above_two_millimeters() -> None:
     solver = object.__new__(PlacoIkSolver)
     solver._actual_joints = np.zeros(7)
+    solver._q_offsets = np.arange(7, 14)
     solver._robot = SimpleNamespace(
-        state=SimpleNamespace(q=np.zeros(14)),
+        state=SimpleNamespace(q=np.zeros(21)),
         update_kinematics=lambda: None,
     )
-    solver._frame_task = SimpleNamespace(T_world_frame=None)
+    solver._frame_task = SimpleNamespace(T_a_b=None)
     solver._solver = SimpleNamespace(solve=lambda update: None)
     solver._validate_result = lambda result, previous: None
     solver._target_errors = lambda: (2.1e-3, 0.0)

xr_rm_teleop/xr_rm_teleop/placo_ik_solver.py

@@ -8,8 +8,24 @@ from pathlib import Path
 import numpy as np
 
 EXPECTED_PLACO_VERSION = "0.9.4"
-RM75_JOINT_NAMES = [f"joint_{index}" for index in range(1, 8)]
+ARM_CHAINS = {
-RM75_Q_SLICE = slice(7, 14)
+    "left": (
+        "scissor_base_link",
+        "scissor_scissor_tcp",
+        "scissor",
+        "omnipic",
+    ),
+    "right": (
+        "omnipic_base_link",
+        "omnipic_OmniPic_tcp",
+        "omnipic",
+        "scissor",
+    ),
+}
+DUAL_RM75_JOINT_NAMES = [
+    *[f"omnipic_joint_{index}" for index in range(1, 8)],
+    *[f"scissor_joint_{index}" for index in range(1, 8)],
+]
 QP_MAX_ITERATIONS = 30
 QP_POSITION_TOLERANCE_M = 2e-3
 QP_ORIENTATION_TOLERANCE_RAD = 5e-3
@@ -43,9 +59,21 @@ class PlacoIkSolver:
         self,
         urdf_path: str,
         dt: float,
+        arm: str,
     ) -> None:
         if dt <= 0.0:
             raise ValueError("dt must be positive")
+        if arm not in ARM_CHAINS:
+            raise ValueError("arm must be left or right")
+        self._base_frame, self._tcp_frame, prefix, inactive_prefix = (
+            ARM_CHAINS[arm]
+        )
+        self._joint_names = [
+            f"{prefix}_joint_{index}" for index in range(1, 8)
+        ]
+        inactive_joint_names = [
+            f"{inactive_prefix}_joint_{index}" for index in range(1, 8)
+        ]
         try:
             installed_version = version("placo")
             import placo
@@ -65,30 +93,44 @@ class PlacoIkSolver:
 
         self._dt = dt
         self._robot = placo.RobotWrapper(str(model_path))
-        if self._robot.state.q.shape != (14,):
+        if self._robot.state.q.shape != (21,):
             raise RuntimeError(
-                f"expected Placo q shape (14,), got {self._robot.state.q.shape}"
+                "expected Placo q shape (21,), got "
+                f"{self._robot.state.q.shape}"
             )
-        if list(self._robot.joint_names()) != RM75_JOINT_NAMES:
+        if list(self._robot.joint_names()) != DUAL_RM75_JOINT_NAMES:
             raise RuntimeError(
-                f"unexpected RM75 joint order: {list(self._robot.joint_names())}"
+                "unexpected dual RM75 joint order: "
+                f"{list(self._robot.joint_names())}"
+            )
+
+        self._q_offsets = np.asarray(
+            [self._robot.get_joint_offset(name) for name in self._joint_names],
+            dtype=int,
+        )
+        self._v_offsets = np.asarray(
+            [
+                self._robot.get_joint_v_offset(name)
+                for name in self._joint_names
+            ],
+            dtype=int,
+        )
+        if len(set(self._q_offsets.tolist())) != 7:
+            raise RuntimeError(
+                f"invalid RM75 q offsets: {self._q_offsets.tolist()}"
+            )
+        if len(set(self._v_offsets.tolist())) != 7:
+            raise RuntimeError(
+                f"invalid RM75 v offsets: {self._v_offsets.tolist()}"
             )
-        offsets = [
-            self._robot.get_joint_offset(name) for name in RM75_JOINT_NAMES
-        ]
-        if offsets != list(range(7, 14)):
-            raise RuntimeError(f"unexpected RM75 q offsets: {offsets}")
 
         self._joint_limits = np.asarray(
-            [self._robot.get_joint_limits(name) for name in RM75_JOINT_NAMES]
+            [self._robot.get_joint_limits(name) for name in self._joint_names]
         )
-        velocity_offsets = [
-            self._robot.get_joint_v_offset(name) for name in RM75_JOINT_NAMES
-        ]
         self._velocity_limits = np.asarray(
             [
                 self._robot.model.velocityLimit[index]
-                for index in velocity_offsets
+                for index in self._v_offsets
             ]
         )
         self._actual_joints: np.ndarray | None = None
@@ -96,12 +138,15 @@ class PlacoIkSolver:
         self._solver = placo.KinematicsSolver(self._robot)
         self._solver.dt = dt
         self._solver.mask_fbase(True)
+        for name in inactive_joint_names:
+            self._solver.mask_dof(name)
         self._solver.enable_velocity_limits(True)
-        self._frame_task = self._solver.add_frame_task(
+        self._frame_task = self._solver.add_relative_frame_task(
-            "omnipicker_tcp",
+            self._base_frame,
+            self._tcp_frame,
             np.eye(4),
         )
-        self._frame_task.configure("rm75_frame", "soft", 1.0)
+        self._frame_task.configure("rm75_relative_frame", "soft", 1.0)
         self._solver.add_kinetic_energy_regularization_task(1e-6)
 
     @property
@@ -114,11 +159,14 @@ class PlacoIkSolver:
             raise ValueError("joint state must contain 7 finite values")
         is_first_feedback = self._actual_joints is None
         self._actual_joints = values.copy()
-        self._robot.state.q[RM75_Q_SLICE] = values
+        self._robot.state.q[self._q_offsets] = values
         self._robot.update_kinematics()
-        base_to_tool = self._robot.get_T_world_frame("omnipicker_tcp")
+        base_to_tool = (
+            np.linalg.inv(self._robot.get_T_world_frame(self._base_frame))
+            @ self._robot.get_T_world_frame(self._tcp_frame)
+        )
         if is_first_feedback:
-            self._frame_task.T_world_frame = base_to_tool.copy()
+            self._frame_task.T_a_b = base_to_tool.copy()
         return base_to_tool.copy()
 
     def _target_errors(self) -> tuple[float, float]:
@@ -134,11 +182,11 @@ class PlacoIkSolver:
     def solve(self, target_tool_pose: np.ndarray) -> list[float]:
         if self._actual_joints is None:
             raise RuntimeError("joint state must be initialized before QP solve")
-        self._frame_task.T_world_frame = _validated_transform(
+        self._frame_task.T_a_b = _validated_transform(
             target_tool_pose
         )
         result = np.asarray(
-            self._robot.state.q[RM75_Q_SLICE],
+            self._robot.state.q[self._q_offsets],
             dtype=float,
         ).copy()
         position_error, orientation_error = self._target_errors()
@@ -153,7 +201,7 @@ class PlacoIkSolver:
             self._solver.solve(True)
             self._robot.update_kinematics()
             result = np.asarray(
-                self._robot.state.q[RM75_Q_SLICE],
+                self._robot.state.q[self._q_offsets],
                 dtype=float,
             ).copy()
             self._validate_result(result, previous)

xr_rm_teleop/xr_rm_teleop/single_arm_velocity_teleop.py

@@ -325,6 +325,7 @@ class SingleArmVelocityTeleop(Node):
         self._ik_solver = PlacoIkSolver(
             str(self.get_parameter("robot_urdf_path").value),
             self._dt,
+            peripheral_arm,
         )
         self._adapter = self._make_adapter()
         self._adapter.connect()