871 lines
26 KiB
Markdown
871 lines
26 KiB
Markdown
# 双 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` 启动验收取得新鲜证据。
|
||
- 未连接真机，未增加碰撞控制、依赖或无关重构。
|