docs: 记录 RM75 QP 与 UDP 修复计划

docs/superpowers/plans/2026-07-30-rm75-joint-command-braking.md

@@ -46,7 +46,7 @@
 - 修改：`xr_rm_teleop/test/test_joint_control.py:206`
 - 测试：`xr_rm_teleop/test/test_joint_control.py`
 
-- [ ] **步骤 1：在现有首周期加速度测试后增加固定目标测试**
+- [x] **步骤 1：在现有首周期加速度测试后增加固定目标测试**
 
 增加以下测试：
 
@@ -90,7 +90,7 @@ def test_joint_command_step_brakes_before_fixed_target_without_overshoot() -> No
 该测试同时覆盖正负方向、不同目标距离、最大速度、最大加速度、禁止越过固定目标
 和最终停止。
 
-- [ ] **步骤 2：运行新增测试并确认失败**
+- [x] **步骤 2：运行新增测试并确认失败**
 
 运行：
 
@@ -113,7 +113,7 @@ PYTHONPATH="/home/robot/WS_xr/src/xr_rm_teleop:${PYTHONPATH:-}" \
   `xr_rm_teleop/xr_rm_teleop/single_arm_velocity_teleop.py:1232-1263`
 - 测试：`xr_rm_teleop/test/test_joint_control.py`
 
-- [ ] **步骤 1：用离散制动逻辑替换现有限幅计算**
+- [x] **步骤 1：用离散制动逻辑替换现有限幅计算**
 
 保留方法签名和现有长度、参数校验，将
 `desired_velocity = np.clip(...)` 到返回值的部分替换为：
@@ -188,7 +188,7 @@ PYTHONPATH="/home/robot/WS_xr/src/xr_rm_teleop:${PYTHONPATH:-}" \
 不要增加 ROS 参数或辅助类。制动距离直接使用当前方法已有的
 `max_acceleration`、`max_speed` 和 `dt`。
 
-- [ ] **步骤 2：运行新增测试并确认通过**
+- [x] **步骤 2：运行新增测试并确认通过**
 
 运行：
 
@@ -202,7 +202,7 @@ PYTHONPATH="/home/robot/WS_xr/src/xr_rm_teleop:${PYTHONPATH:-}" \
 
 预期：`PASS`。
 
-- [ ] **步骤 3：运行关节控制测试文件**
+- [x] **步骤 3：运行关节控制测试文件**
 
 运行：
 
@@ -222,7 +222,7 @@ PYTHONPATH="/home/robot/WS_xr/src/xr_rm_teleop:${PYTHONPATH:-}" \
 
 - 不修改文件。
 
-- [ ] **步骤 1：运行 `xr_rm_teleop` 全部测试**
+- [x] **步骤 1：运行 `xr_rm_teleop` 全部测试**
 
 运行：
 
@@ -230,13 +230,13 @@ PYTHONPATH="/home/robot/WS_xr/src/xr_rm_teleop:${PYTHONPATH:-}" \
 source /opt/ros/humble/setup.bash
 export RM75_TEST_PYTHONPATH="/home/robot/WS_xr/src/xr_rm_teleop:/home/robot/miniconda3/envs/xr/lib/python3.10/site-packages:/home/robot/miniconda3/envs/xr/lib/python3.10/site-packages/cmeel.prefix/lib/python3.10/site-packages"
 PYTHONPATH="${RM75_TEST_PYTHONPATH}:${PYTHONPATH:-}" \
