diff --git a/docs/superpowers/plans/2026-08-03-dual-rm75-ik-model.md b/docs/superpowers/plans/2026-08-03-dual-rm75-ik-model.md new file mode 100644 index 0000000..6284700 --- /dev/null +++ b/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` 启动验收取得新鲜证据。 +- 未连接真机，未增加碰撞控制、依赖或无关重构。 diff --git a/docs/superpowers/specs/2026-08-03-dual-rm75-ik-model-design.md b/docs/superpowers/specs/2026-08-03-dual-rm75-ik-model-design.md new file mode 100644 index 0000000..304cb94 --- /dev/null +++ b/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`。