YikaiFu-cart/acRealman_xr
1
1
Fork 1
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Remove outdated design documents for RM75 control and feedback systems

Browse Source
This commit is contained in:
YikaiFu-cart
2026-07-29 16:24:07 +08:00
parent 08996434e5
commit f795c06d44
16 changed files with 3 additions and 4120 deletions
Download Patch File Download Diff File Expand all files Collapse all files
AGENTS.md
+3 -1
View File
@@ -227,7 +227,9 @@
## Git 与提交 ## Git 与提交
除非用户明确要求，否则不要自动提交、推送、创建分支或修改远程仓库。 除非用户明确要求，否则不要自动提交、推送或修改远程仓库。
使用 Superpowers 执行计划时，允许 subagent 按相关 skill 创建和使用独立 worktree 及其配套本地分支；其他情况下，除非用户明确要求，不要自动创建分支。
如果用户要求生成提交信息，提交信息应： 如果用户要求生成提交信息，提交信息应：
docs/superpowers/plans/2026-07-27-rm75-placo-qp-ik.md
-1644
View File
File diff suppressed because it is too large Load Diff
docs/superpowers/plans/2026-07-28-rm75-control-timing-stats.md
-154
View File
@@ -1,154 +0,0 @@
# RM75 Control Timing Stats 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:** 在 Grip 激活期间每约 5 秒向 `arm_debug.launch.py` 终端输出一次控制链路耗时统计。
**Architecture:** 在现有 `SingleArmVelocityTeleop` 控制回调内使用单调高精度时钟记录实际周期、控制路径总耗时、QP、关节发送和反馈年龄。节点保存一个固定长度样本窗口，满窗后用 NumPy 计算 mean/P95/P99/max，输出一条 ROS 日志并清空窗口。
**Tech Stack:** Python 3.10、ROS2 Humble `rclpy`、NumPy、pytest。
---
### Task 1: 控制周期统计
**Files:**
- Modify: `xr_rm_teleop/test/test_joint_control.py`
- Modify: `xr_rm_teleop/xr_rm_teleop/single_arm_velocity_teleop.py`
- [x] **Step 1: 写失败测试**
在 `test_joint_control.py` 添加确定性两样本窗口测试：
```python
def test_timing_stats_logs_summary_and_clears_window() -> None:
messages = []
teleop = object.__new__(SingleArmVelocityTeleop)
teleop._arm_name = "right_rm75"
teleop._dt = 0.008
teleop._timing_stats_window = 2
teleop._timing_samples = {
name: []
for name in ("period", "total", "qp", "send", "feedback_age")
}
teleop.get_logger = lambda: SimpleNamespace(
info=lambda message: messages.append(message)
)
teleop._record_timing_sample(7.0, 6.0, 1.0, 0.5, 3.0)
assert messages == []
teleop._record_timing_sample(9.0, 10.0, 2.0, 0.7, 4.0)
assert len(messages) == 1
assert "right_rm75 timing n=2 deadline=8.000 ms" in messages[0]
assert "period[n=2 mean=8.000 p95=8.900 p99=8.980 max=9.000 ms overruns=1]" in messages[0]
assert "total[n=2 mean=8.000 p95=9.800 p99=9.960 max=10.000 ms overruns=1]" in messages[0]
assert "qp[n=2" in messages[0]
assert "send[n=2" in messages[0]
assert "feedback_age[n=2" in messages[0]
assert all(not samples for samples in teleop._timing_samples.values())
```
- [x] **Step 2: 确认测试因功能缺失而失败**
在工作空间根目录运行：
```bash
source /opt/ros/humble/setup.bash
PYTHONPATH=src/xr_rm_teleop pytest -q \
src/xr_rm_teleop/test/test_joint_control.py::test_timing_stats_logs_summary_and_clears_window
```
预期：失败并提示 `SingleArmVelocityTeleop` 没有 `_record_timing_sample`。
- [x] **Step 3: 实现最小统计逻辑**
在节点初始化中创建约 5 秒的窗口：
```python
self._timing_stats_window = max(1, int(round(5.0 / self._dt)))
self._timing_samples = {
name: []
for name in ("period", "total", "qp", "send", "feedback_age")
}
self._last_control_tick_started_ns: int | None = None
```
为每组样本计算统计摘要：
```python
def _timing_summary(
self,
name: str,
samples: list[float],
deadline_ms: float | None = None,
) -> str:
values = np.asarray(samples)
result = (
f"{name}[n={len(samples)} mean={np.mean(values):.3f} "
f"p95={np.percentile(values, 95):.3f} "
f"p99={np.percentile(values, 99):.3f} "
f"max={np.max(values):.3f} ms"
)
if deadline_ms is not None:
result += f" overruns={np.count_nonzero(values > deadline_ms)}"
return result + "]"
```
满窗后输出并清空：
```python
def _record_timing_sample(
self,
period_ms: float | None,
total_ms: float,
qp_ms: float,
send_ms: float,
feedback_age_ms: float,
) -> None:
if period_ms is not None:
self._timing_samples["period"].append(period_ms)
self._timing_samples["total"].append(total_ms)
self._timing_samples["qp"].append(qp_ms)
self._timing_samples["send"].append(send_ms)
self._timing_samples["feedback_age"].append(feedback_age_ms)
if len(self._timing_samples["total"]) < self._timing_stats_window:
return
deadline_ms = self._dt * 1000.0
summaries = [
self._timing_summary("period", self._timing_samples["period"], deadline_ms),
self._timing_summary("total", self._timing_samples["total"], deadline_ms),
self._timing_summary("qp", self._timing_samples["qp"]),
self._timing_summary("send", self._timing_samples["send"]),
self._timing_summary("feedback_age", self._timing_samples["feedback_age"]),
]
self.get_logger().info(
f"{self._arm_name} timing n={len(self._timing_samples['total'])} "
f"deadline={deadline_ms:.3f} ms | " + " | ".join(summaries)
)
for samples in self._timing_samples.values():
samples.clear()
```
在 `_control_tick()` 中围绕 QP 和发送调用采样，并在关节命令处理完成后记录总耗时。早退周期不进入统计窗口，现有控制和安全逻辑保持不变。
- [x] **Step 4: 运行测试确认通过**
```bash
source /opt/ros/humble/setup.bash
PYTHONPATH=src/xr_rm_teleop pytest -q src/xr_rm_teleop/test/test_joint_control.py
```
预期：全部通过。
- [x] **Step 5: 完整验证**
```bash
source /opt/ros/humble/setup.bash
pytest -q src/xr_rm_teleop/test/test_orientation_control.py
colcon build --symlink-install
```
预期：姿态测试和工作空间构建全部通过。根据仓库规则，不自动提交 Git。
docs/superpowers/plans/2026-07-28-rm75-feedback-absolute-scheduling.md
-109
View File
@@ -1,109 +0,0 @@
# RM75 Feedback Absolute Scheduling 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:** 将关节反馈线程从“读取后固定等待 8 ms”改为无历史周期补跑的绝对起始周期调度。
**Architecture:** `RealManAdapter._feedback_loop()` 保留现有读取、告警和停止结构，只把固定 `Event.wait(feedback_period)` 替换为下一截止时间计算。读取提前完成时等待剩余时间；读取超期时重置调度基准并立即进入下一周期。
**Tech Stack:** Python 3.10、threading、time.monotonic、pytest、ROS2 Humble、colcon
---
### Task 1: 反馈绝对周期调度
**Files:**
- Modify: `xr_rm_teleop/xr_rm_teleop/realman_adapter.py`
- Test: `xr_rm_teleop/test/test_initial_joint_pose.py`
- [ ] **Step 1: 写失败测试**
用确定性的 FakeTime 和 FakeStopEvent 运行 `_feedback_loop()` 三次读取：
- 第一次读取在 5 ms 完成，应只等待剩余 3 ms；
- 第二次在 18 ms 完成，超过 16 ms 截止时间，应不等待并把基准重置为
18 ms；
- 第三次在 23 ms 完成，应等待到新基准的 26 ms，即再次等待 3 ms。
断言读取三次且 `wait()` 参数为 `[0.003, 0.003]`。该结果同时证明没有补跑
旧的 8 ms 和 16 ms 截止点。
- [ ] **Step 2: 确认测试失败**
```bash
source /opt/ros/humble/setup.bash
python3 -m pytest -q src/xr_rm_teleop/test/test_initial_joint_pose.py
```
预期：当前固定等待实现记录 `[0.008, 0.008, 0.008]`，测试失败。
- [ ] **Step 3: 写最小实现**
把 `_feedback_loop()` 改为：
```python
def _feedback_loop(self) -> None:
next_read_at = time.monotonic()
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
next_read_at += self._feedback_period
remaining = next_read_at - time.monotonic()
if remaining <= 0.0:
next_read_at = time.monotonic()
continue
self._feedback_stop.wait(remaining)
```
- [ ] **Step 4: 确认局部测试通过**
重复 Step 2 命令。预期：全部 PASS。
### Task 2: 调度回归验证
**Files:**
- Verify: `xr_rm_teleop`
- Verify: ROS2 workspace
- [ ] **Step 1: 运行遥操作包测试**
```bash
source /opt/ros/humble/setup.bash
python3 -m pytest -q src/xr_rm_teleop/test
```
预期：全部 PASS。
- [ ] **Step 2: 运行姿态控制指定测试**
```bash
source /opt/ros/humble/setup.bash
python3 -m pytest -q src/xr_rm_teleop/test/test_orientation_control.py
```
预期：全部 PASS。
- [ ] **Step 3: 构建工作空间**
```bash
source /opt/ros/humble/setup.bash
colcon build --symlink-install
```
预期：四个包构建成功。
- [ ] **Step 4: 检查最终差异**
```bash
git diff --check
git status --short
```
预期：只包含已确认的统计、工具坐标系幂等修复、反馈绝对周期调度、对应测试
及 Superpowers 文档。按仓库规则不自动提交。
docs/superpowers/plans/2026-07-28-rm75-feedback-thread-timing.md
-167
View File
@@ -1,167 +0,0 @@
# RM75 Feedback Thread Timing 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:** 在不改变反馈轮询和机械臂控制行为的前提下，统计 `rm_get_joint_degree()` 调用耗时与成功反馈更新间隔。
**Architecture:** `RealManAdapter` 在反馈读取边界测量时间，并把可选计时值随 `JointStateSnapshot` 放入现有缓存。`SingleArmVelocityTeleop` 复用现有 timing 窗口，只对新的反馈时间戳记录一次并输出汇总。
**Tech Stack:** Python 3.10、ROS2 Humble、pytest、NumPy、colcon
---
### Task 1: 在反馈缓存中携带真实读取计时
**Files:**
- Modify: `xr_rm_teleop/xr_rm_teleop/realman_adapter.py`
- Test: `xr_rm_teleop/test/test_initial_joint_pose.py`
- [ ] **Step 1: 写失败测试**
在 `test_joint_feedback_is_cached_in_radians` 中通过 `monkeypatch` 固定
`perf_counter_ns()` 和 `monotonic()`，连续读取两次，并验证：
```python
assert first.read_duration_ms == pytest.approx(2.0)
assert first.update_interval_ms is None
assert second.read_duration_ms == pytest.approx(3.0)
assert second.update_interval_ms == pytest.approx(11.0)
```
- [ ] **Step 2: 确认测试失败**
在 `/home/robot/WS_xr` 执行：
```bash
source /opt/ros/humble/setup.bash
python3 -m pytest -q src/xr_rm_teleop/test/test_initial_joint_pose.py::test_joint_feedback_is_cached_in_radians
```
预期：因 `JointStateSnapshot` 尚无计时字段而失败。
- [ ] **Step 3: 最小实现反馈计时**
给快照增加可选字段，保持现有两参数构造兼容：
```python
@dataclass(frozen=True)
class JointStateSnapshot:
positions: list[float]
received_at: float
read_duration_ms: float | None = None
update_interval_ms: float | None = None
```
在 `_read_joint_state_once()` 中只包围 SDK 调用测量 `read_duration_ms`；数据校验
成功后取得 `received_at`，并在缓存锁内根据上一快照计算
`update_interval_ms`。`get_latest_joint_state()` 同步复制两个字段。Mock 使用
字段默认值，不伪造计时。
- [ ] **Step 4: 确认局部测试通过**
重复 Step 2 命令。预期：PASS。
### Task 2: 将唯一反馈样本加入现有 timing 汇总
**Files:**
- Modify: `xr_rm_teleop/xr_rm_teleop/single_arm_velocity_teleop.py`
- Test: `xr_rm_teleop/test/test_joint_control.py`
- [ ] **Step 1: 写失败测试**
扩展 `test_timing_stats_logs_summary_and_clears_window`：在三个控制样本中传入
“快照 A、重复快照 A、快照 B”，并断言日志包含：
```python
assert "feedback_read[n=2" in messages[0]
assert "feedback_interval[n=1" in messages[0]
```
这样同时验证新反馈只计一次、重复缓存不重复计数、首次反馈无更新间隔。
- [ ] **Step 2: 确认测试失败**
在 `/home/robot/WS_xr` 执行：
```bash
source /opt/ros/humble/setup.bash
python3 -m pytest -q src/xr_rm_teleop/test/test_joint_control.py::test_timing_stats_logs_summary_and_clears_window
```
预期：因 timing 样本尚不支持新字段而失败。
- [ ] **Step 3: 最小实现唯一反馈统计**
在节点初始化时：
```python
self._timing_samples = {
name: []
for name in (
"period",
"total",
"qp",
"send",
"feedback_age",
"feedback_read",
"feedback_interval",
)
}
self._last_timing_feedback_received_at: float | None = None
```
让 `_record_timing_sample()` 接收当前 `JointStateSnapshot`。仅当
`received_at` 与 `_last_timing_feedback_received_at` 不同时，追加非 `None`
的读取耗时和更新间隔。汇总时仅输出非空的新数组，避免 mock 模式对空数组
求百分位数。控制循环把已有 `snapshot` 传入该函数。
- [ ] **Step 4: 确认相关测试通过**
```bash
source /opt/ros/humble/setup.bash
python3 -m pytest -q src/xr_rm_teleop/test/test_joint_control.py src/xr_rm_teleop/test/test_initial_joint_pose.py
```
预期：全部 PASS。
### Task 3: 回归验证
**Files:**
- Verify: `xr_rm_teleop`
- Verify: ROS2 workspace
- [ ] **Step 1: 运行遥操作包测试**
```bash
source /opt/ros/humble/setup.bash
python3 -m pytest -q src/xr_rm_teleop/test
```
预期：全部 PASS。
- [ ] **Step 2: 运行姿态控制指定测试**
```bash
source /opt/ros/humble/setup.bash
python3 -m pytest -q src/xr_rm_teleop/test/test_orientation_control.py
```
预期：全部 PASS。
- [ ] **Step 3: 构建工作空间**
```bash
source /opt/ros/humble/setup.bash
colcon build --symlink-install
```
预期：四个包构建成功。
- [ ] **Step 4: 检查差异**
```bash
git diff --check
git status --short
```
预期：只包含设计、计划、反馈计时实现及相关测试。按仓库规则不自动提交。
docs/superpowers/plans/2026-07-28-rm75-idempotent-tool-frame.md
-107
View File
@@ -1,107 +0,0 @@
# RM75 Idempotent Tool Frame 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:** 让 RealMan 工具坐标系在首次启动时创建、后续启动时更新，并检查所有相关 SDK 返回值。
**Architecture:** `fun_peripheral.py` 增加一个只负责工具坐标系的内部函数，先查询名称列表，再选择创建或更新，最后切换。现有 `peripheral_cfg()` 继续负责 IO 和夹爪初始化，只把原来的两次无检查调用替换为该函数。
**Tech Stack:** Python 3.10、RealMan Python API2、pytest、ROS2 Humble、colcon
---
### Task 1: 工具坐标系幂等配置
**Files:**
- Modify: `xr_rm_teleop/xr_rm_teleop/fun_peripheral.py`
- Test: `xr_rm_teleop/test/test_initial_joint_pose.py`
- [ ] **Step 1: 写失败测试**
导入新的 `_configure_tool_frame`，用 FakeArm 分别返回包含和不包含 `omnipic`
的名称列表。断言不存在时调用 `rm_set_manual_tool_frame`，存在时调用
`rm_update_tool_frame`，两条路径最后都调用 `rm_change_tool_frame`。
再用参数化失败返回码验证查询、创建/更新和切换失败均抛出包含 SDK 操作名称
的 `RuntimeError`。
- [ ] **Step 2: 确认测试失败**
```bash
source /opt/ros/humble/setup.bash
python3 -m pytest -q src/xr_rm_teleop/test/test_initial_joint_pose.py
```
预期：因 `_configure_tool_frame` 尚不存在而在测试收集阶段失败。
- [ ] **Step 3: 写最小实现**
在 `fun_peripheral.py` 增加：
```python
def _check_sdk_return(result: Any, operation: str) -> None:
if result != 0:
raise RuntimeError(f"{operation} failed with code {result}: {result!r}")
def _configure_tool_frame(robot, tool_frame, tool_name: str) -> None:
frames = robot.rm_get_total_tool_frame()
if not isinstance(frames, dict):
raise RuntimeError(
f"rm_get_total_tool_frame returned invalid data: {frames!r}"
)
_check_sdk_return(
frames.get("return_code"),
"rm_get_total_tool_frame",
)
tool_names = frames.get("tool_names")
if not isinstance(tool_names, (list, tuple)):
raise RuntimeError(
f"rm_get_total_tool_frame returned invalid tool_names: {tool_names!r}"
)
if tool_name in tool_names:
operation = "rm_update_tool_frame"
result = robot.rm_update_tool_frame(frame=tool_frame)
else:
operation = "rm_set_manual_tool_frame"
result = robot.rm_set_manual_tool_frame(frame=tool_frame)
_check_sdk_return(result, operation)
_check_sdk_return(
robot.rm_change_tool_frame(tool_name),
"rm_change_tool_frame",
)
```
在 `peripheral_cfg()` 中用
`_configure_tool_frame(robot, tool_frame, tool_name)` 替换原来的创建和切换
调用。
- [ ] **Step 4: 确认测试通过**
重复 Step 2 命令。预期：全部 PASS。
### Task 2: 工具修复回归验证
**Files:**
- Verify: `xr_rm_teleop`
- Verify: ROS2 workspace
- [ ] **Step 1: 运行遥操作包测试**
```bash
source /opt/ros/humble/setup.bash
python3 -m pytest -q src/xr_rm_teleop/test
```
预期：全部 PASS。
- [ ] **Step 2: 构建工作空间**
```bash
source /opt/ros/humble/setup.bash
colcon build --symlink-install
```
预期：四个包构建成功。
docs/superpowers/plans/2026-07-28-rm75-so3-omnipicker-teleop.md
-390
View File
@@ -1,390 +0,0 @@
# RM75 SO(3) 姿态跟随与 OmniPicker 模型 Implementation Plan
> **For Codex:** REQUIRED SUB-SKILL: Use `superpowers:executing-plans` to implement this plan task-by-task.
**Goal:** 去掉遥操作控制路径中的 RPY 往返转换，使 RM75 TCP 姿态始终沿 SO(3) 最短路径跟随，并让左右臂的 Placo QP 直接控制一体化模型中的 `omnipicker_tcp`。
**Architecture:** 保留现有单节点、单步 Placo QP、关节反馈、RealMan 连接和安全停止链路。XR 四元数映射为机器人旋转矩阵；平移使用直接位置差，姿态使用 SO(3) 对数误差，二者以解耦 `3+3` 形式处理。Placo 接收完整 `4×4` 目标矩阵并直接约束 URDF 的 `omnipicker_tcp`，不再读取外设工具位姿做 QP 末端换算。
**Tech Stack:** Ubuntu 22.04、ROS2 Humble、Python 3.10、NumPy、Placo 0.9.4、Pinocchio 3.7.0、pytest、URDF。
**Repository rule:** 不执行 `git commit`、`git push` 或真机命令。所有启动验证必须显式使用 `use_mock:=true`；`peripherals_rm75.yaml`、`avoid_singularity`、可操作度任务和既有安全限制保持不变。
---
## 文件范围
- Create: `xr_rm_teleop/models/rm75_omnipicker/urdf/RM75-B_OmniPicker_fixed.urdf`
- Create: `xr_rm_teleop/models/rm75_omnipicker/meshes/rm75/*.STL`
- Create: `xr_rm_teleop/models/rm75_omnipicker/meshes/omnipicker/*.STL`
- Modify: `xr_rm_teleop/setup.py`
- Modify: `xr_rm_teleop/xr_rm_teleop/placo_ik_solver.py`
- Modify: `xr_rm_teleop/xr_rm_teleop/single_arm_velocity_teleop.py`
- Modify: `xr_rm_teleop/test/test_orientation_control.py`
- Modify: `xr_rm_teleop/test/test_placo_transforms.py`
- Modify: `xr_rm_teleop/test/test_joint_control.py`
- Modify: `xr_rm_teleop/test/placo_ik_smoke.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`
- Modify: `README.md`
不删除旧 `xr_rm_teleop/models/rm75` 资源，只让 launch 停止选用它，避免扩大无关清理范围。
### Task 1: 导入 fixed 一体化模型并定义 TCP
**Files:**
- Create: `xr_rm_teleop/models/rm75_omnipicker/urdf/RM75-B_OmniPicker_fixed.urdf`
- Create: `xr_rm_teleop/models/rm75_omnipicker/meshes/rm75/*.STL`
- Create: `xr_rm_teleop/models/rm75_omnipicker/meshes/omnipicker/*.STL`
- Modify: `xr_rm_teleop/setup.py`
- Modify: `xr_rm_teleop/test/test_placo_transforms.py`
- [x] **Step 1: 先写模型结构失败测试**
在 `test_placo_transforms.py` 中用 `xml.etree.ElementTree` 读取 fixed URDF，断言：
```python
assert moving_joint_names == [f"joint_{index}" for index in range(1, 8)]
assert tcp_joint.attrib["type"] == "fixed"
assert tcp_joint.find("parent").attrib["link"] == "omnipicker_base_link"
assert tcp_joint.find("child").attrib["link"] == "omnipicker_tcp"
assert tcp_joint.find("origin").attrib["xyz"] == "0 0 0.16"
assert tcp_joint.find("origin").attrib["rpy"] == "0 0 0"
```
运行：
```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，模型包尚不存在。
- [x] **Step 2: 从上传 ZIP 只导入运行所需资源**
从
`/home/robot/下载/Models/RM75-B_OmniPicker_Pinocchio.zip`
导入 fixed URDF 和两组 mesh 到
`xr_rm_teleop/models/rm75_omnipicker`；不导入独立描述包元数据、示例脚本、
活动式 URDF 或额外验证文档。保留上传模型的几何、惯量、关节限制和 fixed
OmniPicker 关节，并在 `xr_rm_teleop/setup.py` 中安装这些资源。
- [x] **Step 3: 在 fixed URDF 增加已确认的 TCP**
```xml
<link name="omnipicker_tcp"/>
<joint name="omnipicker_tcp_joint" type="fixed">
<parent link="omnipicker_base_link"/>
<child link="omnipicker_tcp"/>
<origin xyz="0 0 0.16" rpy="0 0 0"/>
</joint>
```
- [x] **Step 4: 重跑模型测试**
Expected: PASS；运动关节仍严格为 `joint_1` 至 `joint_7`，TCP 偏移为
`+Z 0.16 m`。
### Task 2: 让 Placo 直接接收 SE(3) 并约束 `omnipicker_tcp`
**Files:**
- Modify: `xr_rm_teleop/xr_rm_teleop/placo_ik_solver.py`
- Modify: `xr_rm_teleop/test/test_placo_transforms.py`
- Modify: `xr_rm_teleop/test/placo_ik_smoke.py`
- [x] **Step 1: 先把变换和 smoke 测试改为矩阵接口**
测试改为：
```python
current = solver.update_joint_state(joints)
assert current.shape == (4, 4)
target = current.copy()
target[0, 3] += 0.01
target[:3, :3] = rotation_delta @ target[:3, :3]
joints = solver.solve(target)
```
同时覆盖非法形状、NaN 和非 SE(3) 最后一行会被拒绝。smoke 使用
`dt=1/125`，以旋转矩阵相对角度计算姿态误差，不再转换 RPY。
运行现有两项测试，确认它们先因旧 `ArmPose/tool_pose` 接口失败。
- [x] **Step 2: 最小化求解器接口**
将构造函数改为：
```python
PlacoIkSolver(urdf_path: str, dt: float)
```
并完成以下替换：
- 删除 `_rpy_to_rotation`、`_rotation_to_rpy`、`_arm_pose_to_transform`、
`_transform_to_arm_pose`、`_tool_pose_to_transform`。
- 删除 `_tool_transform` 和 `_tool_inverse`。
- frame task 从 `link_7` 改为 `omnipicker_tcp`。
- 可操作度任务继续作用于原来的 `link_7`，并保留原权重
`soft, 5e-2`。
- `update_joint_state()` 直接返回
`get_T_world_frame("omnipicker_tcp").copy()`。
- `solve()` 校验并直接设置传入的 `4×4` 目标矩阵。
- frame task、动能正则、虚拟基座固定、关节位置/速度校验保持原状。
- [x] **Step 3: 运行纯单元测试**
```bash
PYTHONPATH=src/xr_rm_teleop pytest -q \
src/xr_rm_teleop/test/test_placo_transforms.py
```
Expected: PASS。该命令不构造 Placo，不要求系统 Python 安装厂商 SDK。
### Task 3: 用 SO(3) 最短路径替换 RPY 姿态控制
**Files:**
- Modify: `xr_rm_teleop/test/test_orientation_control.py`
- Modify: `xr_rm_teleop/xr_rm_teleop/single_arm_velocity_teleop.py`
- [x] **Step 1: 先写 SO(3) 回归测试**
保留零四元数停止测试，并增加以下最小覆盖：
- `q` 与 `-q` 得到同一旋转矩阵。
- 初始 pitch 接近 `+90°`、`-90°` 时，小手柄旋转只产生同量级的小旋转。
- 跨过旧 RPY 分支时，相对旋转仍取最短路径。
- 死区按 `norm(Log(R_target R_currentᵀ))` 判断。
- `alpha=0.5` 时 SO(3) 误差角减半。
- `dt=1/125`、`max_orientation_speed=0.5` 时单步不超过 `0.004 rad`。
- 关闭某姿态轴时，在机器人基坐标系将对应旋转向量分量清零。
- 矩阵转调试四元数后有限且单位化。
运行：
```bash
PYTHONPATH=src/xr_rm_teleop pytest -q \
src/xr_rm_teleop/test/test_orientation_control.py
```
Expected: FAIL，旧代码仍返回和处理 RPY。
- [x] **Step 2: 实现最少的 NumPy SO(3) 运算**
在现有遥操作模块中加入并只加入实际调用的函数：
```text
quaternion -> rotation matrix
rotation matrix -> normalized quaternion
Log_SO3(rotation) -> 3D rotation vector
Exp_SO3(rotation vector) -> rotation matrix
position + rotation -> 4×4 transform
```
输入必须有限。近似旋转矩阵仅在
`norm(RᵀR-I) <= 1e-3` 且行列式为正时用 SVD 投影；明显无效输入抛出
`ValueError`。`Log_SO3` 在接近 `π` 时仍返回最短的有限旋转向量。
- [x] **Step 3: 替换姿态目标、滤波和限速**
控制路径统一为：
```python
R_xr_delta = R_xr_now @ R_xr_start.T
R_robot_delta = mapping @ R_xr_delta @ mapping.T
axis_delta = log_so3(R_robot_delta)
axis_delta[disabled_axes] = 0.0
R_raw = exp_so3(axis_delta) @ R_robot_start
error = log_so3(R_target @ R_current.T)
R_next = exp_so3(scale * error) @ R_current
```
继续分别保存平移列表和旋转矩阵状态，但构造 QP 目标与调试目标时合成为
`4×4` 矩阵。删除控制路径中的 `_matrix_to_euler`、
`_quaternion_to_euler`、分量 `_angle_delta` 及 RPY
死区/滤波/限速；位置死区、滤波、工作空间和圆柱限位原样保留。
- [x] **Step 4: 重跑姿态测试**
Expected: PASS。
### Task 4: 把节点状态、QP 和调试话题贯通为 SE(3)
**Files:**
- Modify: `xr_rm_teleop/xr_rm_teleop/single_arm_velocity_teleop.py`
- Modify: `xr_rm_teleop/test/test_joint_control.py`
- [x] **Step 1: 先将关节控制测试改为 `4×4` 矩阵**
Fake solver 的 `update_joint_state()` 返回有限齐次矩阵；QP
成功、失败和首帧反馈测试均断言矩阵接口。运行测试，确认旧类型假设失败。
- [x] **Step 2: 完成节点矩阵状态迁移**
- `_robot_start_pose`、`_last_current_pose` 和调试 fallback 改存 `4×4`
矩阵。
- `PlacoIkSolver` 初始化不再接收
`self._peripheral_config.tool_pose`；外设配置仍只传给
`RealManAdapter.configure_peripheral()`。
- 原始目标与发送目标均合成为 `omnipicker_tcp` 的 SE(3)。
- `TwistStamped.angular` 使用
`Log(R_sent R_previousᵀ) / dt`，表达在 `rm_base`。
- `PoseStamped` 只在发布边界把旋转矩阵转四元数。
- QP 异常继续返回 last-known-good；Grip 松开、超时、反馈错误和发送错误继续
走现有慢停与状态重置。
- [x] **Step 3: 运行相关单元测试**
```bash
PYTHONPATH=src/xr_rm_teleop pytest -q \
src/xr_rm_teleop/test/test_orientation_control.py \
src/xr_rm_teleop/test/test_joint_control.py \
src/xr_rm_teleop/test/test_placo_transforms.py \
src/xr_rm_teleop/test/test_initial_joint_pose.py
```
Expected: PASS。
### Task 5: 切换 launch 模型并同步已确认参数
**Files:**
- 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`
- Modify: `xr_rm_teleop/setup.py`
- Modify: `xr_rm_teleop/xr_rm_teleop/single_arm_velocity_teleop.py`
- Modify: `README.md`
- [x] **Step 1: 修改模型来源**
`_rm75_urdf()` 改为：
```python
PathJoinSubstitution([
FindPackageShare("xr_rm_teleop"),
"models",
"rm75_omnipicker",
"urdf",
"RM75-B_OmniPicker_fixed.urdf",
])
```
并让 `xr_rm_teleop/setup.py` 安装该目录下的 fixed URDF 和两组 mesh。
- [x] **Step 2: 只修改已确认参数**
节点默认值、launch 默认值和三份 YAML 对应项同步：
```yaml
control_rate_hz: 125.0
orientation_deadband_rad: 0.005
orientation_filter_alpha: 0.65
max_orientation_speed: 0.5
follow: false
```
其中右臂 YAML 的
`move_to_initial_pose_on_connect: True`
改为 `false`。不修改任何工作空间、圆柱、线速度、关节速度、初始角、
`avoid_singularity`、安全配置或外设配置。
- [x] **Step 3: 更新 README 中已失真的运行说明**
只更新：
- 默认控制频率 `90.0 -> 125.0`。
- QP 模型改为一体化 fixed URDF，并直接控制 `omnipicker_tcp`。
- 姿态死区、滤波和限速使用 SO(3) 最短路径，不使用 RPY。
- `peripherals_rm75.yaml` 仍只用于真实控制器工具坐标、负载和外设选择，不再
参与 Placo TCP 矩阵换算。
### Task 6: 构建、数值 smoke 与 mock 启动验证
**Files:**
- Modify: `xr_rm_teleop/test/placo_ik_smoke.py`
- [x] **Step 1: 构建整个工作空间**
```bash
cd /home/robot/WS_xr
source /opt/ros/humble/setup.bash
colcon build --symlink-install
```
Expected: `xr_rm_teleop` 和 `xr_rm_bringup` 构建成功。
- [x] **Step 2: 运行指定姿态测试和相关回归测试**
```bash
source /opt/ros/humble/setup.bash
PYTHONPATH=src/xr_rm_teleop pytest -q \
src/xr_rm_teleop/test/test_orientation_control.py
PYTHONPATH=src/xr_rm_teleop pytest -q \
src/xr_rm_teleop/test/test_joint_control.py \
src/xr_rm_teleop/test/test_placo_transforms.py \
src/xr_rm_teleop/test/test_initial_joint_pose.py
```
Expected: PASS。
- [x] **Step 3: 使用固定 XR Python 运行 Placo 数值 smoke**
```bash
source install/setup.bash
/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_omnipicker/urdf/RM75-B_OmniPicker_fixed.urdf
```
对左右初始关节姿态分别验证：
- 七个运动关节及顺序正确。
- `omnipicker_tcp` 相对 `link_7` 为 `[0, 0, 0.16]`、单位旋转。
- QP 输出七个有限关节角并满足位置与单周期速度限制。
- 目标停止两秒时打印最大关节变化，但不把漂移设为失败条件。
- 运动目标最终 TCP 位置误差 `<= 5 mm`，姿态误差 `<= 2°`。
- 打印平均/最大求解耗时及超过 `8 ms` 周期预算的次数，只记录、不设机器相关
的硬失败阈值。
- [x] **Step 4: 只启动 mock**
分别短时启动：
```bash
source /opt/ros/humble/setup.bash
source install/setup.bash
ros2 launch xr_rm_bringup arm_debug.launch.py arm:=left use_mock:=true
ros2 launch xr_rm_bringup arm_debug.launch.py arm:=right use_mock:=true
ros2 launch xr_rm_bringup arm_debug.launch.py arm:=both use_mock:=true
```
确认 fixed URDF、Placo 和左右节点名加载成功，无 RealMan SDK 导入或网络连接。
由人工结束 mock launch；Codex 不执行任何 `use_mock:=false` 命令。
- [x] **Step 5: 最终范围检查**
```bash
git diff --check
git status --short
git diff -- \
src/xr_rm_teleop \
src/xr_rm_bringup \
src/README.md \
src/docs/superpowers
```
确认 `peripherals_rm75.yaml`、`avoid_singularity`、可操作度权重和所有既有安全
限制未被改变。
docs/superpowers/plans/2026-07-29-rm75-canfd-udp-feedback.md
-508
View File
@@ -1,508 +0,0 @@
# RM75 CANFD UDP Feedback 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:** Replace synchronous TCP joint polling with the vendor UDP realtime callback while making YAML the source of robot behavior and hardware defaults.
**Architecture:** Keep one `RoboticArm(RM_TRIPLE_MODE_E)` handle per arm. TCP sends CANFD and safety/tool commands; a 5 ms controller UDP push invokes a minimal callback that updates the existing locked joint snapshot. Launch keeps only topology, mock safety mode, PICO input, and generated paths/topics.
**Tech Stack:** Python 3.10, ROS2 Humble, RealMan Python API2, YAML, pytest, colcon
---
### Task 1: Add failing UDP feedback adapter tests
**Files:**
- Modify: `xr_rm_teleop/test/test_initial_joint_pose.py`
- [ ] **Step 1: Replace polling-specific tests with UDP callback tests**
Add `sys`, `types`, and `SimpleNamespace` imports. Replace
`test_joint_feedback_is_cached_in_radians` and
`test_feedback_loop_uses_absolute_schedule_without_catch_up` with helpers and
tests equivalent to:
```python
def _udp_state(robot_ip="127.0.0.1", joints=None, error_code=0):
return SimpleNamespace(
errCode=error_code,
arm_ip=robot_ip.encode(),
joint_status=SimpleNamespace(
joint_position=joints or [0.0, 10.0, -20.0, 30.0, -40.0, 50.0, -60.0]
),
)
def test_udp_feedback_is_cached_in_radians(monkeypatch) -> None:
monotonic = iter([10.0, 10.005])
monkeypatch.setattr(realman_adapter.time, "monotonic", lambda: next(monotonic))
adapter = RealManAdapter(
"127.0.0.1",
8080,
0,
"127.0.0.1",
8090,
)
adapter._accept_realtime_feedback = True
adapter._on_realtime_arm_state(_udp_state())
first = adapter.get_latest_joint_state()
adapter._on_realtime_arm_state(_udp_state())
second = adapter.get_latest_joint_state()
assert first is not None
assert first.positions == pytest.approx(
[math.radians(value) for value in [0, 10, -20, 30, -40, 50, -60]]
)
assert first.read_duration_ms is None
assert first.update_interval_ms is None
assert second is not None
assert second.update_interval_ms == pytest.approx(5.0)
@pytest.mark.parametrize(
"state",
[
_udp_state(error_code=-3),
_udp_state(robot_ip="192.168.192.18"),
_udp_state(joints=[0.0] * 6),
_udp_state(joints=[0.0, 0.0, 0.0, math.nan, 0.0, 0.0, 0.0]),
],
)
def test_invalid_udp_feedback_does_not_replace_snapshot(state) -> None:
adapter = RealManAdapter(
"127.0.0.1",
8080,
0,
"127.0.0.1",
8090,
)
adapter._accept_realtime_feedback = True
adapter._on_realtime_arm_state(_udp_state(joints=[1.0] * 7))
before = adapter.get_latest_joint_state()
adapter._on_realtime_arm_state(state)
assert adapter.get_latest_joint_state() == before
```
Add a fake vendor module that records callback registration and push config.
Its `rm_set_realtime_push()` invokes the registered callback with `_udp_state()`.
Assert:
```python
adapter.connect()
arm = fake_module.RoboticArm.instance
assert arm.config.args == (5, True, 8090, 0, "192.168.192.148")
assert arm.callback is adapter._realtime_callback
assert adapter.get_latest_joint_state() is not None
assert not hasattr(adapter, "_feedback_thread")
```
Add failure cases where `rm_set_realtime_push()` returns `1`, and where
`adapter._feedback_ready.wait` returns `False`. Both must raise `RuntimeError`;
the fake arm must record one `rm_delete_robot_arm()` call.
- [ ] **Step 2: Run the focused tests and verify RED**
Run:
```bash
cd /home/robot/WS_xr
source /opt/ros/humble/setup.bash
python3 -m pytest -q src/xr_rm_teleop/test/test_initial_joint_pose.py
```
Expected: FAIL because `RealManAdapter` does not accept realtime push
parameters and has no `_on_realtime_arm_state`.
---
### Task 2: Implement single-handle UDP feedback
**Files:**
- Modify: `xr_rm_teleop/xr_rm_teleop/realman_adapter.py`
- Modify: `xr_rm_teleop/xr_rm_teleop/single_arm_velocity_teleop.py`
- Modify: `xr_rm_teleop/test/test_initial_joint_pose.py`
- [ ] **Step 1: Replace polling constructor state with realtime push state**
Change `RealManAdapter.__init__` positional parameters from `feedback_period`
to:
```python
realtime_push_host_ip: str,
realtime_push_port: int,
realtime_push_cycle_ms: int = 5,
```
Validate with stdlib `ipaddress.IPv4Address`:
```python
try:
self._realtime_push_host_ip = str(
ipaddress.IPv4Address(realtime_push_host_ip)
)
except ipaddress.AddressValueError as exc:
raise ValueError("realtime_push_host_ip must be a valid IPv4 address") from exc
if not 1 <= realtime_push_port <= 65535:
raise ValueError("realtime_push_port must be between 1 and 65535")
if realtime_push_cycle_ms <= 0 or realtime_push_cycle_ms % 5 != 0:
raise ValueError("realtime_push_cycle_ms must be a positive multiple of 5")
```
Store the port and cycle, then replace feedback thread members with:
```python
self._feedback_ready = threading.Event()
self._realtime_callback: Any | None = None
self._accept_realtime_feedback = False
self._feedback_fault_logged = False
```
- [ ] **Step 2: Configure callback and UDP push during connect**
Import these SDK symbols inside `connect()` so mock mode stays SDK-free:
```python
from Robotic_Arm.rm_robot_interface import (
RoboticArm,
rm_realtime_arm_state_callback_ptr,
rm_realtime_push_config_t,
rm_thread_mode_e,
)
```
After existing safety and optional initial-pose configuration:
```python
self._feedback_ready.clear()
self._accept_realtime_feedback = True
self._realtime_callback = rm_realtime_arm_state_callback_ptr(
self._on_realtime_arm_state
)
self._arm.rm_realtime_arm_state_call_back(self._realtime_callback)
config = rm_realtime_push_config_t(
self._realtime_push_cycle_ms,
True,
self._realtime_push_port,
0,
self._realtime_push_host_ip,
)
self._check_return(
self._arm.rm_set_realtime_push(config),
"rm_set_realtime_push",
)
if not self._feedback_ready.wait(timeout=2.0):
raise RuntimeError(
"RealMan UDP realtime feedback did not receive a valid frame within 2 seconds"
)
```
Wrap post-handle initialization so any exception disables callback acceptance,
deletes the handle, sets `_arm = None`, and re-raises.
- [ ] **Step 3: Implement the bounded callback**
Replace `_feedback_loop()` and `_read_joint_state_once()` with:
```python
def _on_realtime_arm_state(self, data: Any) -> None:
if not self._accept_realtime_feedback:
return
try:
if data is None or int(data.errCode) != 0:
raise ValueError("invalid realtime feedback error code")
arm_ip = data.arm_ip
if isinstance(arm_ip, bytes):
arm_ip = arm_ip.decode("utf-8").split("\x00", 1)[0]
if str(arm_ip) != self._robot_ip:
raise ValueError(f"unexpected realtime feedback source: {arm_ip}")
degrees = list(data.joint_status.joint_position)
if (
len(degrees) != 7
or not all(isinstance(value, Number) for value in degrees)
):
raise ValueError("RM75 UDP feedback must contain 7 numeric joints")
positions = [math.radians(float(value)) for value in degrees]
if not all(math.isfinite(value) for value in positions):
raise ValueError("RM75 UDP feedback contains NaN/Inf")
received_at = time.monotonic()
with self._joint_state_lock:
update_interval_ms = (
None
if self._latest_joint_state is None
else (received_at - self._latest_joint_state.received_at) * 1000.0
)
self._latest_joint_state = JointStateSnapshot(
positions,
received_at,
None,
update_interval_ms,
)
self._feedback_fault_logged = False
self._feedback_ready.set()
except Exception as exc:
if not self._feedback_fault_logged:
self._log_warn(f"RealMan UDP realtime feedback invalid: {exc}")
self._feedback_fault_logged = True
```
In `close()`, set `_accept_realtime_feedback = False` before slow-stop and
handle deletion. Remove feedback thread stop/join logic. Keep the callback
reference alive until after `rm_delete_robot_arm()`.
- [ ] **Step 4: Declare and pass ROS parameters**
In `SingleArmVelocityTeleop`, declare:
```python
self.declare_parameter("realtime_push_host_ip", "")
self.declare_parameter("realtime_push_port", 0)
self.declare_parameter("realtime_push_cycle_ms", 5)
```
Replace `feedback_period=self._dt` in `_make_adapter()` with:
```python
realtime_push_host_ip=str(
self.get_parameter("realtime_push_host_ip").value
),
realtime_push_port=int(
self.get_parameter("realtime_push_port").value
),
realtime_push_cycle_ms=int(
self.get_parameter("realtime_push_cycle_ms").value
),
```
Update all direct `RealManAdapter(...)` calls in tests to pass a host and port.
- [ ] **Step 5: Run focused tests and verify GREEN**
Run:
```bash
cd /home/robot/WS_xr
source /opt/ros/humble/setup.bash
python3 -m pytest -q src/xr_rm_teleop/test/test_initial_joint_pose.py
```
Expected: all focused tests pass, with no real SDK connection.
---
### Task 3: Move robot defaults into YAML and simplify launch
**Files:**
- Modify: `xr_rm_bringup/config/right_arm_rm75.yaml`
- Modify: `xr_rm_bringup/config/left_arm_rm75.yaml`
- Modify: `xr_rm_bringup/config/dual_arm_rm75.yaml`
- Modify: `xr_rm_bringup/launch/arm_debug.launch.py`
- [ ] **Step 1: Run a failing ownership assertion**
Run a one-off Python assertion that requires the three YAMLs to contain UDP
and tool parameters, and requires launch not to declare robot behavior
arguments:
```python
from pathlib import Path
import yaml
config_dir = Path("xr_rm_bringup/config")
for name in ("left_arm_rm75.yaml", "right_arm_rm75.yaml"):
params = yaml.safe_load((config_dir / name).read_text())
params = params["single_arm_velocity_teleop"]["ros__parameters"]
assert "use_mock" not in params
assert params["realtime_push_host_ip"] == "192.168.192.148"
assert params["realtime_push_cycle_ms"] == 5
assert params["enable_tool_control"] is True
source = Path("xr_rm_bringup/launch/arm_debug.launch.py").read_text()
for name in (
"left_robot_ip",
"right_robot_ip",
"robot_port",
"avoid_singularity",
"control_rate_hz",
"follow",
"configure_safety_limits",
"move_to_initial_pose_on_connect",
):
assert f'DeclareLaunchArgument("{name}"' not in source
```
Expected: FAIL because the YAML parameters are missing and launch still
declares overrides.
- [ ] **Step 2: Update all YAML nodes**
Remove `use_mock`. Add:
```yaml
realtime_push_host_ip: 192.168.192.148
realtime_push_cycle_ms: 5
enable_tool_control: true
enable_trigger_gripper_control: true
trigger_close_threshold: 0.95
configure_peripheral_on_connect: true
```
Use `realtime_push_port: 8089` for left-arm nodes and `8090` for right-arm
nodes. Keep:
```yaml
# all single-arm and dual-arm nodes
follow: false
canfd_trajectory_mode: 2
```
The right-arm high-follow default was reverted after the first hardware test
exposed an unplanned stationary null-space trajectory. Do not change speeds,
workspace limits, timeouts, safety limits, or initial pose defaults.
- [ ] **Step 3: Reduce launch overrides**
Make `_single_arm_node(arm, use_mock)` and `_dual_arm_nodes(use_mock)` load
their YAML first, then pass only:
```python
{
"use_mock": use_mock,
"robot_urdf_path": _rm75_urdf(),
"peripheral_config_file": _config_file("peripherals_rm75.yaml"),
"peripheral_arm": arm,
"tool_command_topic": f"/xr_rm/{_arm_name(arm)}/tool_enable",
}
```
Keep equivalent per-side generated values in dual mode. Remove
`_initial_pose_override`, robot IP/port, avoid-singularity, control-rate,
follow, safety, tool-control and initial-pose parsing from `_launch_setup`.
Keep only these launch arguments:
```python
DeclareLaunchArgument("arm", default_value="right")
DeclareLaunchArgument("use_mock", default_value="true")
DeclareLaunchArgument("udp_host", default_value="0.0.0.0")
DeclareLaunchArgument("udp_port", default_value="15000")
DeclareLaunchArgument("udp_timer_hz", default_value="200.0")
```
- [ ] **Step 4: Re-run ownership assertion and inspect launch arguments**
Run the assertion from Step 1, then:
```bash
cd /home/robot/WS_xr
source /opt/ros/humble/setup.bash
source install/setup.bash
ros2 launch xr_rm_bringup arm_debug.launch.py --show-args
```
Expected: the assertion passes; launch lists only `arm`, `use_mock`,
`udp_host`, `udp_port`, and `udp_timer_hz`.
---
### Task 4: Synchronize launcher UI and README
**Files:**
- Modify: `xr_rm_bringup/tools/launcher_ui.py`
- Modify: `README.md`
- [ ] **Step 1: Remove deleted launch arguments from UI commands**
Keep ping targets unchanged. Change real launch commands to:
```python
"ros2 launch xr_rm_bringup arm_debug.launch.py arm:=left use_mock:=false"
"ros2 launch xr_rm_bringup arm_debug.launch.py arm:=right use_mock:=false"
"ros2 launch xr_rm_bringup arm_debug.launch.py arm:=both use_mock:=false"
```
- [ ] **Step 2: Update README ownership and commands**
Remove examples and launch-argument descriptions for robot IP/port,
avoid-singularity, control-rate, follow, safety/tool flags, and initial-pose
overrides. State that these values live in the selected YAML. Add the UDP
feedback parameters, host `192.168.192.148`, ports `8089/8090`, 5 ms cycle,
and the command used after Wi-Fi changes:
```bash
ip -4 route get 192.168.192.19
```
Keep `arm`, `use_mock`, and PICO UDP arguments documented as launch
arguments. Keep the warning that checked-in default `use_mock=true` prevents
an accidental real connection.
- [ ] **Step 3: Check syntax and stale references**
Run:
```bash
python3 -m py_compile \
xr_rm_bringup/launch/arm_debug.launch.py \
xr_rm_bringup/tools/launcher_ui.py
rg -n "left_robot_ip:=|right_robot_ip:=|move_to_initial_pose_on_connect:=" \
README.md xr_rm_bringup/tools/launcher_ui.py
```
Expected: compilation passes; `rg` returns no stale command-line overrides.
---
### Task 5: Full verification
**Files:**
- Verify all files changed by Tasks 1–4
- [ ] **Step 1: Run teleop tests**
```bash
cd /home/robot/WS_xr
source /opt/ros/humble/setup.bash
python3 -m pytest -q src/xr_rm_teleop/test
python3 -m pytest -q src/xr_rm_teleop/test/test_orientation_control.py
```
Expected: all tests pass.
- [ ] **Step 2: Build all workspace packages**
```bash
cd /home/robot/WS_xr
source /opt/ros/humble/setup.bash
colcon build --symlink-install --executor sequential
```
Expected: `xr_rm_interfaces`, `xr_rm_input`, `xr_rm_teleop`, and
`xr_rm_bringup` all finish successfully.
- [ ] **Step 3: Verify mock launch without vendor hardware**
```bash
cd /home/robot/WS_xr
source /opt/ros/humble/setup.bash
source install/setup.bash
timeout 8s ros2 launch xr_rm_bringup arm_debug.launch.py \
arm:=right use_mock:=true
```
Expected: the mock teleop and UDP input nodes start; timeout ends the launch.
No RealMan SDK connection is attempted.
- [ ] **Step 4: Inspect final diff**
```bash
cd /home/robot/WS_xr/src
git diff --check
git status --short
git diff --stat
```
Expected: no whitespace errors and no unrelated files. Do not commit, push,
or connect to the real robot unless the user explicitly requests it.
docs/superpowers/plans/2026-07-29-rm75-high-follow-yaml-defaults.md
-42
View File
@@ -1,42 +0,0 @@
# RM75 Right-Arm High-Follow YAML Defaults 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:** Make right-arm single-arm debugging default to RealMan high-follow complete passthrough for phase-two testing.
**Architecture:** Change only the existing right-arm YAML parameters. Keep launch, left-arm, dual-arm, speed limits, safety limits, timeouts, and stop behavior unchanged.
**Tech Stack:** ROS2 Humble, YAML, pytest, colcon
---
### Task 1: Change right-arm CANFD defaults
**Files:**
- Modify: `xr_rm_bringup/config/right_arm_rm75.yaml`
- [ ] **Step 1: Run a failing configuration assertion**
Run a Python YAML assertion requiring `follow is True` and
`canfd_trajectory_mode == 0`.
Expected: FAIL because the current values are `false` and `2`.
- [ ] **Step 2: Apply the minimal configuration change**
```yaml
follow: true
canfd_trajectory_mode: 0
```
- [ ] **Step 3: Verify configuration and regressions**
Run the same YAML assertion and expect PASS. Then run:
```bash
source /opt/ros/humble/setup.bash
python3 -m pytest -q src/xr_rm_teleop/test
colcon build --symlink-install --executor sequential
```
Expected: all tests and all four workspace packages pass.
docs/superpowers/specs/2026-07-27-rm75-placo-qp-ik-design.md
-284
View File
@@ -1,284 +0,0 @@
# RM75 Placo 单步 QP 逆解设计
日期：2026-07-27
状态：已批准，等待实施
## 1. 目标
将 `single_arm_velocity_teleop` 当前通过 `rm_movep_canfd` 调用睿尔曼控制器内部逆解的链路，替换为独立的 Placo QP 逆解：
1. 每个 90 Hz 控制周期读取最新实际关节角。
2. 将本周期工具 TCP 目标交给 Placo。
3. 每周期只调用一次 `solver.solve(True)`。
4. 得到 7 个目标关节角后，通过 `rm_movej_canfd(..., follow=False)` 控制 RM75。
实现借鉴 XRoboToolkit 真机示例的控制理念，但不依赖或复制 XRoboToolkit 项目代码。首轮分别独立验证左臂和右臂 RM75；实现本身继续支持现有左臂、右臂和双臂启动方式。
## 2. 不在本次范围内
- 不采用上传示例中的 Pinocchio + OSQP 多轮迭代逆解实现。
- 不新增独立 ROS2 QP 求解节点。
- 不改变 XR 输入协议、控制器话题或左右臂节点名。
- 不关闭现有工作空间、圆柱、速度、超时或安全停止逻辑。
- 不由 Codex 连接或移动真实机械臂、夹爪。
- 不在本轮验证左右臂同时运行的双臂真机模式。
## 3. 选定方案
在 `xr_rm_teleop` 中新增轻量 `PlacoIkSolver`，由每个 `single_arm_velocity_teleop` 进程持有一个实例：
- 遥操作节点继续负责 XR 相对位姿、滤波、死区、工作空间和速度限制。
- `PlacoIkSolver` 负责 RM75 模型、工具 TCP/法兰变换以及单步 QP。
- `RealManAdapter` 负责唯一的厂商 SDK 连接、关节反馈缓存、关节目标下发、安全停止和工具控制。
- `MockRealManAdapter` 提供同一套关节反馈和关节目标接口，不导入厂商 SDK。
没有选择以下方案：
- 将 Placo 逻辑继续堆入已有的大型遥操作节点：改动集中，但职责更混乱且难以独立测试。
- 增加独立 QP ROS2 节点：隔离更强，但引入额外话题、时序和状态同步，对当前单机 90 Hz 控制没有必要。
## 4. 启动与连接生命周期
`launcher_ui.py` 不直接创建 Adapter。启动链路为：
```text
launcher_ui.py
-> arm_debug.launch.py (arm:=left/right/both)
-> 对应 single_arm_velocity_teleop 节点
-> 节点内部创建 PlacoIkSolver 和 Adapter
```
具体行为：
- `arm:=left use_mock:=false`：一个左臂节点、一个求解器、一个左臂 RealMan 连接。
- `arm:=right use_mock:=false`：一个右臂节点、一个求解器、一个右臂 RealMan 连接。
- `arm:=both use_mock:=false`：左右节点各自持有一个求解器，并各自连接对应 IP。
- `use_mock:=true`：创建 `MockRealManAdapter`，不加载厂商 SDK，不建立真机连接。
每个单臂节点只调用一次 `rm_create_robot_arm`。关节反馈、`rm_movej_canfd`、慢停止和工具控制复用同一个 SDK 句柄，不为反馈建立第二条连接，也不让一条连接控制两台机械臂。
## 5. RM75 模型与关节约束
将上传文件中的 `RM75-B.urdf` 及其网格作为 `xr_rm_teleop` 包资源安装，不携带上传示例的 Pinocchio、OSQP 或仿真控制代码。
模型约定：
- 固定基座：`base_link`。
- 运动关节：按 `joint_1` 到 `joint_7` 顺序映射 SDK 的 7 个关节角。
- 末端法兰帧：`link_7`。
- 节点和 Placo 内部统一使用弧度；Adapter 在 SDK 反馈/指令边界完成度与弧度转换。
Placo 启用 URDF 关节位置和速度限制。上传 URDF 中的位置范围与睿尔曼官方 RM75-B 范围一致：
```text
J1 ±178°, J2 ±130°, J3 ±178°, J4 ±135°,
J5 ±178°, J6 ±128°, J7 ±360°
```
旧的、当前未被调用的 `fun_peripheral.alg_init()` 自定义限位不作为 QP 限位来源。控制器侧现有 `configure_safety_limits`、关节最大速度和最大加速度设置继续保留。
参考：
- [睿尔曼 RM75-B 本体参数](https://develop.realman-robotics.com/robot/robotParameter/RM75OntologyParameters/)
- [XRoboToolkit DualArmURController](https://github.com/XR-Robotics/XRoboToolkit-Teleop-Sample-Python/blob/main/xrobotoolkit_teleop/hardware/dual_arm_ur_controller.py)
## 6. 工具 TCP 处理
URDF 只描述到 `link_7`，实际工具来自 `peripherals_rm75.yaml`。同一份工具配置有两个使用者：
```text
peripherals_rm75.yaml
├─ RealManAdapter：设置真实控制器工具坐标系和负载
└─ PlacoIkSolver：构造法兰到工具 TCP 的固定变换
```
当前选择为：
- 左臂 `scissorgripper: 2`：`minisci`，局部 Z 偏移 `+0.19 m`。
- 右臂 `scissorgripper: 1`：`omnipic`，局部 Z 偏移 `+0.16 m`。
实现读取完整的 `[x, y, z, qx, qy, qz, qw]`，不硬编码为世界坐标 Z 偏移。设：
- `B_T_F(q)`：Placo 由关节角计算的基座到法兰变换。
- `F_T_T`：YAML 给出的法兰到工具 TCP 固定变换。
- `B_T_T_target`：经过现有安全和速度限制后的目标工具 TCP。
正解和目标换算为：
```text
B_T_T(q) = B_T_F(q) * F_T_T
B_T_F_target = B_T_T_target * inverse(F_T_T)
```
`F_T_T` 及其逆矩阵在启动时预计算。每周期只执行少量固定尺寸矩阵运算，不重新读取 YAML、求逆或加载 URDF。工作空间、圆柱限制、调试位姿和误差验收均以工具 TCP 为准；只有 Placo frame task 使用换算后的法兰目标。
## 7. Placo 求解器
每个求解器包含：
- 一个 `placo.RobotWrapper`。Placo 0.9.4 会为模型加入 7 个虚拟浮动基座状态，
因此 `robot.state.q` 长度为 14，真实 RM75 关节固定映射为
`robot.state.q[7:14]`。
- 一个 `placo.KinematicsSolver`，`dt = 1 / control_rate_hz`。
- 一个作用于 `link_7` 的软约束完整位姿任务。
- 一个可操作度任务。
- 一个动能正则项。
- 启用的关节位置与速度限制。
RM75 基座实际固定，创建求解器后必须调用 `solver.mask_fbase(True)`，禁止 QP
通过移动虚拟基座减小末端误差。所有状态同步和结果提取只读写
`robot.state.q[7:14]`。
初始权重沿用 XR 真机示例的最小配置：
```text
frame task: soft, 1.0
manipulability task: soft, 5e-2
kinetic energy regularizer: 1e-6
```
每个周期先用实际关节反馈覆盖 Placo 状态并更新运动学，再设置法兰目标，最后只调用一次 `solver.solve(True)`。这里的“一步”指一次外层 Placo 求解调用；QP 求解器完成该次优化所需的内部数值迭代不算额外控制周期。
Placo 0.9.4 在 RM75 全零 neutral 位形下会出现 QP `NaN`；左右臂现有实际
初始关节角的一步求解均能得到 7 个有限结果。因此全零位形不作为启动状态或
健康检查，必须等待首帧实际关节反馈后才能启用 QP。
## 8. 90 Hz 数据流
```text
XR 相对位姿
-> 现有死区、滤波、工作空间/圆柱限制
-> 现有线速度和角速度单周期限制
-> 目标工具 TCP
-> 换算目标法兰位姿
-> 读取 Adapter 最新实际关节角
-> 同步 Placo 状态
-> solver.solve(True) 一次
-> 校验 7 个目标关节角
-> rad 转 deg
-> rm_movej_canfd(..., follow=False)
```
`RealManAdapter` 连接后在后台连续调用 `rm_get_joint_degree()`，把最新 7 关节角和单调时钟时间戳存入线程安全缓存。控制定时器只复制缓存，不在 90 Hz 回调中等待关节查询。缓存锁只保护内存数据，不包围网络调用。
第一次有效反馈到达前不调用 `solver.solve(True)`，也不发送运动命令。首帧必须
包含 7 个有限关节角且未过期；收到后将度转换为弧度写入
`robot.state.q[7:14]`，更新运动学，把当前工具 TCP 设为初始目标，并以实际
关节角初始化 `last_valid_joint_target`。Mock 模式使用现有
`initial_joint_pose` 初始化 7 关节状态并立即提供同样的首帧有效反馈，再通过
同一 Placo 正解计算工具 TCP；原先仅用于笛卡尔 mock 的
`mock_initial_pose` 随旧控制链路移除。
原 `rm_movep_canfd` 不再位于遥操作运动链路中。
## 9. 异常与停止策略
启动时先校验 Placo、URDF、关节顺序和工具配置，成功后才连接真机。运行时分为两类异常。
### 9.1 沿用 XR 的 last-known-good 策略
第一帧有效关节反馈到达后，用实际关节角初始化 `last_valid_joint_target`。
- QP 成功且输出通过校验：更新并发送新的 `last_valid_joint_target`。
- QP 抛出异常、返回错误维数、`NaN/Inf`，或输出违反关节位置/单周期速度限制：不更新目标，继续发送上一组有效关节目标。
- 下一周期 QP 恢复：自动恢复目标更新，不要求重新按 Grip。
- 求解失败日志限频，避免日志影响控制周期。
不可达目标本身不视为求解异常；软约束任务继续在约束内每周期靠近一步。
### 9.2 输入、反馈或通信不可信时慢停止
以下情况不使用旧关节目标，沿用现有只发送一次慢停止并重置激活状态的逻辑：
- XR 指令超过现有 `command_timeout_sec`。
- Grip 松开。
- 真实关节反馈没有首帧、过期、维数错误或包含 `NaN/Inf`。
- SDK 关节指令发送失败。
- 四元数非法。
- 节点关闭。
关节反馈时效先复用现有 `command_timeout_sec=0.12`，避免增加含义相近的参数。若真机测量证明正常反馈无法稳定满足该阈值，再单独拆分反馈超时参数。
`configure_safety_limits` 保持启用；`move_to_initial_pose_on_connect` 的启动默认值保持 `false`。
## 10. 依赖、Python 环境与配置
- 复用现有 `/home/robot/miniconda3/envs/xr` 环境及其中已经验证的
Placo `0.9.4`、Pin `3.7.0` 和 NumPy `2.2.6`，不新增 XRoboToolkit
项目依赖。
- `arm_debug.launch.py` 明确使用
`/home/robot/miniconda3/envs/xr/bin/python` 启动
`single_arm_velocity_teleop`；ROS2 launch 和 `colcon` 仍使用系统
`/usr/bin/python3`。
- 禁止升级 Placo，禁止向系统 Python、`pip --user` 或其他全局位置安装
Placo、Pinocchio、EigenPy 或 NumPy。构建不改用 Conda Python。
- launch 启动前校验 XR Python 路径存在；不存在时直接报错，不回退到可能
缺少 Placo 或版本不同的系统 Python。
- 真机模式继续按需导入睿尔曼 Python API2。
- Mock 模式依赖 Placo 和 RM75 模型，但不得导入或要求安装睿尔曼 SDK。
- 工具选择继续只由 `peripherals_rm75.yaml` 和现有 `peripheral_arm` 决定。
- 左、右、双臂 YAML 中与 QP 相关的共同配置保持一致；左右现有空间、映射和初始关节角保持各自配置。
- 将左右单臂 YAML 的 `move_to_initial_pose_on_connect` 默认值统一为 `false`，并同步 README；需要自动回初始位姿时必须由用户显式传 `true`。
- 不新增“为以后准备”的插件接口、求解器工厂或额外 ROS 消息。
## 11. 验证与验收
### 11.1 自动验证
- 工具 TCP/法兰变换可往返，包含末端旋转后的局部 Z 偏移。
- RM75 URDF 能加载，且映射顺序严格为 `joint_1` 到 `joint_7`。
- 使用 Placo 0.9.4 时固定虚拟基座，真实关节只映射
`robot.state.q[7:14]`。
- 没有首帧有效关节反馈时不调用 QP、不发送关节目标；首帧到达后用实际关节角
初始化状态和 `last_valid_joint_target`。
- 一次 QP 求解输出 7 个有限关节角并满足位置、单周期速度限制。
- 强制 QP 失败时继续使用上一组有效关节目标。
- 强制反馈过期时执行慢停止。
- Mock 模式不导入睿尔曼 SDK。
- 运行现有姿态控制测试：
```bash
pytest src/xr_rm_teleop/test/test_orientation_control.py
```
- 从工作空间根目录构建：
```bash
source /opt/ros/humble/setup.bash
colcon build --symlink-install
```
### 11.2 左右臂单独 Mock 验收
```bash
ros2 launch xr_rm_bringup arm_debug.launch.py arm:=left use_mock:=true
ros2 launch xr_rm_bringup arm_debug.launch.py arm:=right use_mock:=true
```
左臂和右臂必须分别独立启动并完成相同验收。每个单臂目标停止变化并保持 `0.5 s` 后：
- 工具 TCP 位置误差不超过 `5 mm`。
- 工具 TCP 姿态误差不超过 `2°`。
- 记录 Placo 单次求解耗时和控制周期超时情况。
- 左臂使用 `minisci +0.19 m` 工具变换，右臂使用 `omnipic +0.16 m` 工具变换。
90 Hz 的周期预算约为 `11.1 ms`。性能数据作为验证报告输出，不把易受机器负载影响的耗时阈值写成单元测试硬断言。
### 11.3 左右臂单独真机验收
Codex 分别提供 `launcher_ui.py` 左臂、右臂启动步骤和检查清单，不执行真机连接、运动或夹爪操作。用户在确认急停、障碍物、低速和初始姿态后，先只启动一侧完成验证，停止该侧节点后再验证另一侧。本轮不以 `arm:=both` 进行真机验收。两侧真机首次启动都必须保持 `move_to_initial_pose_on_connect:=false`。
## 12. 完成标准
满足以下条件才视为实现完成：
1. 遥操作运动链路不再调用 `rm_movep_canfd`。
2. 每个有效控制周期只有一次 Placo `solve(True)`。
3. 目标通过 7 个关节角和 `rm_movej_canfd` 下发。
4. 同一机械臂始终只有一个 RealMan SDK 连接。
5. 工具 TCP 偏移参与目标换算、正解和误差验收。
6. QP 失败使用上一组有效目标，输入/反馈/通信失败执行慢停止。
7. 指定构建、测试以及左臂、右臂各自的 mock 验收通过。
8. 分别提供左臂、右臂真机人工验证步骤，但不代替用户执行。
9. 遥操作节点由 launch 显式使用 XR Python 和 Placo 0.9.4，未升级或全局安装
数值依赖。
docs/superpowers/specs/2026-07-28-rm75-control-timing-stats-design.md
-30
View File
@@ -1,30 +0,0 @@
# RM75 控制周期统计设计
## 目标
在不改变控制、QP、安全停止和真机通信行为的前提下，确认激活遥操作时的
125 Hz 控制链路是否满足 `8 ms` 周期。
## 方案
在 `SingleArmVelocityTeleop` 内使用 `time.perf_counter_ns()` 采样，仅在
Grip 激活并执行关节命令的周期记录：
- 相邻控制回调的实际周期；
- 控制回调总执行时间；
- Placo QP 求解时间；
- `send_joint_target()` 调用时间；
- 当前关节反馈年龄。
每累计约 5 秒激活样本，通过现有 ROS logger 输出一次汇总并清空窗口。每项
输出样本数、mean、P95、P99 和 max；实际周期与总执行时间额外输出超过
`self._dt` 的次数。首个激活周期没有可靠的相邻周期值，因此不记录周期。
统计只输出日志，不新增 ROS 消息、话题、参数、依赖或后台线程。计时与日志
异常不得影响控制路径。
## 验证
先添加一个小单元测试，使用确定性样本验证百分位数、超限计数、日志输出和
窗口清空。随后运行相关 pytest、姿态控制测试和
`colcon build --symlink-install`。
docs/superpowers/specs/2026-07-28-rm75-feedback-scheduling-follow-speed-design.md
-131
View File
@@ -1,131 +0,0 @@
# RM75 反馈调度与跟随速度优化设计
## 目标
在保留 Placo 单步 QP、工作空间与圆柱限位、关节速度限制、指令超时和安全
停止的前提下，分阶段解决 PICO 遥操机械臂跟随速度很慢的问题。
每阶段只改变一个控制因素，真机验证通过后才进入下一阶段：
1. 提高关节反馈的新鲜度；
2. 启用 `rm_movej_canfd` 高跟随完全透传；
3. 将右臂单独调试 TCP 速度提高到 `0.2 m/s`。
## 真机证据
右臂在 125 Hz、低跟随模式下连续四个约 5 秒窗口的结果为：
- `period mean=8.000 ms`，四个窗口最大值为 `9.023–9.424 ms`；
- `total mean=1.908–2.004 ms`，最大值不超过 `4.109 ms`；
- `qp mean=0.230–0.245 ms`；
- `send mean=0.122–0.127 ms`；
- `feedback_read mean=9.101–9.630 ms`；
- `feedback_interval mean=17.368–17.918 ms`；
- `feedback_age mean=9.079–9.632 ms`，最差达到 `46.480 ms`。
每个窗口只有 `279–288` 次新反馈，即实际反馈频率约为 `56–58 Hz`。
`feedback_interval - feedback_read` 在四个窗口中稳定为 `8.27–8.39 ms`，
确认当前反馈线程把一次 SDK 查询耗时和完整的 8 ms 等待串联起来：
```text
当前更新间隔 = rm_get_joint_degree 调用耗时 + 8 ms 固定等待
```
控制回调、QP 和发送均有充足余量，不是当前反馈慢的原因。
## 阶段一：反馈线程绝对周期调度
### 调度语义
将当前“读取完成后固定等待 8 ms”改为“读取起始时间之间以 8 ms 为目标”：
```text
读取耗时 < 8 ms：只等待剩余时间
读取耗时 ≥ 8 ms：不再额外等待，从当前时间重新建立周期基准
```
调度不补跑已经错过的历史周期。一次长阻塞结束后最多立即开始下一次读取，
不会为了追赶多个旧截止点而密集补调用 SDK。
等价的目标启动间隔为：
```text
max(8 ms, 本次反馈读取耗时)
```
当前读取平均约 9.3 ms，因此预期反馈频率接近 `100 Hz`，但不强求达到
`125 Hz`。
### 范围
本阶段只修改 `RealManAdapter._feedback_loop()` 的等待计算。以下内容保持不变：
- `follow=false`；
- `canfd_trajectory_mode=2`；
- 右臂单独调试 `max_linear_speed=0.15 m/s`；
- 125 Hz ROS 控制定时器和 Placo QP；
- 同一个 RealMan 连接承担反馈与发送，不新增连接；
- 所有安全限位、超时和停止逻辑。
现有 `feedback_read`、`feedback_interval` 和其他 timing 指标继续保留。
### 验收
右臂真机连续采集四个 timing 窗口，全部满足：
- `feedback_interval mean ≤ 12 ms`；
- `feedback_interval mean - feedback_read mean ≤ 2 ms`；
- `feedback_age mean ≤ 7 ms`；
- `period max < 10 ms`；
- `total max < 8 ms`；
- 无反馈超时、异常停止或 SDK 发送错误。
若更密集的反馈查询使 `period max` 达到或超过 10 ms，或明显增加发送耗时，
停止后续高跟随阶段，继续定位同一 SDK 连接的读写竞争。
## 阶段二：高跟随完全透传
只有阶段一通过后才验证：
- `follow=true`；
- `canfd_trajectory_mode=0`；
- 右臂单独调试速度仍为 `0.15 m/s`。
先通过现有 launch 参数显式启用高跟随完成右臂单机验证；验证通过后，再把
`arm_debug.launch.py` 默认值和 `dual_arm_rm75.yaml`、`left_arm_rm75.yaml`、
`right_arm_rm75.yaml` 同步为高跟随完全透传。
验收条件：
- 快速移动手柄约 10 cm 后，机械臂追赶不超过 1 秒；
- 连续四个 timing 窗口 `period max < 10 ms`；
- 无明显振荡、跳动、反馈超时或异常停止。
若仍追赶超过 1 秒，不进入加速阶段；先增加关节目标与实测关节误差统计，
确认慢速来自 QP 单步目标还是控制器执行。
## 阶段三：右臂速度提高到 0.2 m/s
只有阶段二通过后，将 `right_arm_rm75.yaml` 中右臂单独调试的
`max_linear_speed` 从 `0.15` 提高到 `0.2 m/s`。
- `dual_arm_rm75.yaml` 的左右臂已经是 `0.2 m/s`，无需修改；
- 左臂单独调试速度保持现状；
- 右臂真机 `max_line_speed=0.25 m/s` 安全上限保持不变。
右臂 `scale=0.7` 时，手柄移动 10 cm 对应约 7 cm TCP 目标，理论限速时间约
0.35 秒。验收追赶时间不超过 0.7 秒，并确认没有明显振荡或限位异常。
## 测试与交付
阶段一实现采用测试先行：
- 用确定性时钟和停止事件验证短读取只等待剩余时间；
- 验证读取超期后不额外等待，也不补跑多个历史周期；
- 运行 `xr_rm_teleop` 全部 pytest；
- 运行 `test_orientation_control.py`；
- 在 `/home/robot/WS_xr` 运行 `colcon build --symlink-install`。
Codex 不连接真机、不移动机械臂。每个阶段的真机验证由用户通过
`xr_rm_bringup/launch/arm_debug.launch.py` 在右臂、小范围动作下完成，并把连续
四个完整 timing 窗口返回后再进入下一阶段。
docs/superpowers/specs/2026-07-28-rm75-feedback-thread-timing-design.md
-53
View File
@@ -1,53 +0,0 @@
# RM75 关节反馈线程计时统计设计
## 目标
在不改变关节反馈轮询、QP、关节指令和安全停止行为的前提下，测清当前
RealMan 反馈线程的两个关键时间：
- `rm_get_joint_degree()` 单次调用耗时；
- 相邻两次成功写入关节反馈缓存的实际更新间隔。
本轮只增加统计。高跟随、TCP 速度、反馈调度和 QP 控制方式均保持现状，待
真机日志确认根因后再修改。
## 方案
`RealManAdapter` 在 `_read_joint_state_once()` 中使用单调高精度时钟记录：
- `feedback_read`：从调用 `rm_get_joint_degree()` 前到调用返回后的耗时；
- `feedback_interval`：本次成功反馈时间戳与上次成功反馈时间戳之差。
两个数值随 `JointStateSnapshot` 写入现有线程安全缓存。首次成功反馈没有可靠
的前序时间戳，因此不提供 `feedback_interval`。
`SingleArmVelocityTeleop` 只在看到新的反馈时间戳时，将这两个数值各记录一次，
避免 125 Hz 控制循环重复读取同一缓存而造成重复统计。统计加入现有约 5 秒
timing 窗口，并输出各自的样本数、mean、P95、P99 和 max：
```text
feedback_read[n=<样本数> mean=<均值> p95=<P95> p99=<P99> max=<最大值> ms]
feedback_interval[n=<样本数> mean=<均值> p95=<P95> p99=<P99> max=<最大值> ms]
```
读取失败不产生成功样本，继续沿用现有一次告警、反馈超时和安全停止逻辑。
Mock 模式不伪造厂商 API 调用耗时。
## 验证
- 扩展现有关节控制单元测试，使用确定性快照验证新反馈只统计一次、重复缓存
不重复计数、首次反馈没有更新间隔。
- 运行 `xr_rm_teleop` 相关 pytest。
- 按项目规则在工作空间根目录运行 `colcon build --symlink-install`。
- 真机测试仍由用户使用 `arm_debug.launch.py arm:=right use_mock:=false` 执行；
本轮不自动连接机械臂。
## 后续决策
用户提供真机 timing 日志后再判断：
- 若 `feedback_interval` 主要由 `feedback_read + 8 ms` 构成，再评估绝对周期
调度；
- 若 `feedback_read` 本身经常超过 8 ms，优先定位厂商查询或同一连接的读写
竞争；
- 反馈问题确认前，不把高跟随或预测式 QP 与本轮统计改动混在一起。
docs/superpowers/specs/2026-07-28-rm75-idempotent-tool-frame-design.md
-61
View File
@@ -1,61 +0,0 @@
# RM75 工具坐标系幂等配置设计
## 目标
修复遥操作节点每次启动都无条件创建 RealMan 工具坐标系、忽略重复名称错误，
随后仍误报“外设配置完成”的问题。
本修复只处理控制器工具坐标系的创建、更新、切换和返回值检查，不修改夹爪
IO、Modbus、Placo、URDF 或任何机械臂运动控制参数。
## 根因
右臂 `scissorgripper: 1` 选择 `peripherals_rm75.yaml` 中的 `omnipic`。
`configure_peripheral_on_connect` 默认为 `true`，因此节点每次启动都会进入
`peripheral_cfg()`。
当前实现无条件执行：
```python
robot.rm_set_manual_tool_frame(frame=tool_frame)
robot.rm_change_tool_frame(tool_name)
```
RealMan 控制器会持久保存工具坐标系。首次启动创建成功，后续启动因
`omnipic` 已存在而创建失败。两个返回值均未检查，因此代码继续执行并输出
配置成功日志；若 YAML 中的 TCP、重量或重心已变化，控制器仍可能保留旧值。
## 方案
在 `fun_peripheral.py` 中增加一个小型内部函数，负责单一工具坐标系的幂等
配置：
1. 调用 `rm_get_total_tool_frame()` 获取现有工具坐标系名称并检查
`return_code`。
2. 若目标名称不存在，调用 `rm_set_manual_tool_frame()`。
3. 若目标名称已存在，调用 `rm_update_tool_frame()`。
4. 检查创建或更新返回值。
5. 调用 `rm_change_tool_frame()` 并检查返回值。
任一步失败都抛出包含 SDK 操作名称和返回码的 `RuntimeError`。异常沿现有
节点初始化链路向上传播，因此不会继续误报“外设配置完成”。
不通过“先删除再创建”实现更新，避免在切换中的控制器上产生短暂无工具
坐标系状态。
## 测试
在现有外设相关测试文件中使用 FakeArm 覆盖：
- 名称不存在时只调用创建，然后切换；
- 名称存在时只调用更新，然后切换；
- 查询、创建/更新或切换失败时抛出明确错误。
随后运行：
- `xr_rm_teleop` 全部 pytest；
- `test_orientation_control.py`；
- `/home/robot/WS_xr` 下的 `colcon build --symlink-install`。
Codex 不连接真机。真机验证由用户重新启动右臂 launch，确认不再出现
`Failed to create the tool frame system`，并能看到外设配置完成日志。
docs/superpowers/specs/2026-07-28-rm75-so3-omnipicker-teleop-design.md
-241
View File
@@ -1,241 +0,0 @@
# RM75 SO(3) 姿态跟随与 OmniPicker 一体化模型设计
日期：2026-07-28
状态：已确认
## 1. 背景与目标
当前遥操作链路已经使用 Placo QP 将 TCP 目标转换为 RM75 七关节目标，但姿态目标在进入 QP 前会转换为 RPY，并按三个欧拉角分量进行滤波和限速。RM75 当前姿态接近俯仰角 `±90°` 时，同一个物理姿态可能切换到另一组等价 RPY，导致控制器把很小的手柄旋转解释为大角度路径，出现机械臂绕行和跟随时间过长。
本设计是
`2026-07-27-rm75-placo-qp-ik-design.md`
的增量修改；它覆盖旧设计中的模型、TCP 变换、姿态表示、控制频率和相关验收
内容。现有的关节反馈、单步 QP、`rm_movej_canfd`、last-known-good 和停止链路
继续保留。
本次修改的目标是：
1. 姿态控制全程使用旋转矩阵和 SE(3)，按 SO(3) 最短旋转路径滤波和限速。
2. 左右臂统一使用用户提供的 `RM75-B_OmniPicker_fixed.urdf`。
3. 在 URDF 内定义 `omnipicker_tcp`，取消 Placo 中重复的运行时末端变换。
4. 首轮真机测试继续使用低跟随，将控制频率设为 `125 Hz`，TCP 目标角速度上限统一为 `0.5 rad/s`。
5. 保留现有工作空间、圆柱、关节速度、指令超时和安全停止行为。
## 2. 本次不处理的内容
- 不开启高跟随，`follow` 继续为 `false`。
- 不改变现有 Wi-Fi/有线混合网络拓扑。
- 不新增高跟随周期看门狗。
- 不修改 `avoid_singularity`，不新增奇异点检测或降速策略。
- 首轮不删除或调整现有可操作度优化任务。
- 不修改 `peripherals_rm75.yaml` 中的夹爪选择、工具位姿或负载。
- 不新增 ROS2 节点、消息、求解器工厂或第三方依赖。
- Codex 不连接或移动真实机械臂和夹爪。
## 3. 姿态数据模型
控制路径中的机器人位姿统一为有限的 `4×4` NumPy SE(3) 矩阵：
```text
T = [ R p ]
[ 0 1 ]
```
其中 `R` 为 `3×3` 旋转矩阵，`p` 为 TCP 在机器人基坐标系下的位置。Placo 正解直接返回该矩阵，QP 目标也直接接收该矩阵。现有仅保存 `x/y/z/rx/ry/rz` 的 `ArmPose` 不再作为控制路径接口，避免在遥操作节点与 Placo 之间发生 RPY 往返转换。
ROS 调试边界按消息类型转换：
- `PoseStamped`：旋转矩阵转换为四元数后发布。
- `TwistStamped.angular`：发布相邻两个目标旋转之间、在机器人基坐标系表达的 SO(3) 旋转向量速度。
## 4. XR 姿态到机器人姿态
Grip 按下时保存 XR 初始四元数和当前 `omnipicker_tcp` 初始旋转矩阵。每周期计算：
```text
R_xr_delta = R_xr_now * transpose(R_xr_start)
R_robot_delta = M * R_xr_delta * transpose(M)
R_raw_target = R_robot_delta * R_robot_start
```
`M` 为现有 `xr_to_robot_matrix`，其左右臂映射保持不变。`enable_orientation_axes` 继续保留；被关闭的机器人基坐标轴通过将对应 SO(3) 相对旋转向量分量置零实现，不再通过拼接 RPY 分量实现。
姿态处理统一使用基坐标系下的左乘增量：
```text
R_error = R_target * transpose(R_current)
r = Log(R_error)
R_next = Exp(scale * r) * R_current
```
处理顺序为：
1. 以 `norm(Log(R_target * R_lastᵀ))` 判断 `orientation_deadband_rad`。
2. 以 `orientation_filter_alpha` 缩放从滤波状态到目标的最短旋转向量。
3. 将从上一发送姿态到滤波姿态的旋转角限制在
`max_orientation_speed / control_rate_hz` 以内。
4. 将限速后的旋转和已通过现有工作空间限制的位置合成为 SE(3)。
该路径不使用 RPY 展开、分量插值或分量限速。相对旋转接近 `π` 时也必须选择物理最短路径；四元数 `q` 与 `-q` 必须得到相同目标。
位姿误差采用与 Placo `FrameTask` 一致的解耦 `3+3` 形式：
```text
e_position = p_target - p_current
e_orientation = Log(R_target * transpose(R_current))
e_pose = [e_position; e_orientation]
```
其中 `e_pose` 可视为六维任务误差，但不是
`Log_SE3(T_target * inverse(T_current))`。本次不引入完整 SE(3) 对数映射中的
平移—旋转耦合。
## 5. URDF 与 TCP
用户上传的
`/home/robot/下载/Models/RM75-B_OmniPicker_Pinocchio.zip`
作为模型来源。将 fixed URDF 和被引用的 RM75、OmniPicker mesh 放入
`xr_rm_teleop/models/rm75_omnipicker`，由现有 `xr_rm_teleop` 包安装。
在 `RM75-B_OmniPicker_fixed.urdf` 中增加：
```xml
<link name="omnipicker_tcp"/>
<joint name="omnipicker_tcp_joint" type="fixed">
<parent link="omnipicker_base_link"/>
<child link="omnipicker_tcp"/>
<origin xyz="0 0 0.16" rpy="0 0 0"/>
</joint>
```
该变换表示 TCP 相对 RM75 法兰坐标系沿 `+Z` 方向平移 `0.16 m`、坐标轴方向不变。上传模型中 `rm75_flange` 到 `omnipicker_base_link` 为零固定变换，因此上述定义与已确认的法兰到 TCP 变换一致。
`arm_debug.launch.py` 的左臂、右臂和双臂节点统一加载这一份 fixed URDF。模型仍只有 `joint_1` 至 `joint_7` 七个运动关节，OmniPicker 关节均保持 fixed。
## 6. Placo QP
`PlacoIkSolver` 改为：
```text
构造参数：URDF 路径、dt
当前位姿：get_T_world_frame("omnipicker_tcp")
目标任务：add_frame_task("omnipicker_tcp", target_se3)
```
删除仅供 Placo 使用的 `tool_pose` 构造参数、`_tool_transform`、
`_tool_inverse` 和对应的法兰/TCP换算函数。`peripherals_rm75.yaml` 保持原状，
仍供 `RealManAdapter` 配置真实控制器的工具坐标、负载和末端外设；其中的
`pose` 不再传入 Placo，因此不会在 QP 中重复叠加末端偏移。
QP 配置首轮保持：
```text
frame task: soft, 1.0
manipulability task: soft, 5e-2
kinetic energy regularizer: 1e-6
```
可操作度任务已知会造成目标静止时的关节姿态变化，但按用户决定本轮保留，
待确认 RPY 绕行消失后再单独评估。关节位置和单周期速度校验继续使用 URDF
限制，固定虚拟基座和 `q[7:14]` 的七关节映射保持不变。
## 7. 参数与启动行为
以下配置在 `left_arm_rm75.yaml`、`right_arm_rm75.yaml` 和
`dual_arm_rm75.yaml` 对应节点中同步：
```yaml
control_rate_hz: 125.0
orientation_deadband_rad: 0.005
orientation_filter_alpha: 0.65
max_orientation_speed: 0.5
follow: false
```
`arm_debug.launch.py` 的 `control_rate_hz` 默认值同步为 `125.0`，`follow`
默认值继续为 `false`。`single_arm_velocity_teleop` 内部参数默认值同步，
避免绕过 YAML 启动时回到旧频率或旧角速度。
`right_arm_rm75.yaml` 的 `move_to_initial_pose_on_connect` 从 `True` 改为
`false`，与左臂、双臂以及 launch 默认安全行为一致。其他左右臂空间范围、
线速度、关节速度、加速度、初始关节角和外设配置不变。
## 8. 异常与停止
现有异常策略保持：
- 非法 XR 四元数、Grip 松开、XR 超时、关节反馈过期或通信失败时执行现有慢停止并重置激活状态。
- QP 失败或输出违反关节位置/速度限制时继续使用上一组有效关节目标。
- 第一次有效关节反馈前不发送运动命令。
- `configure_safety_limits` 保持启用。
- Mock 模式不导入睿尔曼 SDK。
新增 SO(3) 运算必须拒绝非有限矩阵和零四元数。旋转矩阵若满足
`norm(RᵀR-I) <= 1e-3` 且行列式为正，则使用 `3×3` SVD 投影到最近的合法旋转；
超出该范围时停止输出，不能把明显无效的输入静默修正成运动目标。
## 9. 验证
### 9.1 自动测试
扩展 `test_orientation_control.py`，至少覆盖：
- 初始姿态俯仰接近 `+90°` 和 `-90°` 时，小手柄旋转仍产生相同量级的最短物理旋转。
- 目标跨越原 RPY 表示分支时，不产生接近 `π` 的错误路径。
- `q` 与 `-q` 产生相同旋转矩阵。
- SO(3) 死区使用整体旋转角。
- 每周期姿态步长不超过 `0.5 / 125 rad`。
- SO(3) 滤波沿最短路径收敛。
- 调试四元数有限且归一化。
更新 Placo 变换测试和 smoke test，验证：
- fixed URDF 可加载，运动关节仍严格为七个且顺序正确。
- `omnipicker_tcp` 相对法兰的变换为 `[0, 0, 0.16]` 和单位旋转。
- 当前/原始目标/发送目标均表示 `omnipicker_tcp`。
- 一次 QP 输出七个有限关节角并满足位置与单周期速度限制。
- 目标静止两秒时记录左右臂最大关节变化，但本轮不以 `≤0.5°` 作为通过条件。
- 目标停止后 TCP 位置误差不超过 `5 mm`、姿态误差不超过 `2°`。
### 9.2 命令
所有命令从 `/home/robot/WS_xr` 执行：
```bash
source /opt/ros/humble/setup.bash
PYTHONPATH=src/xr_rm_teleop pytest -q \
src/xr_rm_teleop/test/test_orientation_control.py
source /opt/ros/humble/setup.bash
colcon build --symlink-install
source /opt/ros/humble/setup.bash
source install/setup.bash
ros2 launch xr_rm_bringup arm_debug.launch.py arm:=left use_mock:=true
ros2 launch xr_rm_bringup arm_debug.launch.py arm:=right use_mock:=true
```
Placo smoke test继续使用已验证的
`/home/robot/miniconda3/envs/xr/bin/python`，周期改为 `1/125 s`。
### 9.3 真机人工验收
Codex 只提供步骤，不执行真机操作。用户应分别测试左右臂：
1. 确认急停可用、周围无障碍物且 `move_to_initial_pose_on_connect=false`。
2. 先保持手柄和目标姿态不变，记录 TCP 与关节变化。
3. 仅改变手柄姿态，重点跨越原俯仰 `±90°` 附近的 RPY 分支。
4. 确认 TCP 以最短物理旋转跟随，没有绕一大圈。
5. 确认 `omnipicker_tcp` 位置保持在允许误差内。
## 10. 完成标准
1. 控制路径中的目标生成、滤波、限速和 QP 接口不再使用 RPY。
2. 左右臂都由同一 fixed URDF 的 `omnipicker_tcp` 作为控制帧。
3. Placo 不再使用 `peripherals_rm75.yaml` 的工具位姿进行矩阵换算。
4. 可操作度任务保持现状，漂移数据被记录但不作为首轮阻断项。
5. 三份配置使用 `125 Hz`、`0.5 rad/s` 和低跟随。
6. 右臂连接时不再自动移动到初始关节姿态。
7. 指定测试、构建和左右臂 mock 启动验证通过。
8. 未改变工作空间、圆柱、关节限制、指令超时、安全停止或奇异点设置。
docs/superpowers/specs/2026-07-29-rm75-canfd-udp-feedback-design.md
-198
View File
@@ -1,198 +0,0 @@
# RM75 CANFD UDP 主动反馈设计
> 真机验证修正：右臂高跟随首轮测试触发掉使能。离线复现发现静止目标存在
> `23.582°` 空空间漂移，第一周期约 `3315°/s²`。当前实现已移除
> manipulability 自运动、增加软件关节加速度限幅和故障 Grip 锁存，并将三份
> YAML 恢复为 `follow: false` 安全基线；高跟随须在基线验证后单独测试。
## 目标
按睿尔曼 MovejCANFD 示例，将真机关节反馈从同一 TCP 控制连接上的
`rm_get_joint_degree()` 周期轮询，替换为控制器 UDP 主动状态推送。
控制命令继续通过现有单个 `RoboticArm(RM_TRIPLE_MODE_E)` 句柄发送，不新增
RealMan 连接，不修改 Placo QP、工作空间/圆柱限位、速度限制、指令超时或安全
停止条件。
## 根因与证据
低跟随模式下，125 Hz CANFD 发送与绝对周期 TCP 反馈轮询能够同时工作：
- `feedback_interval mean=10.041–10.389 ms`；
- `feedback_age mean=5.730–6.138 ms`；
- 控制周期最大值不超过 `8.979 ms`。
启用高跟随后，即使 `canfd_trajectory_mode=2`，控制发送仍正常：
- `period max=9.275 ms`；
- `send max=0.235 ms`。
但同步反馈退化为：
- `feedback_read mean=11.278 ms`、`max=80.320 ms`；
- `feedback_age max=117.502 ms`。
反馈年龄逼近现有 `command_timeout_sec=0.12 s`，触发“关节反馈缺失或过期”
安全停止，造成 Grip 按住期间控制反复退出和重新锁定。增大超时只会允许 QP
继续使用更旧的关节状态，不解决 TCP 反馈阻塞。
睿尔曼 MovejCANFD 示例使用三线程模式、`rm_set_realtime_push()` 和
`rm_realtime_arm_state_call_back()`，通过 UDP 回调获取关节状态，而不是在
CANFD 透传期间同步轮询关节角。
## 数据流
```text
PICO -> ROS 125 Hz 控制回调 -> Placo QP -> TCP rm_movej_canfd
RM75 控制器 -> UDP 5 ms 主动推送 -> SDK 第三线程回调
-> JointStateSnapshot 缓存 -> ROS 125 Hz 控制回调
```
TCP 仍承担 CANFD、慢停、安全配置和末端工具命令。UDP 只承担状态反馈，两条
传输路径共用同一个机械臂句柄。
## SDK 连接与反馈生命周期
`RealManAdapter.connect()` 保持三线程模式和单次 `rm_create_robot_arm()`：
1. 创建并检查机械臂句柄。
2. 下发已有安全参数和可选初始位姿。
3. 创建并保存 `rm_realtime_arm_state_callback_ptr`，避免 Python 回调被垃圾
回收。
4. 使用 `rm_realtime_push_config_t` 配置 5 ms UDP 主动上报。
5. 注册 `rm_realtime_arm_state_call_back()`。
6. 等待第一帧有效 UDP 反馈，最长 2 秒。
若配置接口返回非零，或 2 秒内没有有效反馈，连接初始化失败并删除已创建的
机械臂句柄；不静默回退到 TCP 轮询。
连接成功后不再创建反馈线程，也不再调用 `rm_get_joint_degree()`。
`close()` 先停止接受回调更新，再执行现有慢停和句柄删除。控制器的 UDP 配置
由下一次启动重新覆盖，不额外增加关闭阶段配置命令。
## UDP 回调与缓存
回调只执行有界、非阻塞工作：
1. 检查回调对象、`errCode` 和来源机械臂 IP。
2. 读取 7 个 `joint_position`，检查数量、数值类型及 NaN/Inf。
3. 将厂商反馈的角度转换为弧度。
4. 使用 `time.monotonic()` 记录接收时刻，并计算与上一帧的更新间隔。
5. 在现有 `_joint_state_lock` 下替换 `JointStateSnapshot`。
6. 第一帧有效数据唤醒连接初始化等待。
无效 UDP 帧不覆盖上一帧缓存。若后续持续丢包，现有 120 ms 新鲜度检查自然
触发安全停止。
UDP 回调不执行 QP、ROS 发布、停止命令或其他 SDK 调用，避免阻塞 SDK 接收
线程。
## 参数与三份 YAML
新增真机参数：
- `realtime_push_host_ip`：机械臂可直接访问的上位机地址；
- `realtime_push_port`：单臂 UDP 接收端口；
- `realtime_push_cycle_ms`：主动上报周期，默认并配置为 `5`。
本次现场配置：
| 配置 | 节点 | host | port |
|---|---|---|---:|
| `right_arm_rm75.yaml` | 右臂 | `192.168.192.148` | 8090 |
| `left_arm_rm75.yaml` | 左臂 | `192.168.192.148` | 8089 |
| `dual_arm_rm75.yaml` | 左臂 | `192.168.192.148` | 8089 |
| `dual_arm_rm75.yaml` | 右臂 | `192.168.192.148` | 8090 |
左右臂端口必须不同。更换上位机或网络后，只需同步修改 YAML 中的
`realtime_push_host_ip`。
Mock 模式不导入厂商 SDK，也不要求 UDP 参数有效。
## YAML 与 launch 参数所有权
此前 `arm_debug.launch.py` 会用 launch 默认值覆盖 YAML 中的机械臂参数，
导致 YAML 无法单独控制 `follow` 等行为。
用户选择由 YAML 作为机械臂行为和硬件参数的唯一默认来源。
以下参数只由 `left_arm_rm75.yaml`、`right_arm_rm75.yaml` 和
`dual_arm_rm75.yaml` 管理，launch 不再声明或覆盖：
- `robot_ip`、`robot_port`；
- `avoid_singularity`；
- `control_rate_hz`；
- `follow`、`canfd_trajectory_mode`、`canfd_radio`；
- `configure_safety_limits`；
- `move_to_initial_pose_on_connect`；
- `enable_tool_control`、`enable_trigger_gripper_control`；
- `trigger_close_threshold`；
- `configure_peripheral_on_connect`；
- 本设计新增的 UDP 主动反馈参数。
三份 YAML 补齐工具控制参数；删除其中不再生效的 `use_mock`，避免出现两个
配置来源。
`arm_debug.launch.py` 只保留：
- `arm=left|right|both`，选择启动拓扑；
- `use_mock=true|false`，作为显式安全运行模式，默认仍为 `true`；
- PICO 输入节点的 `udp_host`、`udp_port`、`udp_timer_hz`；
- launch 根据安装路径和左右臂生成的 `robot_urdf_path`、
`peripheral_config_file`、`peripheral_arm` 和 `tool_command_topic`。
`launcher_ui.py` 和 README 中的启动命令同步删除机械臂 IP、初始化移动等已
移交 YAML 的 launch 参数，只保留 `arm`、`use_mock` 和 PICO 输入覆盖。
控制模式保持分阶段范围：
- 三份 YAML 均使用 `follow: false`、`canfd_trajectory_mode: 2` 完成安全基线；
- 基线验证通过前不启用高跟随或模式 0，不提高 `max_linear_speed`。
## Timing 日志
保留：
- `period`；
- `total`；
- `qp`；
- `send`；
- `feedback_age`；
- `feedback_interval`。
`feedback_read` 表示同步 SDK 查询耗时；UDP 架构不存在该查询，因此该字段
不再产生样本，现有条件日志逻辑会自动省略它，不新增同义统计项。
## 测试
使用 FakeArm 和伪造 SDK 模块覆盖：
- 连接时使用正确 host、port、5 ms 周期配置 UDP 并注册回调；
- 第一帧有效回调转换 7 个关节角为弧度并解除启动等待；
- 连续回调正确记录 `feedback_interval`；
- 错误码、来源 IP、长度或 NaN/Inf 无效帧不覆盖缓存；
- UDP 配置失败或首帧超时会清理句柄并抛出明确异常；
- 真机适配器不再启动轮询线程或调用 `rm_get_joint_degree()`；
- Mock 模式无需厂商 SDK。
- launch 不再覆盖 YAML 的机械臂行为和硬件参数；
- `use_mock` 仍由 launch 默认设为 `true`；
- `launcher_ui.py` 不再传递已经删除的 launch 参数。
随后运行：
```bash
cd /home/robot/WS_xr
source /opt/ros/humble/setup.bash
python3 -m pytest -q src/xr_rm_teleop/test
python3 -m pytest -q src/xr_rm_teleop/test/test_orientation_control.py
colcon build --symlink-install --executor sequential
```
Codex 不连接真机。用户在右臂小范围测试中确认：
- 启动日志显示收到 UDP 首帧；
- 按住 Grip 不再出现反馈过期或 SDK `-2`；
- 连续四个 timing 窗口 `period max < 10 ms`、`total max < 8 ms`；
- `feedback_interval mean` 接近 5 ms；
- `feedback_age mean < 5 ms`，且最大值不触发 120 ms 安全停止。