1645 lines
51 KiB
Markdown
1645 lines
51 KiB
Markdown
# RM75 Placo 单步 QP 逆解 Implementation Plan
|
||
|
||
> **For agentic workers:** REQUIRED SUB-SKILL: Use superpowers:subagent-driven-development (recommended) or superpowers:executing-plans to implement this plan task-by-task. Steps use checkbox (`- [ ]`) syntax for tracking.
|
||
|
||
**Goal:** 用 Placo 0.9.4 每控制周期执行一次 QP，把左右 RM75 的工具 TCP 目标转换成 7 个关节目标并通过 `rm_movej_canfd` 下发。
|
||
|
||
**Architecture:** 继续使用现有 `single_arm_velocity_teleop` 节点、`RealManAdapter` 和 `arm_debug.launch.py`。新增一个进程内 `PlacoIkSolver`，从首帧有效关节反馈初始化 `robot.state.q[7:14]`，固定虚拟基座，并在现有 TCP 安全限幅之后执行一步 QP；Adapter 的同一个 SDK 连接同时承担关节反馈、关节指令、安全停止和工具控制。
|
||
|
||
**Tech Stack:** Ubuntu 22.04、ROS2 Humble、Python 3.10、Placo 0.9.4、Pin 3.7.0、NumPy、睿尔曼 Python API2、pytest。
|
||
|
||
**Repository rule:** 未获得用户明确授权前不执行 `git commit`、`git push` 或真机命令。所有 Codex 运行验证均使用 `use_mock:=true`；真机步骤只写入检查清单，由用户执行。
|
||
|
||
---
|
||
|
||
## 文件结构
|
||
|
||
本次只触及以下职责边界：
|
||
|
||
- Create: `xr_rm_teleop/models/rm75/RM75-B.urdf`
|
||
- Create: `xr_rm_teleop/models/rm75/meshes/base_link.STL`
|
||
- Create: `xr_rm_teleop/models/rm75/meshes/link_1.STL`
|
||
- Create: `xr_rm_teleop/models/rm75/meshes/link_2.STL`
|
||
- Create: `xr_rm_teleop/models/rm75/meshes/link_3.STL`
|
||
- Create: `xr_rm_teleop/models/rm75/meshes/link_4.STL`
|
||
- Create: `xr_rm_teleop/models/rm75/meshes/link_5.STL`
|
||
- Create: `xr_rm_teleop/models/rm75/meshes/link_6.STL`
|
||
- Create: `xr_rm_teleop/models/rm75/meshes/link_7.STL`
|
||
- Create: `xr_rm_teleop/xr_rm_teleop/placo_ik_solver.py` — Placo 模型、工具变换和一步 QP。
|
||
- Create: `xr_rm_teleop/test/test_placo_transforms.py` — 不依赖 Placo 的 TCP/法兰变换测试。
|
||
- Create: `xr_rm_teleop/test/test_joint_control.py` — 首帧反馈门控和 last-known-good 测试。
|
||
- Create: `xr_rm_teleop/test/placo_ik_smoke.py` — 使用 XR Python 的左右臂 45 周期数值验收。
|
||
- Modify: `xr_rm_teleop/xr_rm_teleop/fun_peripheral.py` — 让已加载的配置直接提供所选工具名和位姿。
|
||
- Modify: `xr_rm_teleop/xr_rm_teleop/realman_adapter.py` — 用关节反馈缓存和 `rm_movej_canfd` 替换笛卡尔接口。
|
||
- Modify: `xr_rm_teleop/xr_rm_teleop/single_arm_velocity_teleop.py` — 保留现有 TCP 目标生成与安全限幅，接入反馈门控和 QP。
|
||
- Modify: `xr_rm_teleop/test/test_initial_joint_pose.py` — 覆盖 SDK 关节反馈与关节透传参数。
|
||
- Modify: `xr_rm_teleop/test/test_orientation_control.py` — 删除已移除的 mock 笛卡尔接口测试，其余姿态控制回归测试保持不变。
|
||
- Modify: `xr_rm_teleop/setup.py` — 安装 URDF 和网格，不声明或下载 Placo。
|
||
- Modify: `xr_rm_bringup/launch/arm_debug.launch.py` — 显式用 XR Python 启动遥操作节点并传入 URDF。
|
||
- Modify: `xr_rm_bringup/config/left_arm_rm75.yaml`
|
||
- Modify: `xr_rm_bringup/config/right_arm_rm75.yaml`
|
||
- Modify: `xr_rm_bringup/config/dual_arm_rm75.yaml`
|
||
- Modify: `README.md` — 更新控制链路、环境约束和左右臂独立验收步骤。
|
||
|
||
不修改 `xr_rm_interfaces`，不新增 ROS 消息，不新增 QP ROS 节点，不修改 `launcher_ui.py` 的进程拓扑。
|
||
|
||
### Task 1: 导入 RM75 模型并实现工具变换
|
||
|
||
**Files:**
|
||
|
||
- Create: `xr_rm_teleop/models/rm75/RM75-B.urdf`
|
||
- Create: `xr_rm_teleop/models/rm75/meshes/*.STL`
|
||
- Create: `xr_rm_teleop/xr_rm_teleop/placo_ik_solver.py`
|
||
- Create: `xr_rm_teleop/test/test_placo_transforms.py`
|
||
- Modify: `xr_rm_teleop/xr_rm_teleop/fun_peripheral.py`
|
||
|
||
- [ ] **Step 1: 从用户上传包复制且只复制模型资源**
|
||
|
||
从工作空间根目录执行：
|
||
|
||
```bash
|
||
cd /home/robot/WS_xr
|
||
model_tmp_dir=$(mktemp -d /tmp/rm75-model.XXXXXX)
|
||
unzip -q /home/robot/XRoboToolkit/Resource/IK_qp-class_version.zip \
|
||
'ik_qp/kine_ctrl/urdf_rm75/RM75-B.urdf' \
|
||
'ik_qp/kine_ctrl/urdf_rm75/meshes/*' \
|
||
-d "$model_tmp_dir"
|
||
mkdir -p src/xr_rm_teleop/models/rm75/meshes
|
||
cp "$model_tmp_dir/ik_qp/kine_ctrl/urdf_rm75/RM75-B.urdf" \
|
||
src/xr_rm_teleop/models/rm75/RM75-B.urdf
|
||
cp "$model_tmp_dir/ik_qp/kine_ctrl/urdf_rm75/meshes/"*.STL \
|
||
src/xr_rm_teleop/models/rm75/meshes/
|
||
```
|
||
|
||
检查只导入一个 URDF 和八个网格，没有导入上传包的 Pinocchio/OSQP 代码：
|
||
|
||
```bash
|
||
find src/xr_rm_teleop/models/rm75 -type f | sort
|
||
```
|
||
|
||
Expected: `RM75-B.urdf`、`base_link.STL`、`link_1.STL` 至 `link_7.STL`，共 9 个文件。
|
||
|
||
- [ ] **Step 2: 先写 TCP/法兰变换失败测试**
|
||
|
||
创建 `src/xr_rm_teleop/test/test_placo_transforms.py`：
|
||
|
||
```python
|
||
import math
|
||
|
||
import numpy as np
|
||
import pytest
|
||
|
||
from xr_rm_teleop.placo_ik_solver import (
|
||
_arm_pose_to_transform,
|
||
_tool_pose_to_transform,
|
||
_transform_to_arm_pose,
|
||
)
|
||
from xr_rm_teleop.realman_adapter import ArmPose
|
||
|
||
|
||
def test_tool_offset_rotates_with_flange_and_roundtrips() -> None:
|
||
flange_pose = ArmPose(0.30, -0.10, 0.20, 0.0, math.pi / 2.0, 0.0)
|
||
tool_pose = [0.0, 0.0, 0.19, 0.0, 0.0, 0.0, 1.0]
|
||
|
||
base_to_flange = _arm_pose_to_transform(flange_pose)
|
||
flange_to_tool = _tool_pose_to_transform(tool_pose)
|
||
base_to_tool = base_to_flange @ flange_to_tool
|
||
recovered_flange = base_to_tool @ np.linalg.inv(flange_to_tool)
|
||
|
||
assert base_to_tool[:3, 3] == pytest.approx([0.49, -0.10, 0.20])
|
||
assert recovered_flange == pytest.approx(base_to_flange)
|
||
|
||
|
||
def test_transform_to_arm_pose_roundtrip() -> None:
|
||
expected = ArmPose(0.25, -0.30, 0.40, 0.20, -0.30, 0.40)
|
||
|
||
actual = _transform_to_arm_pose(_arm_pose_to_transform(expected))
|
||
|
||
assert actual.xyz() == pytest.approx(expected.xyz())
|
||
assert actual.rpy() == pytest.approx(expected.rpy())
|
||
```
|
||
|
||
- [ ] **Step 3: 运行测试并确认它因模块尚不存在而失败**
|
||
|
||
```bash
|
||
cd /home/robot/WS_xr
|
||
source /opt/ros/humble/setup.bash
|
||
PYTHONPATH=src/xr_rm_teleop pytest -q \
|
||
src/xr_rm_teleop/test/test_placo_transforms.py
|
||
```
|
||
|
||
Expected: FAIL，错误包含 `No module named 'xr_rm_teleop.placo_ik_solver'`。
|
||
|
||
- [ ] **Step 4: 给外设配置增加所选工具的单一来源**
|
||
|
||
在 `PeripheralConfig` 中加入：
|
||
|
||
```python
|
||
@property
|
||
def tool_name(self) -> str:
|
||
return list(self.tools_in_ee)[self.scissorgripper]
|
||
|
||
@property
|
||
def tool_pose(self) -> list[float]:
|
||
return list(self.tools_in_ee[self.tool_name][0])
|
||
```
|
||
|
||
把 `RealManAdapter.configure_peripheral()` 中原来的工具名选择替换为：
|
||
|
||
```python
|
||
tool_name = config.tool_name
|
||
```
|
||
|
||
工具位姿和控制器工具帧由同一个 `PeripheralConfig` 提供，避免左右臂工具索引在两个位置各自解析。
|
||
|
||
- [ ] **Step 5: 实现不在系统 Python 导入 Placo 的变换和求解器骨架**
|
||
|
||
创建 `src/xr_rm_teleop/xr_rm_teleop/placo_ik_solver.py`，模块顶层只导入标准库和 NumPy；`placo` 必须在构造函数内导入：
|
||
|
||
```python
|
||
"""RM75 的 Placo 0.9.4 单步 QP 逆解。"""
|
||
|
||
from __future__ import annotations
|
||
|
||
import math
|
||
from importlib.metadata import PackageNotFoundError, version
|
||
from pathlib import Path
|
||
|
||
import numpy as np
|
||
|
||
from .realman_adapter import ArmPose
|
||
|
||
|
||
EXPECTED_PLACO_VERSION = "0.9.4"
|
||
RM75_JOINT_NAMES = [f"joint_{index}" for index in range(1, 8)]
|
||
RM75_Q_SLICE = slice(7, 14)
|
||
|
||
|
||
def _rpy_to_rotation(roll: float, pitch: float, yaw: float) -> np.ndarray:
|
||
cr, sr = math.cos(roll), math.sin(roll)
|
||
cp, sp = math.cos(pitch), math.sin(pitch)
|
||
cy, sy = math.cos(yaw), math.sin(yaw)
|
||
return np.array(
|
||
[
|
||
[cy * cp, cy * sp * sr - sy * cr, cy * sp * cr + sy * sr],
|
||
[sy * cp, sy * sp * sr + cy * cr, sy * sp * cr - cy * sr],
|
||
[-sp, cp * sr, cp * cr],
|
||
],
|
||
dtype=float,
|
||
)
|
||
|
||
|
||
def _rotation_to_rpy(rotation: np.ndarray) -> tuple[float, float, float]:
|
||
pitch = math.asin(-float(np.clip(rotation[2, 0], -1.0, 1.0)))
|
||
if abs(math.cos(pitch)) > 1e-9:
|
||
roll = math.atan2(float(rotation[2, 1]), float(rotation[2, 2]))
|
||
yaw = math.atan2(float(rotation[1, 0]), float(rotation[0, 0]))
|
||
else:
|
||
roll = math.atan2(-float(rotation[1, 2]), float(rotation[1, 1]))
|
||
yaw = 0.0
|
||
return roll, pitch, yaw
|
||
|
||
|
||
def _arm_pose_to_transform(pose: ArmPose) -> np.ndarray:
|
||
transform = np.eye(4)
|
||
transform[:3, :3] = _rpy_to_rotation(pose.rx, pose.ry, pose.rz)
|
||
transform[:3, 3] = pose.xyz()
|
||
return transform
|
||
|
||
|
||
def _tool_pose_to_transform(tool_pose: list[float]) -> np.ndarray:
|
||
values = np.asarray(tool_pose, dtype=float)
|
||
if values.shape != (7,) or not np.isfinite(values).all():
|
||
raise ValueError("tool pose must contain 7 finite values")
|
||
x, y, z, qx, qy, qz, qw = values
|
||
norm = math.sqrt(qx * qx + qy * qy + qz * qz + qw * qw)
|
||
if norm <= 1e-9:
|
||
raise ValueError("tool quaternion norm must be positive")
|
||
qx, qy, qz, qw = qx / norm, qy / norm, qz / norm, qw / norm
|
||
transform = np.eye(4)
|
||
transform[:3, :3] = np.array(
|
||
[
|
||
[1 - 2 * (qy * qy + qz * qz), 2 * (qx * qy - qz * qw), 2 * (qx * qz + qy * qw)],
|
||
[2 * (qx * qy + qz * qw), 1 - 2 * (qx * qx + qz * qz), 2 * (qy * qz - qx * qw)],
|
||
[2 * (qx * qz - qy * qw), 2 * (qy * qz + qx * qw), 1 - 2 * (qx * qx + qy * qy)],
|
||
]
|
||
)
|
||
transform[:3, 3] = [x, y, z]
|
||
return transform
|
||
|
||
|
||
def _transform_to_arm_pose(transform: np.ndarray) -> ArmPose:
|
||
roll, pitch, yaw = _rotation_to_rpy(transform[:3, :3])
|
||
return ArmPose(
|
||
float(transform[0, 3]),
|
||
float(transform[1, 3]),
|
||
float(transform[2, 3]),
|
||
roll,
|
||
pitch,
|
||
yaw,
|
||
)
|
||
```
|
||
|
||
- [ ] **Step 6: 运行纯变换测试**
|
||
|
||
```bash
|
||
cd /home/robot/WS_xr
|
||
source /opt/ros/humble/setup.bash
|
||
PYTHONPATH=src/xr_rm_teleop pytest -q \
|
||
src/xr_rm_teleop/test/test_placo_transforms.py
|
||
```
|
||
|
||
Expected: `2 passed`。系统 Python 不需要也不应能导入 Placo。
|
||
|
||
- [ ] **Step 7: 检查 Task 1 变更范围**
|
||
|
||
```bash
|
||
git diff -- \
|
||
src/xr_rm_teleop/xr_rm_teleop/fun_peripheral.py \
|
||
src/xr_rm_teleop/xr_rm_teleop/placo_ik_solver.py \
|
||
src/xr_rm_teleop/test/test_placo_transforms.py
|
||
git status --short src/xr_rm_teleop/models/rm75
|
||
```
|
||
|
||
Expected: 只包含工具选择属性、变换模块、测试和模型资源。不要提交；等待用户明确授权。
|
||
|
||
### Task 2: 实现 Placo 0.9.4 单步求解
|
||
|
||
**Files:**
|
||
|
||
- Modify: `xr_rm_teleop/xr_rm_teleop/placo_ik_solver.py`
|
||
- Create: `xr_rm_teleop/test/placo_ik_smoke.py`
|
||
|
||
- [ ] **Step 1: 写 XR Python 左右臂数值验收脚本**
|
||
|
||
创建 `src/xr_rm_teleop/test/placo_ik_smoke.py`：
|
||
|
||
```python
|
||
from __future__ import annotations
|
||
|
||
import math
|
||
import sys
|
||
import time
|
||
from pathlib import Path
|
||
|
||
import numpy as np
|
||
|
||
from xr_rm_teleop.placo_ik_solver import PlacoIkSolver
|
||
from xr_rm_teleop.realman_adapter import ArmPose
|
||
|
||
|
||
CASES = {
|
||
"left": (
|
||
[-79.55, -9.99, 71.01, 101.45, 95.07, -84.47, -74.52],
|
||
[0.0, 0.0, 0.19, 0.0, 0.0, 0.0, 1.0],
|
||
),
|
||
"right": (
|
||
[-90.14, 3.76, -86.89, 87.89, -96.53, -79.62, -90.04],
|
||
[0.0, 0.0, 0.16, 0.0, 0.0, 0.0, 1.0],
|
||
),
|
||
}
|
||
|
||
|
||
def angle_error(actual: list[float], target: list[float]) -> float:
|
||
deltas = [
|
||
math.atan2(math.sin(a - b), math.cos(a - b))
|
||
for a, b in zip(actual, target)
|
||
]
|
||
return math.sqrt(sum(value * value for value in deltas))
|
||
|
||
|
||
def main() -> None:
|
||
urdf_path = Path(sys.argv[1]).resolve()
|
||
for arm, (joint_degrees, tool_pose) in CASES.items():
|
||
solver = PlacoIkSolver(str(urdf_path), tool_pose, 1.0 / 90.0)
|
||
joints = np.deg2rad(joint_degrees).tolist()
|
||
current = solver.update_joint_state(joints)
|
||
target = ArmPose(
|
||
current.x + 0.01,
|
||
current.y,
|
||
current.z,
|
||
current.rx,
|
||
current.ry,
|
||
current.rz + 0.05,
|
||
)
|
||
|
||
solve_durations = []
|
||
for _ in range(45):
|
||
solver.update_joint_state(joints)
|
||
started_at = time.perf_counter()
|
||
joints = solver.solve(target)
|
||
solve_durations.append(time.perf_counter() - started_at)
|
||
|
||
actual = solver.update_joint_state(joints)
|
||
position_error = np.linalg.norm(
|
||
np.asarray(actual.xyz()) - np.asarray(target.xyz())
|
||
)
|
||
orientation_error = angle_error(actual.rpy(), target.rpy())
|
||
assert len(joints) == 7
|
||
assert np.isfinite(joints).all()
|
||
assert np.allclose(
|
||
solver.base_configuration,
|
||
[0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0],
|
||
)
|
||
assert position_error <= 0.005
|
||
assert orientation_error <= math.radians(2.0)
|
||
print(
|
||
f"{arm}: position_error={position_error:.6f}m, "
|
||
f"orientation_error={math.degrees(orientation_error):.3f}deg, "
|
||
f"solve_avg={1000.0 * np.mean(solve_durations):.3f}ms, "
|
||
f"solve_max={1000.0 * max(solve_durations):.3f}ms, "
|
||
f"solve_overruns={sum(value > 1.0 / 90.0 for value in solve_durations)}"
|
||
)
|
||
|
||
|
||
if __name__ == "__main__":
|
||
main()
|
||
```
|
||
|
||
该脚本不是 pytest 收集文件；它由已有 XR Python 直接运行，全部断言只使用
|
||
Python 和 NumPy，不向该 Conda 环境安装 pytest。
|
||
|
||
- [ ] **Step 2: 运行脚本并确认求解器类尚不存在**
|
||
|
||
```bash
|
||
cd /home/robot/WS_xr
|
||
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/rm75/RM75-B.urdf
|
||
```
|
||
|
||
Expected: FAIL，错误包含 `cannot import name 'PlacoIkSolver'`。
|
||
|
||
- [ ] **Step 3: 实现固定版本、固定基座和 q[7:14] 映射**
|
||
|
||
在 `placo_ik_solver.py` 追加：
|
||
|
||
```python
|
||
class PlacoIkSolver:
|
||
def __init__(
|
||
self,
|
||
urdf_path: str,
|
||
tool_pose: list[float],
|
||
dt: float,
|
||
) -> None:
|
||
if dt <= 0.0:
|
||
raise ValueError("dt must be positive")
|
||
try:
|
||
installed_version = version("placo")
|
||
import placo
|
||
except (ImportError, PackageNotFoundError) as exc:
|
||
raise RuntimeError(
|
||
"Placo 0.9.4 must come from "
|
||
"/home/robot/miniconda3/envs/xr"
|
||
) from exc
|
||
if installed_version != EXPECTED_PLACO_VERSION:
|
||
raise RuntimeError(
|
||
f"Placo {EXPECTED_PLACO_VERSION} is required, got {installed_version}"
|
||
)
|
||
|
||
model_path = Path(urdf_path).expanduser().resolve()
|
||
if not model_path.is_file():
|
||
raise FileNotFoundError(f"RM75 URDF not found: {model_path}")
|
||
|
||
self._dt = dt
|
||
self._robot = placo.RobotWrapper(str(model_path))
|
||
if self._robot.state.q.shape != (14,):
|
||
raise RuntimeError(
|
||
f"expected Placo q shape (14,), got {self._robot.state.q.shape}"
|
||
)
|
||
if list(self._robot.joint_names()) != RM75_JOINT_NAMES:
|
||
raise RuntimeError(
|
||
f"unexpected RM75 joint order: {list(self._robot.joint_names())}"
|
||
)
|
||
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]
|
||
)
|
||
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]
|
||
)
|
||
self._tool_transform = _tool_pose_to_transform(tool_pose)
|
||
self._tool_inverse = np.linalg.inv(self._tool_transform)
|
||
self._actual_joints: np.ndarray | None = None
|
||
|
||
self._solver = placo.KinematicsSolver(self._robot)
|
||
self._solver.dt = dt
|
||
self._solver.mask_fbase(True)
|
||
self._solver.enable_velocity_limits(True)
|
||
self._frame_task = self._solver.add_frame_task("link_7", np.eye(4))
|
||
self._frame_task.configure("rm75_frame", "soft", 1.0)
|
||
manipulability = self._solver.add_manipulability_task(
|
||
"link_7", "both", 1.0
|
||
)
|
||
manipulability.configure("rm75_manipulability", "soft", 5e-2)
|
||
self._solver.add_kinetic_energy_regularization_task(1e-6)
|
||
|
||
@property
|
||
def base_configuration(self) -> list[float]:
|
||
return self._robot.state.q[:7].tolist()
|
||
|
||
def update_joint_state(self, joints: list[float]) -> ArmPose:
|
||
values = np.asarray(joints, dtype=float)
|
||
if values.shape != (7,) or not np.isfinite(values).all():
|
||
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.update_kinematics()
|
||
base_to_flange = self._robot.get_T_world_frame("link_7")
|
||
if is_first_feedback:
|
||
self._frame_task.T_world_frame = base_to_flange.copy()
|
||
base_to_tool = base_to_flange @ self._tool_transform
|
||
return _transform_to_arm_pose(base_to_tool)
|
||
|
||
def solve(self, target_tool_pose: ArmPose) -> list[float]:
|
||
if self._actual_joints is None:
|
||
raise RuntimeError("joint state must be initialized before QP solve")
|
||
self._frame_task.T_world_frame = (
|
||
_arm_pose_to_transform(target_tool_pose) @ self._tool_inverse
|
||
)
|
||
self._solver.solve(True)
|
||
result = np.asarray(
|
||
self._robot.state.q[RM75_Q_SLICE],
|
||
dtype=float,
|
||
).copy()
|
||
self._validate_result(result)
|
||
return result.tolist()
|
||
|
||
def _validate_result(self, result: np.ndarray) -> None:
|
||
if result.shape != (7,) or not np.isfinite(result).all():
|
||
raise ValueError("QP result must contain 7 finite values")
|
||
lower = self._joint_limits[:, 0]
|
||
upper = self._joint_limits[:, 1]
|
||
if np.any(result < lower - 1e-9) or np.any(result > upper + 1e-9):
|
||
raise ValueError("QP result violates RM75 joint position limits")
|
||
max_step = self._velocity_limits * self._dt + 1e-9
|
||
if np.any(np.abs(result - self._actual_joints) > max_step):
|
||
raise ValueError("QP result violates RM75 one-cycle velocity limits")
|
||
```
|
||
|
||
不要调用 neutral 全零求解。构造函数只建立任务；第一次 `solve()` 必须发生在 `update_joint_state()` 之后。
|
||
|
||
- [ ] **Step 4: 增加求解结果校验测试**
|
||
|
||
在 `test_placo_transforms.py` 的 solver import 中加入 `PlacoIkSolver`，并追加：
|
||
|
||
```python
|
||
def test_qp_result_rejects_nan_position_and_velocity_violations() -> None:
|
||
solver = object.__new__(PlacoIkSolver)
|
||
solver._joint_limits = np.asarray([[-1.0, 1.0]] * 7)
|
||
solver._velocity_limits = np.ones(7)
|
||
solver._dt = 0.1
|
||
solver._actual_joints = np.zeros(7)
|
||
|
||
with pytest.raises(ValueError, match="finite"):
|
||
solver._validate_result(np.full(7, np.nan))
|
||
with pytest.raises(ValueError, match="position"):
|
||
solver._validate_result(np.full(7, 2.0))
|
||
with pytest.raises(ValueError, match="velocity"):
|
||
solver._validate_result(np.full(7, 0.2))
|
||
```
|
||
|
||
运行：
|
||
|
||
```bash
|
||
cd /home/robot/WS_xr
|
||
source /opt/ros/humble/setup.bash
|
||
PYTHONPATH=src/xr_rm_teleop pytest -q \
|
||
src/xr_rm_teleop/test/test_placo_transforms.py
|
||
```
|
||
|
||
Expected: `3 passed`，且系统 Python 没有导入 Placo。
|
||
|
||
- [ ] **Step 5: 运行左右臂 45 周期 QP 验收**
|
||
|
||
```bash
|
||
cd /home/robot/WS_xr
|
||
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/rm75/RM75-B.urdf
|
||
```
|
||
|
||
Expected:
|
||
|
||
```text
|
||
left: position_error=<不大于0.005>m, orientation_error=<不大于2.0>deg
|
||
right: position_error=<不大于0.005>m, orientation_error=<不大于2.0>deg
|
||
```
|
||
|
||
允许 Placo 打印 neutral 自碰撞警告；不得出现 QP `NaN`，基座必须保持
|
||
`[0, 0, 0, 0, 0, 0, 1]`。
|
||
|
||
- [ ] **Step 6: 检查 Task 2 变更，不提交**
|
||
|
||
```bash
|
||
git diff -- \
|
||
src/xr_rm_teleop/xr_rm_teleop/placo_ik_solver.py \
|
||
src/xr_rm_teleop/test/placo_ik_smoke.py
|
||
```
|
||
|
||
Expected: 一个求解器类和一个左右臂数值验收脚本，不包含 Placo 安装命令、可视化、OSQP 或 XRoboToolkit import。
|
||
|
||
### Task 3: 把 Adapter 改成单连接关节反馈与关节透传
|
||
|
||
**Files:**
|
||
|
||
- Modify: `xr_rm_teleop/xr_rm_teleop/realman_adapter.py`
|
||
- Modify: `xr_rm_teleop/test/test_initial_joint_pose.py`
|
||
- Modify: `xr_rm_teleop/test/test_orientation_control.py`
|
||
|
||
- [ ] **Step 1: 先写 SDK 关节边界失败测试**
|
||
|
||
在 `test_initial_joint_pose.py` 保留现有初始化 `rm_movej` 测试，并追加：
|
||
|
||
```python
|
||
import math
|
||
|
||
import pytest
|
||
|
||
from xr_rm_teleop.realman_adapter import MockRealManAdapter
|
||
|
||
|
||
def test_joint_feedback_is_cached_in_radians() -> None:
|
||
class FakeArm:
|
||
def rm_get_joint_degree(self):
|
||
return 0, [0.0, 10.0, -20.0, 30.0, -40.0, 50.0, -60.0]
|
||
|
||
adapter = RealManAdapter("127.0.0.1", 8080, 0, 0.01)
|
||
adapter._arm = FakeArm()
|
||
|
||
adapter._read_joint_state_once()
|
||
snapshot = adapter.get_latest_joint_state()
|
||
|
||
assert snapshot is not None
|
||
assert snapshot.positions == pytest.approx(
|
||
[math.radians(value) for value in [0, 10, -20, 30, -40, 50, -60]]
|
||
)
|
||
|
||
|
||
def test_joint_target_uses_movej_canfd_in_degrees() -> None:
|
||
class FakeArm:
|
||
def __init__(self) -> None:
|
||
self.calls = []
|
||
|
||
def rm_movej_canfd(self, *args):
|
||
self.calls.append(args)
|
||
return 0
|
||
|
||
adapter = RealManAdapter("127.0.0.1", 8080, 0, 0.01)
|
||
adapter._arm = FakeArm()
|
||
target = [math.radians(value) for value in [1, 2, 3, 4, 5, 6, 7]]
|
||
|
||
adapter.send_joint_target(target, follow=False)
|
||
|
||
assert adapter._arm.calls == [
|
||
([1.0, 2.0, 3.0, 4.0, 5.0, 6.0, 7.0], False, 0, 2, 0)
|
||
]
|
||
|
||
|
||
def test_mock_joint_feedback_is_available_without_vendor_sdk() -> None:
|
||
adapter = MockRealManAdapter([1.0, 2.0, 3.0, 4.0, 5.0, 6.0, 7.0])
|
||
|
||
adapter.connect()
|
||
snapshot = adapter.get_latest_joint_state()
|
||
|
||
assert snapshot is not None
|
||
assert snapshot.positions == pytest.approx(
|
||
[math.radians(value) for value in [1, 2, 3, 4, 5, 6, 7]]
|
||
)
|
||
```
|
||
|
||
从 `test_orientation_control.py` 删除
|
||
`test_mock_adapter_uses_shortest_angular_velocity()`，并删除不再使用的
|
||
`MockRealManAdapter` import。其余姿态测试不改。
|
||
|
||
- [ ] **Step 2: 运行测试并确认旧 Adapter API 不满足测试**
|
||
|
||
```bash
|
||
cd /home/robot/WS_xr
|
||
source /opt/ros/humble/setup.bash
|
||
PYTHONPATH=src/xr_rm_teleop pytest -q \
|
||
src/xr_rm_teleop/test/test_initial_joint_pose.py
|
||
```
|
||
|
||
Expected: FAIL，至少包含 `get_latest_joint_state`、`send_joint_target` 或构造参数不匹配。
|
||
|
||
- [ ] **Step 3: 用线程安全反馈快照替换笛卡尔 mock**
|
||
|
||
在 `realman_adapter.py`：
|
||
|
||
```python
|
||
import threading
|
||
import time
|
||
```
|
||
|
||
加入：
|
||
|
||
```python
|
||
@dataclass(frozen=True)
|
||
class JointStateSnapshot:
|
||
positions: list[float]
|
||
received_at: float
|
||
```
|
||
|
||
将 `MockRealManAdapter` 替换成：
|
||
|
||
```python
|
||
class MockRealManAdapter:
|
||
def __init__(self, initial_joint_degrees: list[float]) -> None:
|
||
if len(initial_joint_degrees) != 7:
|
||
raise ValueError("initial joint pose must contain 7 values")
|
||
self._joint_positions = [
|
||
math.radians(value) for value in initial_joint_degrees
|
||
]
|
||
self.last_joint_target: list[float] | None = None
|
||
self.last_tool_open: bool | None = None
|
||
|
||
def connect(self) -> None:
|
||
return
|
||
|
||
def get_latest_joint_state(self) -> JointStateSnapshot:
|
||
return JointStateSnapshot(
|
||
list(self._joint_positions),
|
||
time.monotonic(),
|
||
)
|
||
|
||
def send_joint_target(self, joints: list[float], follow: bool) -> None:
|
||
del follow
|
||
if len(joints) != 7 or not all(math.isfinite(value) for value in joints):
|
||
raise ValueError("joint target must contain 7 finite values")
|
||
self._joint_positions = list(joints)
|
||
self.last_joint_target = list(joints)
|
||
|
||
def stop(self) -> None:
|
||
return
|
||
|
||
def close(self) -> None:
|
||
self.stop()
|
||
|
||
def configure_peripheral(self, config, peripheral_arm: str) -> None:
|
||
del config, peripheral_arm
|
||
|
||
def set_tool_enabled(self, open_tool: bool) -> None:
|
||
self.last_tool_open = open_tool
|
||
```
|
||
|
||
删除 Adapter 内不再使用的 `_angle_delta()` 和笛卡尔发送/读取方法；保留
|
||
`ArmPose`，因为遥操作和 Placo 仍用它表示工具 TCP。`Number` 继续用于 SDK
|
||
反馈数值校验。
|
||
|
||
- [ ] **Step 4: 在 RealManAdapter 的同一连接上加入反馈缓存**
|
||
|
||
把构造参数中的 `frame_type` 替换为：
|
||
|
||
```python
|
||
feedback_period: float,
|
||
```
|
||
|
||
在构造函数末尾加入：
|
||
|
||
```python
|
||
if feedback_period <= 0.0:
|
||
raise ValueError("feedback_period must be positive")
|
||
self._feedback_period = feedback_period
|
||
self._joint_state_lock = threading.Lock()
|
||
self._latest_joint_state: JointStateSnapshot | None = None
|
||
self._feedback_stop = threading.Event()
|
||
self._feedback_thread: threading.Thread | None = None
|
||
self._feedback_fault_logged = False
|
||
```
|
||
|
||
连接成功日志中的命令改成 `rm_movej_canfd`，在安全配置和可选初始移动之后启动反馈线程：
|
||
|
||
```python
|
||
self._feedback_stop.clear()
|
||
self._feedback_thread = threading.Thread(
|
||
target=self._feedback_loop,
|
||
name=f"rm75_feedback_{self._robot_ip}",
|
||
daemon=True,
|
||
)
|
||
self._feedback_thread.start()
|
||
```
|
||
|
||
加入以下方法：
|
||
|
||
```python
|
||
def _feedback_loop(self) -> None:
|
||
while not self._feedback_stop.is_set():
|
||
try:
|
||
self._read_joint_state_once()
|
||
self._feedback_fault_logged = False
|
||
except Exception as exc:
|
||
if not self._feedback_fault_logged:
|
||
self._log_warn(f"RealMan 关节反馈读取失败：{exc}")
|
||
self._feedback_fault_logged = True
|
||
self._feedback_stop.wait(self._feedback_period)
|
||
|
||
def _read_joint_state_once(self) -> None:
|
||
self._require_arm()
|
||
result = self._arm.rm_get_joint_degree()
|
||
self._check_return(result, "rm_get_joint_degree")
|
||
if not isinstance(result, tuple) or len(result) < 2:
|
||
raise RuntimeError(f"rm_get_joint_degree 返回格式错误：{result!r}")
|
||
degrees = result[1]
|
||
if (
|
||
not isinstance(degrees, (list, tuple))
|
||
or len(degrees) != 7
|
||
or not all(isinstance(value, Number) for value in degrees)
|
||
):
|
||
raise RuntimeError(f"RM75 关节反馈必须包含 7 个数值：{degrees!r}")
|
||
positions = [math.radians(float(value)) for value in degrees]
|
||
if not all(math.isfinite(value) for value in positions):
|
||
raise RuntimeError("RM75 关节反馈包含 NaN/Inf")
|
||
snapshot = JointStateSnapshot(positions, time.monotonic())
|
||
with self._joint_state_lock:
|
||
self._latest_joint_state = snapshot
|
||
|
||
def get_latest_joint_state(self) -> JointStateSnapshot | None:
|
||
with self._joint_state_lock:
|
||
if self._latest_joint_state is None:
|
||
return None
|
||
return JointStateSnapshot(
|
||
list(self._latest_joint_state.positions),
|
||
self._latest_joint_state.received_at,
|
||
)
|
||
|
||
def send_joint_target(self, joints: list[float], follow: bool) -> None:
|
||
self._require_arm()
|
||
if len(joints) != 7 or not all(math.isfinite(value) for value in joints):
|
||
raise ValueError("joint target must contain 7 finite values")
|
||
degrees = [math.degrees(value) for value in joints]
|
||
ret = self._arm.rm_movej_canfd(
|
||
degrees,
|
||
follow,
|
||
0,
|
||
self._canfd_trajectory_mode,
|
||
self._canfd_radio,
|
||
)
|
||
self._check_return(ret, "rm_movej_canfd")
|
||
```
|
||
|
||
`Number` 仍用于 SDK 反馈校验，因此保留 `from numbers import Number`。
|
||
|
||
- [ ] **Step 5: 让工具配置复用已加载的 PeripheralConfig**
|
||
|
||
把真机和 mock 的签名统一为：
|
||
|
||
```python
|
||
def configure_peripheral(self, config, peripheral_arm: str) -> None:
|
||
```
|
||
|
||
真机实现不再读取 YAML，直接使用：
|
||
|
||
```python
|
||
self._scissorgripper = config.scissorgripper
|
||
tool_name = config.tool_name
|
||
peripheral_cfg(
|
||
self._arm,
|
||
config.scissorgripper,
|
||
config.tools_in_ee,
|
||
set_initial_tool_state=config.set_initial_tool_state,
|
||
)
|
||
```
|
||
|
||
这样 Placo 与控制器工具帧使用同一次 YAML 解析结果。
|
||
|
||
- [ ] **Step 6: 安全停止后结束反馈线程，再删除唯一连接**
|
||
|
||
将 `close()` 改为：
|
||
|
||
```python
|
||
def close(self) -> None:
|
||
if self._arm is None:
|
||
return
|
||
self.stop()
|
||
self._feedback_stop.set()
|
||
if self._feedback_thread is not None:
|
||
self._feedback_thread.join(timeout=3.0)
|
||
if self._feedback_thread.is_alive():
|
||
self._log_warn("RealMan 关节反馈线程未在 3 秒内退出。")
|
||
self._feedback_thread = None
|
||
try:
|
||
self._arm.rm_delete_robot_arm()
|
||
finally:
|
||
self._arm = None
|
||
```
|
||
|
||
不得创建第二个 `RoboticArm` 或再次调用 `rm_create_robot_arm`。
|
||
|
||
- [ ] **Step 7: 运行 Adapter 与姿态回归测试**
|
||
|
||
```bash
|
||
cd /home/robot/WS_xr
|
||
source /opt/ros/humble/setup.bash
|
||
PYTHONPATH=src/xr_rm_teleop pytest -q \
|
||
src/xr_rm_teleop/test/test_initial_joint_pose.py \
|
||
src/xr_rm_teleop/test/test_orientation_control.py
|
||
```
|
||
|
||
Expected: 全部 PASS；mock 测试过程中不需要 `Robotic_Arm`。
|
||
|
||
- [ ] **Step 8: 静态确认只有一个连接创建点**
|
||
|
||
```bash
|
||
rg -n "rm_create_robot_arm" src/xr_rm_teleop
|
||
```
|
||
|
||
Expected: 只有 `realman_adapter.py` 的 `connect()` 中一处调用。
|
||
|
||
### Task 4: 在单臂节点中加入首帧反馈门控和 XR 失败策略
|
||
|
||
**Files:**
|
||
|
||
- Create: `xr_rm_teleop/test/test_joint_control.py`
|
||
- Modify: `xr_rm_teleop/xr_rm_teleop/single_arm_velocity_teleop.py`
|
||
|
||
- [ ] **Step 1: 写首帧门控和 last-known-good 失败测试**
|
||
|
||
创建 `src/xr_rm_teleop/test/test_joint_control.py`：
|
||
|
||
```python
|
||
import time
|
||
from types import SimpleNamespace
|
||
|
||
import pytest
|
||
|
||
from xr_rm_teleop.realman_adapter import ArmPose, JointStateSnapshot
|
||
from xr_rm_teleop.single_arm_velocity_teleop import SingleArmVelocityTeleop
|
||
|
||
|
||
class FakeLogger:
|
||
def warn(self, *args, **kwargs):
|
||
del args, kwargs
|
||
|
||
|
||
def test_missing_or_stale_feedback_does_not_enable_qp() -> None:
|
||
teleop = object.__new__(SingleArmVelocityTeleop)
|
||
teleop._command_timeout_sec = 0.12
|
||
teleop._adapter = SimpleNamespace(get_latest_joint_state=lambda: None)
|
||
|
||
assert teleop._fresh_joint_state() is None
|
||
|
||
teleop._adapter = SimpleNamespace(
|
||
get_latest_joint_state=lambda: JointStateSnapshot(
|
||
[0.0] * 7,
|
||
time.monotonic() - 1.0,
|
||
)
|
||
)
|
||
assert teleop._fresh_joint_state() is None
|
||
|
||
|
||
def test_stale_feedback_stops_before_qp_solve() -> None:
|
||
class SolverThatMustNotRun:
|
||
def solve(self, target):
|
||
del target
|
||
raise AssertionError("QP must not run with stale feedback")
|
||
|
||
stopped = []
|
||
teleop = object.__new__(SingleArmVelocityTeleop)
|
||
teleop._adapter = SimpleNamespace(
|
||
get_latest_joint_state=lambda: JointStateSnapshot(
|
||
[0.0] * 7,
|
||
time.monotonic() - 1.0,
|
||
)
|
||
)
|
||
teleop._ik_solver = SolverThatMustNotRun()
|
||
teleop._command_timeout_sec = 0.12
|
||
teleop._joint_feedback_ready = True
|
||
teleop._arm_name = "right_rm75"
|
||
teleop.get_clock = lambda: SimpleNamespace(now=lambda: object())
|
||
teleop.get_logger = lambda: FakeLogger()
|
||
teleop._safe_stop = lambda reset_active: stopped.append(reset_active)
|
||
|
||
teleop._control_tick()
|
||
|
||
assert stopped == [True]
|
||
|
||
|
||
def test_first_feedback_initializes_last_valid_target_without_solving() -> None:
|
||
class FakeSolver:
|
||
def __init__(self) -> None:
|
||
self.solve_calls = 0
|
||
|
||
def update_joint_state(self, joints):
|
||
assert joints == [0.1] * 7
|
||
return ArmPose(0.3, 0.0, 0.2)
|
||
|
||
def solve(self, target):
|
||
del target
|
||
self.solve_calls += 1
|
||
return [0.2] * 7
|
||
|
||
teleop = object.__new__(SingleArmVelocityTeleop)
|
||
teleop._ik_solver = FakeSolver()
|
||
teleop._active = False
|
||
teleop._last_valid_joint_target = None
|
||
teleop._last_current_pose = None
|
||
|
||
pose = teleop._sync_joint_feedback(
|
||
JointStateSnapshot([0.1] * 7, time.monotonic())
|
||
)
|
||
|
||
assert pose == ArmPose(0.3, 0.0, 0.2)
|
||
assert teleop._last_valid_joint_target == [0.1] * 7
|
||
assert teleop._ik_solver.solve_calls == 0
|
||
|
||
|
||
def test_qp_failure_returns_last_known_good_target() -> None:
|
||
class FailingSolver:
|
||
def solve(self, target):
|
||
del target
|
||
raise RuntimeError("NaN in QP solution")
|
||
|
||
teleop = object.__new__(SingleArmVelocityTeleop)
|
||
teleop._ik_solver = FailingSolver()
|
||
teleop._last_valid_joint_target = [0.1] * 7
|
||
teleop._arm_name = "right_rm75"
|
||
teleop.get_logger = lambda: FakeLogger()
|
||
|
||
target = teleop._solve_joint_target(ArmPose(0.3, 0.0, 0.2))
|
||
|
||
assert target == pytest.approx([0.1] * 7)
|
||
assert teleop._last_valid_joint_target == pytest.approx([0.1] * 7)
|
||
|
||
|
||
def test_qp_success_updates_last_known_good_target() -> None:
|
||
class SuccessfulSolver:
|
||
def solve(self, target):
|
||
del target
|
||
return [0.2] * 7
|
||
|
||
teleop = object.__new__(SingleArmVelocityTeleop)
|
||
teleop._ik_solver = SuccessfulSolver()
|
||
teleop._last_valid_joint_target = [0.1] * 7
|
||
teleop._arm_name = "left_rm75"
|
||
teleop.get_logger = lambda: FakeLogger()
|
||
|
||
target = teleop._solve_joint_target(ArmPose(0.3, 0.0, 0.2))
|
||
|
||
assert target == pytest.approx([0.2] * 7)
|
||
assert teleop._last_valid_joint_target == pytest.approx([0.2] * 7)
|
||
```
|
||
|
||
- [ ] **Step 2: 运行测试并确认节点尚无这些方法**
|
||
|
||
```bash
|
||
cd /home/robot/WS_xr
|
||
source /opt/ros/humble/setup.bash
|
||
PYTHONPATH=src/xr_rm_teleop pytest -q \
|
||
src/xr_rm_teleop/test/test_joint_control.py
|
||
```
|
||
|
||
Expected: FAIL，错误包含 `_fresh_joint_state`、`_sync_joint_feedback` 或 `_solve_joint_target` 不存在。
|
||
|
||
- [ ] **Step 3: 让节点启动时先解析工具并创建 Placo，再连接 Adapter**
|
||
|
||
在 import 中加入：
|
||
|
||
```python
|
||
import time
|
||
|
||
from .fun_peripheral import load_peripheral_config
|
||
from .placo_ik_solver import PlacoIkSolver
|
||
from .realman_adapter import (
|
||
ArmPose,
|
||
JointStateSnapshot,
|
||
MockRealManAdapter,
|
||
RealManAdapter,
|
||
)
|
||
```
|
||
|
||
声明新参数：
|
||
|
||
```python
|
||
self.declare_parameter("robot_urdf_path", "")
|
||
```
|
||
|
||
删除已失效的参数声明和成员读取：
|
||
|
||
```text
|
||
current_pose_poll_hz
|
||
mock_initial_pose
|
||
frame_type
|
||
```
|
||
|
||
初始化字段加入：
|
||
|
||
```python
|
||
self._last_valid_joint_target: list[float] | None = None
|
||
self._joint_feedback_ready = False
|
||
```
|
||
|
||
把 Adapter 创建顺序替换为：
|
||
|
||
```python
|
||
peripheral_arm = self._peripheral_arm_name()
|
||
config_file = str(self.get_parameter("peripheral_config_file").value)
|
||
self._peripheral_config = load_peripheral_config(
|
||
config_file,
|
||
peripheral_arm,
|
||
)
|
||
self._ik_solver = PlacoIkSolver(
|
||
str(self.get_parameter("robot_urdf_path").value),
|
||
self._peripheral_config.tool_pose,
|
||
self._dt,
|
||
)
|
||
self._adapter = self._make_adapter()
|
||
self._adapter.connect()
|
||
self._setup_tool_control()
|
||
```
|
||
|
||
这样 Placo 版本、URDF、关节映射和工具位姿在建立真机连接前完成校验。
|
||
|
||
- [ ] **Step 4: 让 mock 和真机 Adapter 都使用初始关节配置**
|
||
|
||
将 `_make_adapter()` 改为：
|
||
|
||
```python
|
||
def _make_adapter(self):
|
||
initial_joint_pose = self._float_list_parameter(
|
||
"initial_joint_pose",
|
||
7,
|
||
)
|
||
if self._bool_parameter("use_mock"):
|
||
return MockRealManAdapter(initial_joint_pose)
|
||
|
||
return RealManAdapter(
|
||
robot_ip=self.get_parameter("robot_ip").value,
|
||
robot_port=int(self.get_parameter("robot_port").value),
|
||
avoid_singularity=int(
|
||
self.get_parameter("avoid_singularity").value
|
||
),
|
||
feedback_period=self._dt,
|
||
logger=self.get_logger(),
|
||
configure_safety_limits=self._bool_parameter(
|
||
"configure_safety_limits"
|
||
),
|
||
max_line_speed=float(self.get_parameter("max_line_speed").value),
|
||
max_angular_speed=float(
|
||
self.get_parameter("max_angular_speed").value
|
||
),
|
||
max_line_acc=float(self.get_parameter("max_line_acc").value),
|
||
max_angular_acc=float(
|
||
self.get_parameter("max_angular_acc").value
|
||
),
|
||
joint_max_speed=float(
|
||
self.get_parameter("joint_max_speed").value
|
||
),
|
||
joint_max_acc=float(self.get_parameter("joint_max_acc").value),
|
||
move_to_initial_pose_on_connect=self._bool_parameter(
|
||
"move_to_initial_pose_on_connect"
|
||
),
|
||
initial_joint_pose=initial_joint_pose,
|
||
init_move_speed=int(self.get_parameter("init_move_speed").value),
|
||
canfd_trajectory_mode=int(
|
||
self.get_parameter("canfd_trajectory_mode").value
|
||
),
|
||
canfd_radio=int(self.get_parameter("canfd_radio").value),
|
||
)
|
||
```
|
||
|
||
- [ ] **Step 5: 工具坐标配置与工具开合解耦**
|
||
|
||
在 `_setup_tool_control()` 开头先执行控制器工具配置：
|
||
|
||
```python
|
||
peripheral_arm = self._peripheral_arm_name()
|
||
if self._bool_parameter("configure_peripheral_on_connect"):
|
||
self._adapter.configure_peripheral(
|
||
self._peripheral_config,
|
||
peripheral_arm,
|
||
)
|
||
```
|
||
|
||
然后再判断：
|
||
|
||
```python
|
||
if not self._enable_tool_control:
|
||
```
|
||
|
||
删除该函数中重复读取 `peripheral_config_file` 和重复调用旧
|
||
`configure_peripheral(config_file, peripheral_arm)` 的代码。
|
||
|
||
- [ ] **Step 6: 实现反馈时效、首帧初始化和 QP 失败回退**
|
||
|
||
在节点类中加入：
|
||
|
||
```python
|
||
def _fresh_joint_state(self) -> JointStateSnapshot | None:
|
||
snapshot = self._adapter.get_latest_joint_state()
|
||
if snapshot is None:
|
||
return None
|
||
age = time.monotonic() - snapshot.received_at
|
||
if age < 0.0 or age > self._command_timeout_sec:
|
||
return None
|
||
if (
|
||
len(snapshot.positions) != 7
|
||
or not all(math.isfinite(value) for value in snapshot.positions)
|
||
):
|
||
return None
|
||
return snapshot
|
||
|
||
def _sync_joint_feedback(
|
||
self,
|
||
snapshot: JointStateSnapshot,
|
||
) -> ArmPose:
|
||
current_pose = self._ik_solver.update_joint_state(
|
||
snapshot.positions
|
||
)
|
||
self._last_current_pose = current_pose
|
||
if not self._active or self._last_valid_joint_target is None:
|
||
self._last_valid_joint_target = list(snapshot.positions)
|
||
return current_pose
|
||
|
||
def _solve_joint_target(self, target_pose: ArmPose) -> list[float]:
|
||
if self._last_valid_joint_target is None:
|
||
raise RuntimeError("valid joint feedback has not been initialized")
|
||
try:
|
||
result = self._ik_solver.solve(target_pose)
|
||
except Exception as exc:
|
||
self.get_logger().warn(
|
||
f"{self._arm_name} QP 求解失败，保持上一组关节目标：{exc}",
|
||
throttle_duration_sec=1.0,
|
||
)
|
||
return list(self._last_valid_joint_target)
|
||
self._last_valid_joint_target = list(result)
|
||
return list(result)
|
||
|
||
def _send_joint_target(self, joints: list[float]) -> bool:
|
||
try:
|
||
self._adapter.send_joint_target(joints, self._follow)
|
||
except Exception as exc:
|
||
self.get_logger().error(
|
||
f"{self._arm_name} 发送关节透传命令失败：{exc}",
|
||
throttle_duration_sec=1.0,
|
||
)
|
||
self._safe_stop(reset_active=True)
|
||
return False
|
||
return True
|
||
```
|
||
|
||
- [ ] **Step 7: 在每个控制周期最前面执行反馈门控**
|
||
|
||
`_control_tick()` 在读取 XR 消息前加入：
|
||
|
||
```python
|
||
snapshot = self._fresh_joint_state()
|
||
if snapshot is None:
|
||
if self._joint_feedback_ready:
|
||
self.get_logger().warn(
|
||
f"{self._arm_name} 关节反馈缺失或过期，机械臂停止。",
|
||
throttle_duration_sec=1.0,
|
||
)
|
||
self._joint_feedback_ready = False
|
||
self._safe_stop(reset_active=True)
|
||
return
|
||
try:
|
||
current_pose = self._sync_joint_feedback(snapshot)
|
||
except Exception as exc:
|
||
self.get_logger().error(
|
||
f"{self._arm_name} 关节反馈同步到 Placo 失败：{exc}",
|
||
throttle_duration_sec=1.0,
|
||
)
|
||
self._joint_feedback_ready = False
|
||
self._safe_stop(reset_active=True)
|
||
return
|
||
if not self._joint_feedback_ready:
|
||
self.get_logger().info(
|
||
f"{self._arm_name} 已收到首帧有效关节反馈，QP 可以启用。"
|
||
)
|
||
self._joint_feedback_ready = True
|
||
```
|
||
|
||
当 grip 首次激活时，把当前 `current_pose` 传给 `_enter_active_control()`；该函数不再调用 Adapter 读取 TCP：
|
||
|
||
```python
|
||
self._enter_active_control(
|
||
controller_now,
|
||
controller_quat,
|
||
current_pose,
|
||
now,
|
||
)
|
||
```
|
||
|
||
函数签名改为：
|
||
|
||
```python
|
||
def _enter_active_control(
|
||
self,
|
||
controller_now: list[float],
|
||
controller_quat: tuple[float, float, float, float],
|
||
robot_pose: ArmPose,
|
||
now: Time,
|
||
) -> None:
|
||
```
|
||
|
||
删除该函数原来的 `try: robot_pose = self._read_current_pose_for_control(now)` 块。
|
||
|
||
- [ ] **Step 8: 用一步 QP 和关节透传替换笛卡尔下发**
|
||
|
||
删除 `_maybe_refresh_current_pose(now)` 调用。保留从 `raw_target_xyz` 到
|
||
`target_pose` 的全部死区、滤波、工作空间/圆柱、线速度和角速度限制。
|
||
|
||
把：
|
||
|
||
```python
|
||
if self._send_cartesian_target(target_pose):
|
||
```
|
||
|
||
替换为：
|
||
|
||
```python
|
||
joint_target = self._solve_joint_target(target_pose)
|
||
if self._send_joint_target(joint_target):
|
||
```
|
||
|
||
删除以下旧方法：
|
||
|
||
```text
|
||
_read_current_pose_for_control
|
||
_maybe_refresh_current_pose
|
||
_send_cartesian_target
|
||
```
|
||
|
||
每个有效且激活的控制周期只能有一次
|
||
`self._ik_solver.solve(target_pose)`。
|
||
|
||
- [ ] **Step 9: 运行节点行为和姿态回归测试**
|
||
|
||
```bash
|
||
cd /home/robot/WS_xr
|
||
source /opt/ros/humble/setup.bash
|
||
PYTHONPATH=src/xr_rm_teleop pytest -q \
|
||
src/xr_rm_teleop/test/test_joint_control.py \
|
||
src/xr_rm_teleop/test/test_orientation_control.py
|
||
```
|
||
|
||
Expected: 全部 PASS。
|
||
|
||
- [ ] **Step 10: 静态确认旧笛卡尔运动链路已移除**
|
||
|
||
```bash
|
||
rg -n "rm_movep_canfd|send_cartesian_target|get_current_pose" \
|
||
src/xr_rm_teleop/xr_rm_teleop
|
||
```
|
||
|
||
Expected: 遥操作包的运行代码中无匹配。
|
||
|
||
### Task 5: 固定 XR Python 启动并安装模型资源
|
||
|
||
**Files:**
|
||
|
||
- Modify: `xr_rm_teleop/setup.py`
|
||
- Modify: `xr_rm_bringup/launch/arm_debug.launch.py`
|
||
- Modify: `xr_rm_bringup/config/left_arm_rm75.yaml`
|
||
- Modify: `xr_rm_bringup/config/right_arm_rm75.yaml`
|
||
- Modify: `xr_rm_bringup/config/dual_arm_rm75.yaml`
|
||
|
||
- [ ] **Step 1: 在 setup.py 中只安装模型，不声明 pip Placo 依赖**
|
||
|
||
加入：
|
||
|
||
```python
|
||
from glob import glob
|
||
```
|
||
|
||
把 `data_files` 扩展为：
|
||
|
||
```python
|
||
data_files=[
|
||
(
|
||
"share/ament_index/resource_index/packages",
|
||
[f"resource/{package_name}"],
|
||
),
|
||
(f"share/{package_name}", ["package.xml"]),
|
||
(
|
||
f"share/{package_name}/models/rm75",
|
||
["models/rm75/RM75-B.urdf"],
|
||
),
|
||
(
|
||
f"share/{package_name}/models/rm75/meshes",
|
||
glob("models/rm75/meshes/*.STL"),
|
||
),
|
||
],
|
||
```
|
||
|
||
保持：
|
||
|
||
```python
|
||
install_requires=["setuptools"],
|
||
```
|
||
|
||
不得加入 `placo`、`pin`、`eigenpy` 或 `numpy`，以免 colcon 使用系统 Python 时触发安装或升级。
|
||
|
||
- [ ] **Step 2: launch 固定并验证 XR Python**
|
||
|
||
在 `arm_debug.launch.py` 加入：
|
||
|
||
```python
|
||
from pathlib import Path
|
||
```
|
||
|
||
定义：
|
||
|
||
```python
|
||
XR_PYTHON = "/home/robot/miniconda3/envs/xr/bin/python"
|
||
```
|
||
|
||
在 `_launch_setup()` 最前面校验：
|
||
|
||
```python
|
||
if not Path(XR_PYTHON).is_file():
|
||
raise RuntimeError(
|
||
f"XR Python not found: {XR_PYTHON}; "
|
||
"Placo 0.9.4 must not be installed globally"
|
||
)
|
||
```
|
||
|
||
给三个 `single_arm_velocity_teleop` Node 对象都加入：
|
||
|
||
```python
|
||
prefix=[XR_PYTHON],
|
||
```
|
||
|
||
不要给 `udp_controller_receiver` 加 prefix；它继续使用系统 ROS2 Python。
|
||
|
||
- [ ] **Step 3: launch 向每个遥操作节点传安装后的 URDF**
|
||
|
||
给三个遥操作 `Node` 的参数字典都加入：
|
||
|
||
```python
|
||
"robot_urdf_path": PathJoinSubstitution([
|
||
FindPackageShare("xr_rm_teleop"),
|
||
"models",
|
||
"rm75",
|
||
"RM75-B.urdf",
|
||
]),
|
||
```
|
||
|
||
删除 launch 中已经失效的 `frame_type` 参数传递、函数参数和对应的
|
||
`DeclareLaunchArgument`。
|
||
|
||
将控制周期注释更新为：
|
||
|
||
```python
|
||
# 每周期同步一次实际关节反馈、执行一次 Placo QP，再发送 rm_movej_canfd。
|
||
```
|
||
|
||
- [ ] **Step 4: 同步左、右、双臂 YAML**
|
||
|
||
三个 YAML 都删除：
|
||
|
||
```text
|
||
current_pose_poll_hz
|
||
mock_initial_pose
|
||
frame_type
|
||
```
|
||
|
||
保留每侧原有：
|
||
|
||
```text
|
||
initial_joint_pose
|
||
workspace_min / workspace_max
|
||
cyl_radius_limit
|
||
max_linear_speed / max_orientation_speed
|
||
configure_safety_limits: true
|
||
```
|
||
|
||
把左右单臂 YAML 都改为：
|
||
|
||
```yaml
|
||
move_to_initial_pose_on_connect: false
|
||
```
|
||
|
||
双臂文件原有左右两个 `false` 保持不变。把 `rm_movep_canfd` 注释改为
|
||
“Placo 单步 QP + `rm_movej_canfd` 关节透传”。
|
||
|
||
- [ ] **Step 5: 从工作空间根目录构建**
|
||
|
||
```bash
|
||
cd /home/robot/WS_xr
|
||
source /opt/ros/humble/setup.bash
|
||
colcon build --symlink-install
|
||
```
|
||
|
||
Expected: build summary 无失败包。
|
||
|
||
- [ ] **Step 6: 检查模型和 launch 安装结果**
|
||
|
||
```bash
|
||
test -f install/xr_rm_teleop/share/xr_rm_teleop/models/rm75/RM75-B.urdf
|
||
test -f install/xr_rm_teleop/share/xr_rm_teleop/models/rm75/meshes/link_7.STL
|
||
source install/setup.bash
|
||
ros2 launch xr_rm_bringup arm_debug.launch.py --show-args
|
||
```
|
||
|
||
Expected: 两个 `test` 返回 0；launch 参数列表正常显示，不包含 `frame_type`。
|
||
|
||
- [ ] **Step 7: 使用安装模型再次运行左右臂数值验收**
|
||
|
||
```bash
|
||
cd /home/robot/WS_xr
|
||
PYTHONPATH=src/xr_rm_teleop \
|
||
/home/robot/miniconda3/envs/xr/bin/python \
|
||
src/xr_rm_teleop/test/placo_ik_smoke.py \
|
||
install/xr_rm_teleop/share/xr_rm_teleop/models/rm75/RM75-B.urdf
|
||
```
|
||
|
||
Expected: 左、右均满足 `<=5 mm` 和 `<=2°`。
|
||
|
||
### Task 6: 更新文档并完成 mock 验证
|
||
|
||
**Files:**
|
||
|
||
- Modify: `README.md`
|
||
|
||
- [ ] **Step 1: 更新 README 控制链路**
|
||
|
||
把项目首页链路改为：
|
||
|
||
```text
|
||
PICO/XR 双手柄 UDP JSON
|
||
-> xr_rm_input/udp_controller_receiver
|
||
-> /xr/left_controller 与 /xr/right_controller
|
||
-> xr_rm_teleop/single_arm_velocity_teleop
|
||
-> TCP 安全限幅与工具/法兰目标换算
|
||
-> Placo 0.9.4 每周期一步 QP
|
||
-> rm_movej_canfd 发送 7 个 RM75 关节目标
|
||
```
|
||
|
||
明确说明：
|
||
|
||
```text
|
||
遥操作节点收到首帧有效实际关节反馈前不调用 QP、不发送运动指令。
|
||
QP 失败时沿用上一组有效关节目标；XR 超时、反馈过期、Grip 松开或
|
||
SDK 发送失败时执行慢停止。
|
||
```
|
||
|
||
- [ ] **Step 2: 更新环境说明**
|
||
|
||
在“环境准备”加入：
|
||
|
||
```text
|
||
Placo 固定复用 /home/robot/miniconda3/envs/xr：
|
||
- Python 3.10
|
||
- Placo 0.9.4
|
||
- Pin 3.7.0
|
||
- NumPy 2.2.6
|
||
|
||
arm_debug.launch.py 只对 single_arm_velocity_teleop 使用该 Python；
|
||
ros2、colcon 和 udp_controller_receiver 仍使用系统 Python。
|
||
禁止 pip --user、sudo pip 或系统级安装/升级 Placo、Pin、EigenPy 和 NumPy。
|
||
```
|
||
|
||
加入只读检查命令：
|
||
|
||
```bash
|
||
/home/robot/miniconda3/envs/xr/bin/python -c \
|
||
"import importlib.metadata; print(importlib.metadata.version('placo'))"
|
||
```
|
||
|
||
Expected: `0.9.4`。
|
||
|
||
- [ ] **Step 3: 更新参数和真机安全说明**
|
||
|
||
删除 README 对以下旧参数的描述：
|
||
|
||
```text
|
||
frame_type
|
||
current_pose_poll_hz
|
||
mock_initial_pose
|
||
```
|
||
|
||
把 `follow` 描述改成传给 `rm_movej_canfd`。把单臂自动回初始位姿说明改成：
|
||
|
||
```text
|
||
左臂、右臂和双臂默认都不会自动执行 movej(initial_joint_pose)。
|
||
只有用户在清空工作区并确认急停后显式传
|
||
move_to_initial_pose_on_connect:=true 才允许自动回初始位姿。
|
||
```
|
||
|
||
说明 `peripherals_rm75.yaml` 中左臂 `minisci +0.19 m`、右臂
|
||
`omnipic +0.16 m` 同时用于 RealMan 工具配置和 Placo TCP/法兰换算。
|
||
|
||
- [ ] **Step 4: 运行完整自动测试**
|
||
|
||
```bash
|
||
cd /home/robot/WS_xr
|
||
source /opt/ros/humble/setup.bash
|
||
source install/setup.bash
|
||
pytest -q \
|
||
src/xr_rm_teleop/test/test_orientation_control.py \
|
||
src/xr_rm_teleop/test/test_initial_joint_pose.py \
|
||
src/xr_rm_teleop/test/test_placo_transforms.py \
|
||
src/xr_rm_teleop/test/test_joint_control.py
|
||
```
|
||
|
||
Expected: 全部 PASS。
|
||
|
||
- [ ] **Step 5: 分别启动左臂和右臂 mock**
|
||
|
||
左臂：
|
||
|
||
```bash
|
||
cd /home/robot/WS_xr
|
||
source /opt/ros/humble/setup.bash
|
||
source install/setup.bash
|
||
timeout --signal=INT 8s \
|
||
ros2 launch xr_rm_bringup arm_debug.launch.py \
|
||
arm:=left use_mock:=true
|
||
```
|
||
|
||
右臂：
|
||
|
||
```bash
|
||
cd /home/robot/WS_xr
|
||
source /opt/ros/humble/setup.bash
|
||
source install/setup.bash
|
||
timeout --signal=INT 8s \
|
||
ros2 launch xr_rm_bringup arm_debug.launch.py \
|
||
arm:=right use_mock:=true
|
||
```
|
||
|
||
Expected: 两侧分别打印 Placo 0.9.4 启动、首帧有效关节反馈和节点启动日志；
|
||
没有 `ModuleNotFoundError`、QP `NaN` 或厂商 SDK import 错误。`timeout` 主动结束
|
||
launch 时退出码可以是 124。
|
||
|
||
- [ ] **Step 6: 最终静态安全检查**
|
||
|
||
```bash
|
||
cd /home/robot/WS_xr
|
||
rg -n "rm_movep_canfd|send_cartesian_target" \
|
||
src/xr_rm_teleop src/xr_rm_bringup
|
||
rg -n "rm_create_robot_arm" src/xr_rm_teleop
|
||
rg -n "_ik_solver\\.solve" \
|
||
src/xr_rm_teleop/xr_rm_teleop/single_arm_velocity_teleop.py
|
||
rg -n "configure_safety_limits: false|move_to_initial_pose_on_connect: true" \
|
||
src/xr_rm_bringup/config
|
||
git diff --check
|
||
git status --short
|
||
```
|
||
|
||
Expected:
|
||
|
||
- 旧笛卡尔运动链路无匹配。
|
||
- `rm_create_robot_arm` 只有一个实现调用点。
|
||
- `_ik_solver.solve` 在控制节点中只有一个调用点。
|
||
- 安全配置中没有默认关闭安全限位或默认自动回初始位姿。
|
||
- `git diff --check` 无空白错误。
|
||
- 原有用户改动 `D openspec/config.yaml` 仍保持原样，未被本任务触碰。
|
||
|
||
- [ ] **Step 7: 给用户提供左右臂各自的真机人工验收清单**
|
||
|
||
只提供命令，不由 Codex 执行：
|
||
|
||
```bash
|
||
ros2 launch xr_rm_bringup arm_debug.launch.py \
|
||
arm:=left use_mock:=false \
|
||
move_to_initial_pose_on_connect:=false
|
||
```
|
||
|
||
停止左臂节点后再执行：
|
||
|
||
```bash
|
||
ros2 launch xr_rm_bringup arm_debug.launch.py \
|
||
arm:=right use_mock:=false \
|
||
move_to_initial_pose_on_connect:=false
|
||
```
|
||
|
||
每侧检查：
|
||
|
||
1. 急停可用、工作区清空、速度保持低值。
|
||
2. 日志先出现首帧有效关节反馈，之后才允许按 Grip。
|
||
3. 静止握持时无启动跳变。
|
||
4. 只做小幅单轴位置和小角度姿态动作。
|
||
5. 松开 Grip、停止 XR 数据时均触发慢停止。
|
||
6. 人为制造不可达目标时不出现关节突跳；QP 失败保持上一组有效目标。
|
||
7. 左臂工具 TCP 使用 `minisci +0.19 m`，右臂使用 `omnipic +0.16 m`。
|
||
8. 停止一侧并确认连接释放后，才验证另一侧。
|
||
|
||
本轮不执行 `arm:=both use_mock:=false`。
|
||
|
||
- [ ] **Step 8: 最终检查变更范围，不提交**
|
||
|
||
```bash
|
||
git diff --stat
|
||
git diff -- \
|
||
src/xr_rm_teleop \
|
||
src/xr_rm_bringup \
|
||
src/README.md
|
||
```
|
||
|
||
Expected: 只包含本计划列出的 QP、Adapter、launch、配置、模型、测试和文档变更。
|
||
在用户明确授权之前不创建提交。
|