-  /home/robot/miniconda3/envs/xr/bin/python -m pytest \
+  python3 -m pytest \
   src/xr_rm_teleop/test -v
 ```
 
 预期：全部测试通过，无失败；真实 Placo 回归测试必须执行，不能因缺少模块而跳过。
 
-- [ ] **步骤 2：单独运行姿态控制测试**
+- [x] **步骤 2：单独运行姿态控制测试**
 
 运行：
 
@@ -249,7 +249,7 @@ PYTHONPATH="/home/robot/WS_xr/src/xr_rm_teleop:${PYTHONPATH:-}" \
 
 预期：全部通过。
 
-- [ ] **步骤 3：构建工作空间**
+- [x] **步骤 3：构建工作空间**
 
 运行：
 
@@ -261,7 +261,7 @@ colcon build --symlink-install
 预期：`xr_rm_input`、`xr_rm_interfaces`、`xr_rm_teleop` 和 `xr_rm_bringup`
 全部构建成功。
 
-- [ ] **步骤 4：使用 mock 启动右臂统一 launch**
+- [x] **步骤 4：使用 mock 启动右臂统一 launch**
 
 运行：
 
@@ -279,7 +279,7 @@ timeout 10s ros2 launch xr_rm_bringup arm_debug.launch.py \
 - 不连接厂商 SDK，不发送真实 CANFD；
 - 除 `timeout` 主动结束产生的退出状态外，没有 Python 异常或 ROS 错误。
 
-- [ ] **步骤 5：检查最终差异**
+- [x] **步骤 5：检查最终差异**
 
 运行：
 

docs/superpowers/plans/2026-07-30-rm75-qp-udp-feedback-fix.md

@@ -0,0 +1,352 @@
+# RM75 QP 与 UDP 反馈周期修复实施计划
+
+> **供代理执行：** 必须使用 `superpowers:subagent-driven-development`（推荐）或
+> `superpowers:executing-plans` 子技能，按任务逐项实施。步骤使用复选框
+>（`- [ ]`）跟踪。
+
+**目标：** 将 QP 位置收敛阈值调整为 2 mm，并把项目配置的毫秒周期正确换算为
+睿尔曼 SDK 的 5 ms 周期单位，从根因上降低误触发 UDP 反馈超时的概率。
+
+**架构：** 保持 ROS 参数和 YAML 中 `realtime_push_cycle_ms` 的毫秒语义，仅在
+`RealManAdapter.connect()` 的 SDK 边界执行单位换算。QP 只调整现有位置收敛常量；
+反馈超时状态机保持不变，只在首次超时日志中增加实际反馈年龄。
+
+**技术栈：** Python 3.10、ROS2 Humble、pytest、ament/colcon、睿尔曼 Python
+API2、Placo 0.9.4。
+
+---
+
+## 文件范围
+
+- 修改 `xr_rm_teleop/xr_rm_teleop/placo_ik_solver.py`：QP 位置收敛阈值。
+- 修改 `xr_rm_teleop/test/test_placo_transforms.py`：1.5 mm 近收敛结果测试。
+- 修改 `xr_rm_teleop/xr_rm_teleop/realman_adapter.py`：毫秒到 SDK 周期单位换算。
+- 修改 `xr_rm_teleop/test/test_initial_joint_pose.py`：5 ms、10 ms 换算测试。
+- 修改 `xr_rm_teleop/xr_rm_teleop/single_arm_velocity_teleop.py`：超时日志增加年龄。
+- 修改 `xr_rm_teleop/test/test_joint_control.py`：超时年龄日志测试。
+- 不修改 YAML、launch、UI、消息定义或依赖。
+
+## 测试环境
+
+所有命令从工作空间根目录 `/home/robot/WS_xr` 执行：
+
+```bash
+source /opt/ros/humble/setup.bash
+export PYTHONPATH=/home/robot/WS_xr/src/xr_rm_teleop:/home/robot/miniconda3/envs/xr/lib/python3.10/site-packages:/home/robot/miniconda3/envs/xr/lib/python3.10/site-packages/cmeel.prefix/lib/python3.10/site-packages:${PYTHONPATH}
+```
+
+### 任务 1：接受 2 mm 内的 QP 位置残差
+
+**文件：**
+
+- 修改：`xr_rm_teleop/test/test_placo_transforms.py`
+- 修改：`xr_rm_teleop/xr_rm_teleop/placo_ik_solver.py:14`
+
+- [ ] **步骤 1：编写 1.5 mm 近收敛结果的失败测试**
+
+在 `test_placo_transforms.py` 的 QP 测试附近增加：
+
+```python
+def test_qp_solve_accepts_position_error_within_two_millimeters() -> None:
+    solver = object.__new__(PlacoIkSolver)
+    solver._actual_joints = np.zeros(7)
+    solver._robot = SimpleNamespace(
+        state=SimpleNamespace(q=np.zeros(14))
+    )
+    solver._frame_task = SimpleNamespace(T_world_frame=None)
+    solver._target_errors = lambda: (1.5e-3, 0.0)
+
+    result = solver.solve(np.eye(4))
+
+    assert result == pytest.approx([0.0] * 7)
+```
+
+并在文件顶部加入现有标准库类型：
+
+```python
+from types import SimpleNamespace
+```
+
+同时从 `placo_ik_solver` 导入现有收敛常量：
+
+```python
+from xr_rm_teleop.placo_ik_solver import (
+    QP_POSITION_TOLERANCE_M,
+    PlacoIkSolver,
+    _validated_transform,
+)
+```
+
+将真实 Placo 可达目标测试的位置断言改为引用同一收敛常量：
+
+```python
+assert position_error <= QP_POSITION_TOLERANCE_M
+```
+
+再增加超过 2 mm 时仍拒绝结果的边界测试：
+
+```python
+def test_qp_solve_rejects_position_error_above_two_millimeters() -> None:
+    solver = object.__new__(PlacoIkSolver)
+    solver._actual_joints = np.zeros(7)
+    solver._robot = SimpleNamespace(
+        state=SimpleNamespace(q=np.zeros(14)),
+        update_kinematics=lambda: None,
+    )
+    solver._frame_task = SimpleNamespace(T_world_frame=None)
+    solver._solver = SimpleNamespace(solve=lambda update: None)
+    solver._validate_result = lambda result, previous: None
+    solver._target_errors = lambda: (2.1e-3, 0.0)
+
+    with pytest.raises(RuntimeError, match="QP did not converge after 30"):
+        solver.solve(np.eye(4))
+```
+
+- [ ] **步骤 2：运行测试并确认当前实现失败**
+
+```bash
+python3 -m pytest src/xr_rm_teleop/test/test_placo_transforms.py::test_qp_solve_accepts_position_error_within_two_millimeters -q
+```
+
+预期：失败；当前 1 mm 阈值不会直接接收 1.5 mm 残差，测试对象缺少后续 QP
+求解器。
+
+- [ ] **步骤 3：最小修改 QP 位置阈值**
+
+在 `placo_ik_solver.py` 修改现有常量：
+
+```python
+QP_POSITION_TOLERANCE_M = 2e-3
+```
+
+保留 `QP_MAX_ITERATIONS = 30`、姿态阈值、有限值检查、关节位置和速度限制不变。
+
+- [ ] **步骤 4：运行局部测试并确认通过**
+
+```bash
+python3 -m pytest src/xr_rm_teleop/test/test_placo_transforms.py -q
+```
+
+预期：该文件全部测试通过；1.5 mm 残差被接受，2.1 mm 残差仍在 30 次后被拒绝。
+
+- [ ] **步骤 5：提交 QP 修改**
+
+```bash
+git add src/xr_rm_teleop/xr_rm_teleop/placo_ik_solver.py src/xr_rm_teleop/test/test_placo_transforms.py
+git commit -m "fix: 放宽 RM75 QP 位置收敛阈值"
+```
+
+### 任务 2：修正睿尔曼 UDP 实时上报周期单位
+
+**文件：**
+
+- 修改：`xr_rm_teleop/test/test_initial_joint_pose.py`
+- 修改：`xr_rm_teleop/xr_rm_teleop/realman_adapter.py:190-196`
+
+- [ ] **步骤 1：将现有连接测试改为周期换算参数化测试**
+
+将
+`test_connect_configures_udp_feedback_and_waits_for_first_frame` 改为：
+
+```python
+@pytest.mark.parametrize(
+    ("cycle_ms", "sdk_cycle"),
+    [(5, 1), (10, 2)],
+)
+def test_connect_converts_udp_feedback_cycle_to_sdk_units(
+    monkeypatch,
+    cycle_ms,
+    sdk_cycle,
+) -> None:
+    fake_sdk = _install_fake_sdk(monkeypatch)
+    adapter = RealManAdapter(
+        "127.0.0.1",
+        8080,
+        0,
+        "192.168.192.148",
+        8090,
+        realtime_push_cycle_ms=cycle_ms,
+        configure_safety_limits=False,
+    )
+
+    adapter.connect()
+
+    arm = fake_sdk.RoboticArm.instance
+    assert arm is not None
+    assert arm.config.args == (
+        sdk_cycle,
+        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")
+```
+
+- [ ] **步骤 2：运行参数化测试并确认当前实现失败**
+
+```bash
+python3 -m pytest src/xr_rm_teleop/test/test_initial_joint_pose.py::test_connect_converts_udp_feedback_cycle_to_sdk_units -q
+```
+
+预期：两个用例均失败；当前代码把 5 和 10 直接传给 SDK，而预期分别为 1 和 2。
+
+- [ ] **步骤 3：在 SDK 边界执行单位换算**
+
+在 `RealManAdapter.connect()` 创建 `rm_realtime_push_config_t` 时修改第一个参数：
+
+```python
+config = rm_realtime_push_config_t(
+    self._realtime_push_cycle_ms // 5,
+    True,
+    self._realtime_push_port,
+    0,
+    self._realtime_push_host_ip,
+)
+```
+
+构造函数已有“正的 5 ms 整数倍”校验，因此不新增辅助函数或重复校验。启动日志继续
+输出项目参数的真实毫秒值。
+
+- [ ] **步骤 4：运行适配器测试并确认通过**
+
+```bash
+python3 -m pytest src/xr_rm_teleop/test/test_initial_joint_pose.py -q
+```
+
+预期：该文件全部测试通过；mock 测试仍不依赖厂商 SDK。
+
+- [ ] **步骤 5：提交周期换算修改**
+
+```bash
+git add src/xr_rm_teleop/xr_rm_teleop/realman_adapter.py src/xr_rm_teleop/test/test_initial_joint_pose.py
+git commit -m "fix: 修正 RM75 UDP 上报周期单位"
+```
+
+### 任务 3：在 UDP 超时日志中打印反馈年龄
+
+**文件：**
+
+- 修改：`xr_rm_teleop/test/test_joint_control.py`
+- 修改：`xr_rm_teleop/xr_rm_teleop/single_arm_velocity_teleop.py:1036-1044`
+
+- [ ] **步骤 1：扩展现有短暂超时测试，捕获并断言日志**
+
+在
+`test_short_udp_timeout_repeats_last_limited_target_without_query` 中创建遥操对象后加入：
+
+```python
+warnings = []
+teleop.get_logger = lambda: SimpleNamespace(
+    warn=lambda message: warnings.append(message)
+)
+```
+
+在现有断言末尾加入：
+
+```python
+assert warnings == [
+    "right_rm75 UDP关节反馈超时（age=200.0 ms），保持最后安全目标。"
+]
+```
+
+- [ ] **步骤 2：运行测试并确认当前实现失败**
+
+```bash
+python3 -m pytest src/xr_rm_teleop/test/test_joint_control.py::test_short_udp_timeout_repeats_last_limited_target_without_query -q
+```
+
+预期：失败；当前日志中没有 `age=200.0 ms`。
+
+- [ ] **步骤 3：给首次超时警告增加实际反馈年龄**
+
+在 `_handle_stale_joint_feedback()` 中仅修改现有警告：
+
+```python
+self.get_logger().warn(
+    f"{self._arm_name} UDP关节反馈超时"
+    f"（age={age * 1000.0:.1f} ms），保持最后安全目标。"
+)
+```
+
+不改变 `_joint_feedback_ready` 的一次性日志条件、最后安全目标重发、500 ms
+重新同步、Grip 重使能和故障锁存逻辑。
+
+- [ ] **步骤 4：运行关节控制测试并确认通过**
+
+```bash
+python3 -m pytest src/xr_rm_teleop/test/test_joint_control.py -q
+```
+
+预期：该文件全部测试通过。
+
+- [ ] **步骤 5：提交诊断日志修改**
+
+```bash
+git add src/xr_rm_teleop/xr_rm_teleop/single_arm_velocity_teleop.py src/xr_rm_teleop/test/test_joint_control.py
+git commit -m "fix: 补充 RM75 UDP 超时反馈年龄"
+```
+
+### 任务 4：完整回归与 mock 启动验证
+
+**文件：**
+
+- 不修改生产代码。
+
+- [ ] **步骤 1：运行 `xr_rm_teleop` 全部测试**
+
+```bash
+python3 -m pytest src/xr_rm_teleop/test -q
+```
+
+预期：全部通过，无失败或错误。
+
+- [ ] **步骤 2：单独运行姿态控制测试**
+
+```bash
+python3 -m pytest src/xr_rm_teleop/test/test_orientation_control.py -q
+```
+
+预期：全部通过。
+
+- [ ] **步骤 3：构建完整工作空间**
+
+```bash
+colcon build --symlink-install
+```
+
+预期：`xr_rm_interfaces`、`xr_rm_input`、`xr_rm_teleop`、`xr_rm_bringup`
+全部构建成功。
+
+- [ ] **步骤 4：使用右臂 mock 启动统一 launch**
+
+```bash
+timeout 15s ros2 launch xr_rm_bringup arm_debug.launch.py arm:=right use_mock:=true
+```
+
+预期：节点正常启动，无 Python 异常或厂商 SDK 导入错误；因 `timeout` 主动终止，
+命令退出码可以是 124。禁止将 `use_mock` 改为 `false`。
+
+- [ ] **步骤 5：检查最终差异**
+
+```bash
+git status --short
+git diff HEAD~3 --check
+git diff HEAD~3 --stat
+```
+
+预期：除本实施计划文档外，代码差异仅包含计划列出的 6 个代码/测试文件；无空白
+错误，不包含 YAML、launch、UI、依赖或用户其他改动。
+
+## 真机验收
+
+自动验证完成后，由用户在安全条件下启动真机右臂模式并观察：
+
+1. `feedback_interval mean` 从约 25 ms 降到接近 5 ms；
+2. 正常遥操不再频繁触发 `UDP关节反馈超时`；
+3. 反馈确实超过 120 ms 时，日志显示实际 `age`，且仍保持最后安全目标；
+4. 位置残差小于 2 mm 时不再出现 QP 未收敛警告；
+5. 若仍频繁出现超过 120 ms 的中断，保留当前阈值，依据日志继续排查网络、SDK
+   回调或控制器固件。

docs/superpowers/specs/2026-07-30-rm75-qp-udp-feedback-fix-design.md

@@ -0,0 +1,176 @@
+# RM75 QP 阈值与 UDP 反馈周期修复设计
+
+## 背景
+
+右臂真机在 90 Hz、`follow: false` 遥操过程中频繁出现两类警告：
+
+```text
+QP did not converge after 30 iterations:
+position_error=0.001245~0.001609 m
+
+UDP关节反馈超时，保持最后安全目标。
+```
+
+现场 timing 日志同时表明：
+
+- 控制回调最大约 5.1 ms，没有耗尽 11.111 ms 周期；
+- UDP 反馈间隔均值约 25 ms，即实际约 40 Hz；
+- UDP 间隔存在 37~71 ms 的明显抖动；
+- UDP 短暂超时后约 44 ms 收到新帧，但现有安全状态机要求先松开 Grip。
+
+因此，控制线程计算量不是这两类警告的原因。
+
+## 根因
+
+### QP 近阈值失败
+
+当前 QP 最多迭代 30 次，并要求：
+
+```text
+位置误差 <= 1 mm
+姿态误差 <= 0.005 rad
+```
+
+现场失败时姿态误差约 0.0007 rad，已经满足要求；位置误差仅比 1 mm 高
+0.245~0.609 mm。30 次迭代中的每一步已经通过关节有限值、关节位置限制和单步
+速度限制校验，但最终结果仍因严格的 1 mm 判定被整体丢弃。
+
+用户已明确确认 2 mm 位置残差可接受；该数值也与现有 1 mm 手柄位置死区处于
+同一量级。将位置收敛阈值改为 2 mm，可以接收现场这类安全的近收敛
+结果，同时继续拒绝此前出现过的 7.5 mm 等明显未收敛结果。
+
+### UDP 周期单位错误
+
+项目参数 `realtime_push_cycle_ms` 的单位是毫秒，三份机械臂配置均填写 `5`。
+当前适配器把这个值原样传给：
+
+```python
+rm_realtime_push_config_t(cycle, ...)
+```
+
+但睿尔曼 SDK 的 `cycle` 单位不是毫秒，而是 5 ms 的倍数。因此：
+
+```text
+当前传入 cycle=5
+实际周期 = 5 × 5 ms = 25 ms
+实际频率 = 40 Hz
+```
+
+这与现场 `feedback_interval mean≈25 ms` 完全一致。期望 5 ms 上报时，SDK
+参数应为 `cycle=1`。
+
+在 120 ms 反馈超时窗口内，25 ms 上报只有约 5 次发送机会；修正为 5 ms 后有
+约 24 次发送机会，能显著提高对偶发丢包和调度抖动的容忍度。若网络或 SDK
+回调整体停顿超过 120 ms，仍应触发现有安全超时。
+
+## 目标
+
+- QP 位置收敛阈值由 1 mm 调整为 2 mm；
+- `realtime_push_cycle_ms: 5` 实际配置成 SDK `cycle=1`，恢复 5 ms 上报；
+- UDP 超时警告打印触发时的实际反馈年龄；
+- 保持控制频率 90 Hz 和 `follow: false`；
+- 保持 UDP 短超时保持、持续超时重同步、Grip 重使能、CANFD 恢复和故障锁存；
+- 保留工作空间、圆柱、TCP、姿态和关节安全限制。
+
+## 不在本次范围
+
+- 不增加 QP 最大迭代次数；
+- 不修改姿态收敛阈值；
+- 不放宽 `command_timeout_sec=0.12`；
+- 不修改 `feedback_resync_timeout_sec=0.5`；
+- 不允许短超时后自动恢复 Grip；
+- 不修改三份机械臂 YAML；
+- 不修改 UDP 目标 IP、端口、线程模式或增加新连接；
+- 不处理网卡、交换机或控制器固件问题；
+- 不修改用户当前未提交的 `right_arm_rm75.yaml` 参数调整。
+
+## 修改方案
+
+### QP 收敛
+
+在 `placo_ik_solver.py` 中只修改：
+
+```python
+QP_POSITION_TOLERANCE_M = 2e-3
+```
+
+30 次迭代、逐步关节安全校验、姿态阈值和未收敛异常格式保持不变。
+
+### UDP 周期换算
+
+保留公开参数 `realtime_push_cycle_ms` 的毫秒语义和“正数且为 5 ms 倍数”的现有
+校验。构造 SDK 配置时执行：
+
+```python
+sdk_cycle = self._realtime_push_cycle_ms // 5
+```
+
+示例：
+
+| 项目参数 | SDK `cycle` | 实际周期 |
+|---:|---:|---:|
+| 5 ms | 1 | 5 ms |
+| 10 ms | 2 | 10 ms |
+| 25 ms | 5 | 25 ms |
+
+启动日志仍打印毫秒值，避免把 SDK 内部单位暴露为用户配置。
+
+### UDP 超时日志
+
+首次进入短暂超时时打印：
+
+```text
+right_rm75 UDP关节反馈超时（age=xxx.x ms），保持最后安全目标。
+```
+
+日志只增加诊断值，不改变节流、保持目标、QP 停止和 Grip 重使能行为。
+
+## 数据流与安全
+
+正常路径：
+
+```text
+YAML 5 ms
+→ 适配器换算 SDK cycle=1
+→ 控制器约每 5 ms UDP 上报
+→ 回调校验并缓存 joint_position
+→ 90 Hz 控制读取最新实际反馈
+→ 最多 30 次 QP，位置阈值 2 mm
+→ 现有关节提前制动限幅
+→ rm_movej_canfd(..., follow=false)
+```
+
+反馈年龄超过 120 ms 时仍停止生成新目标和 QP，以 90 Hz 重发最后安全目标，并
+要求 Grip 松开后重新使能。达到 500 ms 时仍只尝试一次
+`rm_get_joint_degree()`；失败时仍停止并锁存故障。
+
+## 测试与验证
+
+自动测试：
+
+1. 增加 QP 1.5 mm 位置残差的行为测试，验证其被 2 mm 阈值接受；
+2. 保留明显未收敛结果抛出异常的行为；
+3. 修改 SDK 配置测试，验证项目 5 ms 参数传入 SDK 时为 `cycle=1`；
+4. 增加 10 ms 到 `cycle=2` 的换算覆盖；
+5. 验证 UDP 超时日志包含实际反馈年龄；
+6. 运行 `xr_rm_teleop` 全部测试和姿态控制测试；
+7. 运行 `colcon build --symlink-install`；
+8. 使用 `arm_debug.launch.py arm:=right use_mock:=true`验证启动路径。
+
+真机由用户验证：
+
+- 连续 timing 窗口中 `feedback_interval mean` 从约 25 ms 降到接近 5 ms；
+- 正常遥操不再频繁出现 UDP 超时；
+- 位置残差小于 2 mm 时不再出现 QP 未收敛警告；
+- 真正超过 120 ms 的反馈中断仍打印带 `age` 的警告并执行现有安全保持；
+- 若修正后仍频繁出现超过 120 ms 的中断，再依据 `age` 和间隔数据排查网络、
+  SDK 回调或控制器固件，不继续盲目放宽超时。
+
+## 文件范围
+
+- 修改 `xr_rm_teleop/xr_rm_teleop/placo_ik_solver.py`；
+- 修改 `xr_rm_teleop/xr_rm_teleop/realman_adapter.py`；
+- 修改 `xr_rm_teleop/xr_rm_teleop/single_arm_velocity_teleop.py`；
+- 修改相关现有测试；
+- 新增本中文设计文档和后续中文实施计划；
+- 不修改 YAML、launch、UI 或依赖。

xr_rm_teleop/test/test_initial_joint_pose.py

@@ -412,7 +412,15 @@ def test_udp_fault_and_recovery_are_logged_once_per_transition() -> None:
     assert len([message for message in logger.infos if "恢复正常" in message]) == 1
 
 
-def test_connect_configures_udp_feedback_and_waits_for_first_frame(monkeypatch) -> None:
+@pytest.mark.parametrize(
+    ("cycle_ms", "sdk_cycle"),
+    [(5, 1), (10, 2)],
+)
+def test_connect_converts_udp_feedback_cycle_to_sdk_units(
+    monkeypatch,
+    cycle_ms,
+    sdk_cycle,
+) -> None:
     fake_sdk = _install_fake_sdk(monkeypatch)
     adapter = RealManAdapter(
         "127.0.0.1",
@@ -420,6 +428,7 @@ def test_connect_configures_udp_feedback_and_waits_for_first_frame(monkeypatch)
         0,
         "192.168.192.148",
         8090,
+        realtime_push_cycle_ms=cycle_ms,
         configure_safety_limits=False,
     )
 
@@ -427,7 +436,13 @@ def test_connect_configures_udp_feedback_and_waits_for_first_frame(monkeypatch)
 
     arm = fake_sdk.RoboticArm.instance
     assert arm is not None
-    assert arm.config.args == (5, True, 8090, 0, "192.168.192.148")
+    assert arm.config.args == (
+        sdk_cycle,
+        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")

xr_rm_teleop/test/test_joint_control.py

@@ -136,12 +136,19 @@ def test_short_udp_timeout_repeats_last_limited_target_without_query() -> None:
         stop=lambda: pytest.fail("stop must not run"),
     )
     teleop = _timeout_teleop(adapter)
+    warnings = []
+    teleop.get_logger = lambda: SimpleNamespace(
+        warn=lambda message: warnings.append(message)
+    )
 
     teleop._handle_stale_joint_feedback(0.2)
 
     assert sends == [([0.1] * 7, False)]
     assert teleop._last_joint_command_target == [0.1] * 7
     assert teleop._grip_rearm_required
+    assert warnings == [
+        "right_rm75 UDP关节反馈超时（age=200.0 ms），保持最后安全目标。"
+    ]
 
 
 def test_short_udp_timeout_without_active_target_stays_stopped() -> None:
@@ -218,6 +225,114 @@ def test_joint_command_step_limits_acceleration_from_rest() -> None:
     assert target == pytest.approx([math.radians(0.0192)] * 7)
 
 
+def test_joint_command_step_rejects_non_finite_limits() -> None:
+    for max_speed, max_acceleration, dt in (
+        (math.inf, 1.0, 0.1),
+        (1.0, math.inf, 0.1),
+        (1.0, 1.0, math.inf),
+    ):
+        with pytest.raises(ValueError, match="finite and positive"):
+            SingleArmVelocityTeleop._limit_joint_command_step(
+                target=[0.5] * 7,
+                previous_target=[0.0] * 7,
+                previous_velocity=[0.0] * 7,
+                max_speed=max_speed,
+                max_acceleration=max_acceleration,
+                dt=dt,
+            )
+
+
+def test_joint_command_step_arrival_respects_max_speed() -> None:
+    target, velocity = SingleArmVelocityTeleop._limit_joint_command_step(
+        target=[0.5] * 7,
+        previous_target=[0.0] * 7,
+        previous_velocity=[0.0] * 7,
+        max_speed=1.0,
+        max_acceleration=100.0,
+        dt=0.1,
+    )
+
+    assert velocity == pytest.approx([1.0] * 7)
+    assert target == pytest.approx([0.1] * 7)
+
+
+def test_joint_command_step_reverses_with_acceleration_limit() -> None:
+    command = [0.0] * 7
+    velocity = [0.0] * 7
+    for _ in range(5):
+        command, velocity = SingleArmVelocityTeleop._limit_joint_command_step(
+            target=[1.0] * 7,
+            previous_target=command,
+            previous_velocity=velocity,
+            max_speed=1.0,
+            max_acceleration=1.0,
+            dt=0.1,
+        )
+
+    previous_command = list(command)
+    previous_velocity = list(velocity)
+    command, velocity = SingleArmVelocityTeleop._limit_joint_command_step(
+        target=[-1.0] * 7,
+        previous_target=command,
+        previous_velocity=velocity,
+        max_speed=1.0,
+        max_acceleration=1.0,
+        dt=0.1,
+    )
+
+    assert previous_velocity == pytest.approx([0.5] * 7)
+    assert velocity == pytest.approx([0.4] * 7)
+    assert [
+        current - previous
+        for current, previous in zip(command, previous_command)
+    ] == pytest.approx([value * 0.1 for value in velocity])
+    assert all(
+        current > previous
+        for current, previous in zip(command, previous_command)
+    )
+
+
+def test_joint_command_step_brakes_before_fixed_target_without_overshoot() -> None:
+    dt = 1.0 / 90.0
+    max_speed = math.radians(180.0)
+    max_acceleration = math.radians(300.0)
+    target = np.radians(
+        [10.0, -10.0, 3.0, -3.0, 1.0, -1.0, 0.1]
+    ).tolist()
+    command = [0.0] * 7
+    velocity = [0.0] * 7
+
+    for _ in range(180):
+        previous_command = list(command)
+        previous_velocity = list(velocity)
+        command, velocity = (
+            SingleArmVelocityTeleop._limit_joint_command_step(
+                target=target,
+                previous_target=command,
+                previous_velocity=velocity,
+                max_speed=max_speed,
+                max_acceleration=max_acceleration,
+                dt=dt,
+            )
+        )
+
+        for index in range(7):
+            assert min(0.0, target[index]) - 1e-12 <= command[index]
+            assert command[index] <= max(0.0, target[index]) + 1e-12
+            assert abs(velocity[index]) <= max_speed + 1e-12
+            assert (
+                abs(velocity[index] - previous_velocity[index])
+                <= max_acceleration * dt + 1e-12
+            )
+            assert command[index] - previous_command[index] == pytest.approx(
+                velocity[index] * dt,
+                abs=1e-12,
+            )
+
+    assert command == pytest.approx(target, abs=1e-12)
+    assert velocity == pytest.approx([0.0] * 7, abs=1e-12)
+
+
 def test_feedback_fault_blocks_grip_until_release() -> None:
     class FakeClock:
         def now(self):

xr_rm_teleop/test/test_placo_transforms.py

@@ -1,11 +1,13 @@
 import math
 from pathlib import Path
+from types import SimpleNamespace
 from xml.etree import ElementTree
 
 import numpy as np
 import pytest
 
 from xr_rm_teleop.placo_ik_solver import (
+    QP_POSITION_TOLERANCE_M,
     PlacoIkSolver,
     _validated_transform,
 )
@@ -94,10 +96,40 @@ def test_qp_solve_converges_to_reachable_tcp_target() -> None:
         )
     )
 
-    assert position_error <= 1e-3
+    assert position_error <= QP_POSITION_TOLERANCE_M
     assert orientation_error <= 5e-3
 
 
+def test_qp_solve_accepts_position_error_within_two_millimeters() -> None:
+    solver = object.__new__(PlacoIkSolver)
+    solver._actual_joints = np.zeros(7)
+    solver._robot = SimpleNamespace(
+        state=SimpleNamespace(q=np.zeros(14))
+    )
+    solver._frame_task = SimpleNamespace(T_world_frame=None)
+    solver._target_errors = lambda: (1.5e-3, 0.0)
+
+    result = solver.solve(np.eye(4))
+
+    assert result == pytest.approx([0.0] * 7)
+
+
+def test_qp_solve_rejects_position_error_above_two_millimeters() -> None:
+    solver = object.__new__(PlacoIkSolver)
+    solver._actual_joints = np.zeros(7)
+    solver._robot = SimpleNamespace(
+        state=SimpleNamespace(q=np.zeros(14)),
+        update_kinematics=lambda: None,
+    )
+    solver._frame_task = SimpleNamespace(T_world_frame=None)
+    solver._solver = SimpleNamespace(solve=lambda update: None)
+    solver._validate_result = lambda result, previous: None
+    solver._target_errors = lambda: (2.1e-3, 0.0)
+
+    with pytest.raises(RuntimeError, match="QP did not converge after 30"):
+        solver.solve(np.eye(4))
+
+
 def test_validated_transform_accepts_finite_se3_and_returns_a_copy() -> None:
     transform = np.eye(4)
     transform[:3, 3] = [0.3, -0.1, 0.2]

xr_rm_teleop/xr_rm_teleop/placo_ik_solver.py

@@ -11,7 +11,7 @@ EXPECTED_PLACO_VERSION = "0.9.4"
 RM75_JOINT_NAMES = [f"joint_{index}" for index in range(1, 8)]
 RM75_Q_SLICE = slice(7, 14)
 QP_MAX_ITERATIONS = 30
-QP_POSITION_TOLERANCE_M = 1e-3
+QP_POSITION_TOLERANCE_M = 2e-3
 QP_ORIENTATION_TOLERANCE_RAD = 5e-3
 
 

xr_rm_teleop/xr_rm_teleop/realman_adapter.py

@@ -188,7 +188,7 @@ class RealManAdapter:
                 self._realtime_callback
             )
             config = rm_realtime_push_config_t(
-                self._realtime_push_cycle_ms,
+                self._realtime_push_cycle_ms // 5,
                 True,
                 self._realtime_push_port,
                 0,

xr_rm_teleop/xr_rm_teleop/single_arm_velocity_teleop.py

@@ -1039,7 +1039,8 @@ class SingleArmVelocityTeleop(Node):
         self._grip_rearm_required = True
         if self._joint_feedback_ready:
             self.get_logger().warn(
-                f"{self._arm_name} UDP关节反馈超时，保持最后安全目标。"
+                f"{self._arm_name} UDP关节反馈超时"
+                f"（age={age * 1000.0:.1f} ms），保持最后安全目标。"
             )
         self._joint_feedback_ready = False
 
@@ -1244,23 +1245,51 @@ class SingleArmVelocityTeleop(Node):
             or len(previous_velocity) != 7
         ):
             raise ValueError("joint command state must contain 7 values")
-        if max_speed <= 0.0 or max_acceleration <= 0.0 or dt <= 0.0:
+        if not all(
-            raise ValueError("joint command limits and dt must be positive")
+            math.isfinite(value) and value > 0.0
-        desired_velocity = np.clip(
+            for value in (max_speed, max_acceleration, dt)
-            (np.asarray(target) - np.asarray(previous_target)) / dt,
+        ):
-            -max_speed,
+            raise ValueError("joint command limits and dt must be finite and positive")
-            max_speed,
+        values = np.asarray([target, previous_target, previous_velocity], dtype=float)
-        )
+        if not np.isfinite(values).all():
+            raise ValueError("joint command contains NaN/Inf")
+
         velocity_step = max_acceleration * dt
-        velocity = np.clip(
+        arrival_distance = velocity_step * dt
-            desired_velocity,
+        limited_target = []
-            np.asarray(previous_velocity) - velocity_step,
+        limited_velocity = []
-            np.asarray(previous_velocity) + velocity_step,
+        for desired_target, last_target, last_velocity in zip(
-        )
+            target, previous_target, previous_velocity
-        limited_target = np.asarray(previous_target) + velocity * dt
+        ):
+            error = desired_target - last_target
+            if abs(last_velocity) <= 1e-12 and abs(error) <= arrival_distance:
+                velocity = _clamp(error / dt, -max_speed, max_speed)
+                position = last_target + velocity * dt
+            else:
+                direction = (
+                    math.copysign(1.0, error) if abs(error) > 1e-12 else 0.0
+                )
+                accelerated_speed = min(abs(last_velocity) + velocity_step, max_speed)
+                braking_steps = max(0, math.ceil(accelerated_speed / velocity_step) - 1)
+                braking_distance = accelerated_speed * dt + dt * (
+                    braking_steps * accelerated_speed
+                    - velocity_step * braking_steps * (braking_steps + 1) / 2.0
+                )
+                desired_velocity = direction * max_speed
+                if last_velocity * error > 0.0 and abs(error) <= braking_distance:
+                    desired_velocity = 0.0
+                velocity = _clamp(
+                    desired_velocity,
+                    last_velocity - velocity_step,
+                    last_velocity + velocity_step,
+                )
+                velocity = _clamp(velocity, -max_speed, max_speed)
+                position = last_target + velocity * dt
+            limited_target.append(position)
+            limited_velocity.append(velocity)
         if not np.isfinite(limited_target).all():
             raise ValueError("joint command contains NaN/Inf")
-        return limited_target.tolist(), velocity.tolist()
+        return limited_target, limited_velocity
 
     def _publish_debug(
         self,