docs: 更新手柄输入与 Superpowers 规则

AGENTS.md

@@ -205,6 +205,7 @@
 * 类型标注方式。
 * 注释风格。
 * 测试组织方式。
+* 新生成的 Markdown（`.md`）文档统一使用中文撰写。
 
 如果项目已有 lint、format 或 test 命令，优先使用项目已有命令，不要擅自更换工具链。
 
@@ -229,7 +230,10 @@
 
 除非用户明确要求，否则不要自动提交、推送或修改远程仓库。
 
-使用 Superpowers 执行计划时，允许 subagent 按相关 skill 创建和使用独立 worktree 及其配套本地分支；其他情况下，除非用户明确要求，不要自动创建分支。
+使用 Superpowers 执行任务时，只允许按相关 skill 工作流创建本地 Git 提交；
+禁止执行 `git push`、合并本地分支、合并 PR 或其他远程写操作。相关 skill
+如需独立 worktree 或配套本地分支，可以创建，但不得将其合并到其他分支。
+其他情况下，除非用户明确要求，不要自动创建分支。
 
 如果用户要求生成提交信息，提交信息应：
 

README.md

@@ -24,7 +24,7 @@ PICO/XR 双手柄 UDP JSON
 - Placo 0.9.4 RM75 QP 逆解；收到首帧有效关节反馈后才启用，求解失败时保留上一组有效关节目标。
 - 真机模式下，点击对应手柄 `trigger` 可切换并保持对应夹爪开/关状态。
 - Tkinter 启动面板 `launcher_ui.py`，用于现场快速启动、监控 topic、检查环境和清理进程。
-- 自定义 PICO 4 Ultra UDP Sender Unity 工程，负责发送左右手柄 pose、`grip`、`trigger` 和 pose 诊断字段。
+- XRoboToolkit bridge 读取左右手柄 pose、Grip、Trigger、摇杆和主副按键。
 
 暂未完成：
 
@@ -37,17 +37,8 @@ PICO/XR 双手柄 UDP JSON
 ```text
 src/
 ├── README.md                       # 项目主文档
-├── CODEX.md                        # Codex/Claude Code 项目工作流和安全规则
-├── docs/
-│   └── pico_udp_sender_ubuntu22_setup.md   # Ubuntu 22.04 下 PICO UDP Sender 配置教程
-├── unity/
-│   ├── XR_RM_PICO_UDP_Sender/      # PICO 4 Ultra UDP Sender Unity 工程
-│   │   ├── Assets/Editor/          # Android/PICO 设置与 APK 构建菜单
-│   │   ├── Assets/Scripts/         # UDP sender、配置面板、KeepAwake
-│   │   ├── Assets/Resources/       # PICO 资源与 Roboto TMP 字体
-│   │   ├── Packages/               # Unity package manifest
-│   │   └── ProjectSettings/
-│   └── PICO-Unity-Integration-SDK-release_3.4.0/
+├── AGENTS.md                       # Codex 项目工作流和安全规则
+├── docs/superpowers/               # Superpowers 设计与实施计划
 ├── xr_rm_bringup/
 │   ├── config/
 │   │   ├── dual_arm_rm75.yaml      # 双臂配置：left_arm_teleop 与 right_arm_teleop
@@ -62,12 +53,15 @@ src/
 ├── xr_rm_input/
 │   ├── launch/
 │   │   └── udp_receiver.launch.py  # 低层 UDP 接收测试入口
+│   ├── test/
+│   │   └── test_controller_fields.py
 │   └── xr_rm_input/
 │       ├── udp_controller_receiver.py
+│       ├── xrobotoolkit_to_udp_bridge.py
 │       └── sample_udp_sender.py    # 本机扫轴/正弦模拟手柄 UDP 数据
 ├── xr_rm_interfaces/
 │   └── msg/
-│       └── XrController.msg        # hand/grip/trigger/pose
+│       └── XrController.msg        # 手柄状态与位姿
 └── xr_rm_teleop/
     ├── models/
     │   ├── rm75/                   # 旧 RM75 模型资源（launch 不再选用）
@@ -81,6 +75,12 @@ src/
 
 `single_arm_velocity_teleop` 这个名字保留是有意的：双臂模式不是一个大节点直接控制两台机械臂，而是启动两个相同的单臂控制节点，分别命名为 `left_arm_teleop` 和 `right_arm_teleop`。
 
+## Superpowers Git 约束
+
+使用 Superpowers 执行任务时，只允许按 skill 工作流创建本地 Git 提交。
+禁止执行 `git push`、合并本地分支、合并 PR 或其他远程写操作。skill 如需
+独立 worktree 或配套本地分支，可以创建，但不得将其合并到其他分支。
+
 ## 环境准备
 
 在工作空间根目录，也就是包含 `src/` 的目录执行：
@@ -268,15 +268,14 @@ ros2 launch xr_rm_bringup arm_debug.launch.py arm:=both use_mock:=false
 - 当前三份 YAML 默认均使用 `follow: false` 完成安全基线验证；确认关节加速度与反馈稳定后，再单独测试高跟随。
 - `initial_joint_pose`：mock 的初始关节反馈，以及显式开启初始化移动时的真机初始关节角。
 
-当前 `/xr/*_controller` 的 Project 坐标约定：
+当前 `/xr/*_controller` 的坐标处理：
 
-- Project：`+X` 向右，`+Y` 向上，`+Z` 向后。
-- Unity APK 的 `Project (+Z back)` 会把 PXR `pxr_predict` 原始坐标转换为 `project.x=native.z`、`project.y=native.y`、`project.z=-native.x`。
-- `Source raw` 模式保留原始 pose source 坐标，只用于现场对照。
+- XRoboToolkit bridge 原样转发 SDK 的手柄位置和四元数，不额外转换坐标轴。
+- receiver 默认按 `xyzw` 解析四元数，也可通过 `quat_order:=wxyz` 切换。
 - 左臂映射：机器人位移增量 = `[-手柄y, 手柄z, -手柄x]`。
 - 右臂映射：机器人位移增量 = `[手柄y, 手柄z, 手柄x]`。
 
-如果 `/xr/*_controller.pose.position` 已符合 Project 坐标，但某个机械臂方向相反，只改对应臂的 `xr_to_robot_matrix` 符号，不要同时改多个控制参数。
+如果某个机械臂方向相反，只改对应臂的 `xr_to_robot_matrix` 符号，不要同时改多个控制参数。
 
 ## 末端工具开合
 
@@ -296,7 +295,7 @@ ros2 topic pub --once /xr_rm/right_rm75/tool_enable std_msgs/msg/Bool "{data: fa
 
 ## UDP 数据格式
 
-当前 Unity APK 每个周期发送一个双手柄 JSON 包：
+当前 XRoboToolkit bridge 每个周期发送一个双手柄 JSON 包：
 
 ```json
 {
@@ -306,42 +305,32 @@ ros2 topic pub --once /xr_rm/right_rm75/tool_enable std_msgs/msg/Bool "{data: fa
   "frame_id": "xr_world",
   "controllers": {
     "left": {
+      "hand": "left",
       "grip": true,
       "trigger": 0.0,
+      "axis": [0.2, -0.4],
+      "buttons": {
+        "primary": true,
+        "secondary": false
+      },
       "pos": [-0.12, 1.05, 0.30],
       "quat": [0.0, 0.0, 0.0, 1.0],
       "pose_valid": true,
-      "pose_source": "pxr_predict",
-      "tracking_state": 3,
-      "controller_status": 2,
-      "grip_value": 1.0,
-      "axis": [0.0, 0.0],
-      "buttons": {
-        "grip": true,
-        "primary": false,
-        "secondary": false,
-        "menu": false,
-        "axis_click": false
-      }
+      "pose_source": "xrobotoolkit"
     },
     "right": {
+      "hand": "right",
       "grip": true,
-      "trigger": 0.4,
+      "trigger": 1.0,
+      "axis": [-0.1, 0.3],
+      "buttons": {
+        "primary": false,
+        "secondary": true
+      },
       "pos": [0.12, 1.05, 0.30],
       "quat": [0.0, 0.0, 0.0, 1.0],
       "pose_valid": true,
-      "pose_source": "unity_xr",
-      "tracking_state": 3,
-      "controller_status": -1,
-      "grip_value": 0.8,
-      "axis": [0.0, 0.0],
-      "buttons": {
-        "grip": true,
-        "primary": false,
-        "secondary": false,
-        "menu": false,
-        "axis_click": false
-      }
+      "pose_source": "xrobotoolkit"
     }
   }
 }
@@ -349,21 +338,40 @@ ros2 topic pub --once /xr_rm/right_rm75/tool_enable std_msgs/msg/Bool "{data: fa
 
 字段说明：
 
-- `t` / `source_time`：Unity 端 `Time.realtimeSinceStartupAsDouble`，用于后续延迟分析。
-- `seq`：Unity 端递增包序号，用于后续丢包分析。
+- `t` / `source_time`：bridge 的 PC 单调时间，用于诊断发送周期。
+- `seq`：bridge 递增的 UDP 包序号，bridge 重启后重新计数。
 - `frame_id`：默认 `xr_world`，会写入 `XrController.header.frame_id`。
 - `grip`：运动使能。`true` 时进入相对位姿控制，`false` 时停止。
-- `trigger`：扳机值，范围 `0.0-1.0`。真机模式下跨过 `0.95` 的上升沿会切换对应夹爪开/关状态。
+- `trigger`：经过 bridge 滞回处理的 `0.0/1.0` 值；上升沿切换对应夹爪状态。
+- `axis`：摇杆 `[x, y]`，每个分量限制在 `-1.0` 到 `1.0`。
+- `buttons.primary`：左手 X 键或右手 A 键。
+- `buttons.secondary`：左手 Y 键或右手 B 键。
 - `pos`：手柄位置，长度 3。
-- `quat`：手柄姿态四元数，默认按 `xyzw` 解析；遥操作节点会用 grip 锁定后的相对旋转控制 TCP 姿态。
-- `pose_valid`：姿态是否可信。ROS 接收端看到 `false` 会强制 `grip=false`。
-- `pose_source`：`pxr_predict`、`unity_xr`、`xrobotoolkit` 或 `none`，用于判断姿态来自 PICO 预测接口、Unity XR fallback 还是官方 XRoboToolkit SDK bridge。
-- `tracking_state` / `controller_status`：Unity/PICO 侧追踪诊断值，只用于日志和排查。
-- `grip_value`、`axis`、`buttons`：PICO 端输入诊断字段，当前不会写入 `XrController` 消息。
+- `quat`：手柄姿态四元数，默认按 `xyzw` 解析。
+- `pose_valid`：姿态是否可信；`false` 时接收端强制 `grip=false`。
+- `pose_source`：当前 bridge 使用 `xrobotoolkit`。
 
-`udp_controller_receiver` 仍兼容调试用的单手柄包：可以直接发送带 `hand`、`pos`、`quat` 的 JSON object，也可以用 `controllers` list、顶层 `left/right`、`pose.position`、`position`、`p`、`q` 等常见字段。四元数默认按 `xyzw` 解析，也可通过 `quat_order:=wxyz` 切换。
+`axis`、`buttons.primary` 和 `buttons.secondary` 会进入 `XrController`；旧 UDP
+包缺少这些字段时分别回退为 `[0,0]`、`false` 和 `false`。
 
-PICO 4 Ultra 在 Ubuntu 22.04 下配置 Unity、构建 APK、安装到头显并向 ROS2 主机发送 UDP 的详细步骤见 [docs/pico_udp_sender_ubuntu22_setup.md](docs/pico_udp_sender_ubuntu22_setup.md)。
+接收端发布的消息格式为：
+
+```text
+std_msgs/Header header
+string hand
+
+bool grip
+float32 trigger
+bool primary
+bool secondary
+float32[2] axis
+
+geometry_msgs/Pose pose
+```
+
+`udp_controller_receiver` 仍兼容调试用的单手柄包：可以直接发送带 `hand`、`pos`、
+`quat` 的 JSON object，也可以用 `controllers` list、顶层 `left/right`、
+`pose.position`、`position`、`p`、`q` 等常见字段。
 
 ## 官方 XRoboToolkit bridge
 
@@ -466,7 +474,8 @@ Controller topic 没有数据：
 - 确认 UDP 发送端目标 IP 是运行 ROS2 的主机 IP。
 - 确认端口是 `15000`，或 launch 与发送端端口一致。
 - 用 `sample_udp_sender` 在本机验证接收链路。
-- 如果 Unity HUD 显示某个手柄 `invalid none`，ROS 侧会把该手柄 `grip` 强制置为 `false`。
+- 确认 `xrobotoolkit_to_udp_bridge` 没有持续打印 SDK read failed；SDK
+  读取失败时 bridge 会发送 `pose_valid=false` 的停止包。
 
 机械臂不动：
 

docs/superpowers/plans/2026-07-29-rm75-feedback-recovery.md

@@ -0,0 +1,964 @@
+# RM75 关节反馈与故障恢复实施计划
+
+> **执行要求：** 使用 `superpowers:executing-plans` 按任务逐项实施。只有用户明确授权 subagent 后，才允许使用 `superpowers:subagent-driven-development`、独立 worktree 或本地分支。所有步骤使用复选框跟踪。
+
+**目标：** 启动时用 `rm_get_joint_degree()` 初始化 RM75 QP；运行时以 UDP `joint_position` 作为实际反馈；短暂丢包时保持最后安全目标；持续丢包或 CANFD 错误时安全同步、停止或等待人工重新使能。
+
+**实现方式：** 继续复用现有唯一 `RealManAdapter` 连接，只增加一个同步读取关节角的方法。恢复决策仍放在 `SingleArmVelocityTeleop`，用少量布尔状态复用现有 slow-stop、Grip 重新使能和关节限速逻辑，不新增状态机类、线程、连接或依赖。
+
+**技术栈：** Python 3.10、ROS2 Humble `rclpy`、睿尔曼 Python API2、Placo、pytest、ament/colcon。
+
+**设计文档：** `docs/superpowers/specs/2026-07-29-rm75-feedback-recovery-design.md`
+
+**厂商接口依据：**
+
+- `rm_get_joint_degree() -> tuple[int, list[float]]`：<https://develop.realman-robotics.com/robot/apipython/classes/armState/>
+- `rm_movej_canfd(..., follow=False, ...)` 为低跟随：<https://develop.realman-robotics.com/robot/apipython/classes/movePlan/>
+
+## 仓库约束
+
+- 所有构建、测试和启动命令均在工作空间根目录 `/home/robot/WS_xr` 执行。
+- 所有 Git 命令均在仓库根目录 `/home/robot/WS_xr/src` 执行。
+- 每次 ROS2 构建、测试或启动前先执行 `source /opt/ros/humble/setup.bash`。
+- 不自动提交、推送、创建分支或 worktree；只有用户明确要求后才执行。
+- 验证只通过 `xr_rm_bringup/launch/arm_debug.launch.py use_mock:=true`。
+- 不连接真机，不发送真实 CANFD，不移动机械臂，不操作夹爪或末端外设。
+- 不修改依赖、锁文件、CI、格式化配置、公开入口和无关代码。
+
+## 文件范围
+
+- 修改 `src/xr_rm_teleop/xr_rm_teleop/realman_adapter.py`
+  - 增加同步关节查询。
+  - 复用 UDP 与同步查询的角度校验。
+- 修改 `src/xr_rm_teleop/xr_rm_teleop/single_arm_velocity_teleop.py`
+  - 启动同步、UDP保持/恢复、CANFD恢复和故障锁存。
+- 修改 `src/xr_rm_teleop/test/test_initial_joint_pose.py`
+  - 适配器同步查询测试。
+- 修改 `src/xr_rm_teleop/test/test_joint_control.py`
+  - 启动、超时、恢复和锁存测试。
+- 同步修改：
+  - `src/xr_rm_bringup/config/dual_arm_rm75.yaml`
+  - `src/xr_rm_bringup/config/left_arm_rm75.yaml`
+  - `src/xr_rm_bringup/config/right_arm_rm75.yaml`
+
+---
+
+## 任务一：给现有适配器增加同步关节查询
+
+**修改文件：**
+
+- `src/xr_rm_teleop/test/test_initial_joint_pose.py`
+- `src/xr_rm_teleop/xr_rm_teleop/realman_adapter.py`
+
+- [x] **步骤1：先写失败测试**
+
+在 `test_initial_joint_pose.py` 增加：
+
+```python
+def test_joint_degree_query_returns_validated_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,
+        "127.0.0.1",
+        8090,
+    )
+    adapter._arm = FakeArm()
+
+    snapshot = adapter.read_joint_state()
+
+    assert snapshot.positions == pytest.approx(
+        [math.radians(value) for value in [0, 10, -20, 30, -40, 50, -60]]
+    )
+    assert snapshot.read_duration_ms is not None
+
+
+@pytest.mark.parametrize(
+    "result",
+    [
+        (7, [0.0] * 7),
+        (0, [0.0] * 6),
+        (0, [0.0, 0.0, 0.0, math.nan, 0.0, 0.0, 0.0]),
+    ],
+)
+def test_joint_degree_query_rejects_sdk_errors_and_invalid_values(result) -> None:
+    adapter = RealManAdapter(
+        "127.0.0.1",
+        8080,
+        0,
+        "127.0.0.1",
+        8090,
+    )
+    adapter._arm = SimpleNamespace(rm_get_joint_degree=lambda: result)
+
+    with pytest.raises((RuntimeError, ValueError)):
+        adapter.read_joint_state()
+
+
+def test_mock_joint_query_uses_current_mock_positions() -> None:
+    adapter = MockRealManAdapter([1.0, 2.0, 3.0, 4.0, 5.0, 6.0, 7.0])
+
+    snapshot = adapter.read_joint_state()
+
+    assert snapshot.positions == pytest.approx(
+        [math.radians(value) for value in [1, 2, 3, 4, 5, 6, 7]]
+    )
+```
+
+把现有 `test_invalid_udp_feedback_does_not_replace_snapshot` 的最终断言改为：
+
+```python
+after = adapter.get_latest_joint_state()
+assert after is not None
+assert before is not None
+assert after.positions == before.positions
+assert after.received_at == before.received_at
+assert after.motion_ready is False
+```
+
+该断言要求无效 UDP 帧保留最后已知角度，但立即禁止这些角度继续参与运动。
+
+- [x] **步骤2：运行测试并确认 RED**
+
+```bash
+source /opt/ros/humble/setup.bash
+python3 -m pytest src/xr_rm_teleop/test/test_initial_joint_pose.py \
+  -k 'joint_degree_query or mock_joint_query or invalid_udp_feedback' -v
+```
+
+预期：测试失败；原因是适配器还没有 `read_joint_state()`，且无效 UDP 帧仍被标记为可运动。
+
+- [x] **步骤3：实现最小同步查询**
+
+给 `MockRealManAdapter` 增加：
+
+```python
+def read_joint_state(self) -> JointStateSnapshot:
+    return self.get_latest_joint_state()
+```
+
+给 `RealManAdapter` 增加：
+
+```python
+def read_joint_state(self) -> JointStateSnapshot:
+    self._require_arm()
+    started_at = time.monotonic()
+    result = self._arm.rm_get_joint_degree()
+    finished_at = time.monotonic()
+    if not isinstance(result, tuple) or len(result) != 2:
+        raise RuntimeError(
+            f"rm_get_joint_degree returned invalid result: {result!r}"
+        )
+    self._check_return(result, "rm_get_joint_degree")
+    return JointStateSnapshot(
+        self._joint_positions_from_degrees(
+            result[1],
+            "rm_get_joint_degree",
+        ),
+        finished_at,
+        (finished_at - started_at) * 1000.0,
+    )
+
+@staticmethod
+def _joint_positions_from_degrees(
+    values: Any,
+    source: str,
+) -> list[float]:
+    try:
+        degrees = list(values)
+    except TypeError as exc:
+        raise ValueError(f"{source} must contain 7 numeric joints") from exc
+    if len(degrees) != 7 or not all(
+        isinstance(value, Number) for value in degrees
+    ):
+        raise ValueError(f"{source} 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(f"{source} contains NaN/Inf")
+    return positions
+```
+
+把 UDP 回调中重复的角度转换替换为：
+
+```python
+positions = self._joint_positions_from_degrees(
+    data.joint_status.joint_position,
+    "RM75 UDP feedback",
+)
+```
+
+在 UDP 回调的异常分支中，保留最后角度但标记为不可运动：
+
+```python
+with self._joint_state_lock:
+    if self._latest_joint_state is not None:
+        current = self._latest_joint_state
+        self._latest_joint_state = JointStateSnapshot(
+            list(current.positions),
+            current.received_at,
+            current.read_duration_ms,
+            current.update_interval_ms,
+            False,
+        )
+```
+
+- [x] **步骤4：运行适配器测试并确认 GREEN**
+
+```bash
+source /opt/ros/humble/setup.bash
+python3 -m pytest src/xr_rm_teleop/test/test_initial_joint_pose.py -v
+```
+
+预期：该文件全部测试通过。
+
+- [x] **步骤5：检查本任务差异**
+
+```bash
+git diff --check
+git diff -- \
+  xr_rm_teleop/xr_rm_teleop/realman_adapter.py \
+  xr_rm_teleop/test/test_initial_joint_pose.py
+```
+
+预期：只有同步查询、共用角度校验、无效反馈安全标记及对应测试。
+
+---
+
+## 任务二：用启动查询结果初始化 QP 和关节命令历史
+
+**修改文件：**
+
+- `src/xr_rm_teleop/test/test_joint_control.py`
+- `src/xr_rm_teleop/xr_rm_teleop/single_arm_velocity_teleop.py`
+
+- [x] **步骤1：先写启动同步失败测试**
+
+增加：
+
+```python
+def test_startup_joint_query_initializes_qp_and_command_history() -> None:
+    positions = [0.1] * 7
+    pose = np.eye(4)
+    teleop = object.__new__(SingleArmVelocityTeleop)
+    teleop._arm_name = "right_rm75"
+    teleop._adapter = SimpleNamespace(
+        read_joint_state=lambda: JointStateSnapshot(
+            positions,
+            time.monotonic(),
+        )
+    )
+    teleop._ik_solver = SimpleNamespace(
+        update_joint_state=lambda joints: pose
+    )
+    teleop.get_logger = lambda: FakeLogger()
+
+    teleop._initialize_joint_state()
+
+    assert teleop._latest_joint_positions == positions
+    assert teleop._last_valid_joint_target == positions
+    assert teleop._last_joint_command_target == positions
+    assert teleop._last_joint_command_velocity == [0.0] * 7
+    assert teleop._last_current_pose is pose
+
+
+def test_startup_joint_query_failure_closes_adapter() -> None:
+    class FailingAdapter:
+        def __init__(self):
+            self.close_calls = 0
+
+        def read_joint_state(self):
+            raise RuntimeError("rm_get_joint_degree failed with code 7")
+
+        def close(self):
+            self.close_calls += 1
+
+    errors = []
+    teleop = object.__new__(SingleArmVelocityTeleop)
+    teleop._arm_name = "left_rm75"
+    teleop._adapter = FailingAdapter()
+    teleop.get_logger = lambda: SimpleNamespace(
+        error=lambda message: errors.append(message)
+    )
+
+    with pytest.raises(RuntimeError, match="code 7"):
+        teleop._initialize_joint_state()
+
+    assert teleop._adapter.close_calls == 1
+    assert "left_rm75" in errors[0]
+    assert "启动关节同步失败" in errors[0]
+```
+
+- [x] **步骤2：运行测试并确认 RED**
+
+```bash
+source /opt/ros/humble/setup.bash
+python3 -m pytest src/xr_rm_teleop/test/test_joint_control.py \
+  -k 'startup_joint_query' -v
+```
+
+预期：测试因 `_initialize_joint_state()` 尚不存在而失败。
+
+- [x] **步骤3：增加启动同步**
+
+增加：
+
+```python
+def _initialize_joint_state(self) -> None:
+    try:
+        self._reset_joint_state(self._adapter.read_joint_state())
+    except Exception as exc:
+        self.get_logger().error(
+            f"{self._arm_name} 启动关节同步失败：{exc}"
+        )
+        self._adapter.close()
+        raise
+
+def _reset_joint_state(
+    self,
+    snapshot: JointStateSnapshot,
+) -> np.ndarray:
+    current_pose = self._ik_solver.update_joint_state(snapshot.positions)
+    positions = list(snapshot.positions)
+    self._latest_joint_positions = positions
+    self._last_current_pose = current_pose
+    self._last_valid_joint_target = list(positions)
+    self._last_joint_command_target = list(positions)
+    self._last_joint_command_velocity = [0.0] * 7
+    return current_pose
+```
+
+在现有适配器连接之后、外设初始化之前调用：
+
+```python
+self._adapter = self._make_adapter()
+self._adapter.connect()
+self._initialize_joint_state()
+self._setup_tool_control()
+```
+
+同步查询结果不得写入 `RealManAdapter._latest_joint_state`；该缓存继续只代表 UDP 反馈。
+
+- [x] **步骤4：运行启动与适配器测试**
+
+```bash
+source /opt/ros/humble/setup.bash
+python3 -m pytest src/xr_rm_teleop/test/test_joint_control.py \
+  -k 'startup_joint_query or first_feedback' -v
+python3 -m pytest src/xr_rm_teleop/test/test_initial_joint_pose.py -v
+```
+
+预期：所选控制测试和全部适配器测试通过。
+
+- [x] **步骤5：检查本任务差异**
+
+```bash
+git diff --check
+git diff -- \
+  xr_rm_teleop/xr_rm_teleop/single_arm_velocity_teleop.py \
+  xr_rm_teleop/test/test_joint_control.py
+```
+
+预期：启动阶段只增加一次同步读取，并初始化现有 QP/关节命令字段。
+
+---
+
+## 任务三：UDP 短暂超时保持，持续超时重新同步
+
+**修改文件：**
+
+- `src/xr_rm_teleop/test/test_joint_control.py`
+- `src/xr_rm_teleop/xr_rm_teleop/single_arm_velocity_teleop.py`
+
+- [x] **步骤1：先写超时行为测试**
+
+在测试文件增加最小构造器：
+
+```python
+def _timeout_teleop(adapter) -> SingleArmVelocityTeleop:
+    teleop = object.__new__(SingleArmVelocityTeleop)
+    teleop._adapter = adapter
+    teleop._arm_name = "right_rm75"
+    teleop._follow = False
+    teleop._active = True
+    teleop._joint_feedback_ready = True
+    teleop._grip_rearm_required = False
+    teleop._feedback_resync_attempted = False
+    teleop._control_fault_latched = False
+    teleop._last_joint_command_target = [0.1] * 7
+    teleop._last_joint_command_velocity = [0.0] * 7
+    teleop._latest_joint_positions = [0.1] * 7
+    teleop._last_valid_joint_target = [0.1] * 7
+    teleop._last_current_pose = np.eye(4)
+    teleop._controller_start = None
+    teleop._controller_orientation_start = None
+    teleop._robot_start_transform = None
+    teleop._filtered_target = None
+    teleop._filtered_orientation_target = None
+    teleop._last_sent_target = None
+    teleop._last_sent_orientation = None
+    teleop._last_command_time = None
+    teleop._ik_solver = SimpleNamespace(
+        update_joint_state=lambda joints: np.eye(4)
+    )
+    teleop._stop_sent = False
+    teleop._feedback_resync_timeout_sec = 0.5
+    teleop._publish_stop_debug = lambda: None
+    teleop.get_logger = lambda: FakeLogger()
+    return teleop
+```
+
+增加：
+
+```python
+def test_missing_or_disabled_joint_snapshot_is_not_motion_ready() -> None:
+    assert not SingleArmVelocityTeleop._joint_snapshot_is_motion_ready(None)
+    assert not SingleArmVelocityTeleop._joint_snapshot_is_motion_ready(
+        JointStateSnapshot(
+            [0.0] * 7,
+            time.monotonic(),
+            motion_ready=False,
+        )
+    )
+
+
+def test_short_udp_timeout_repeats_last_limited_target_without_query() -> None:
+    sends = []
+    adapter = SimpleNamespace(
+        send_joint_target=lambda joints, follow: sends.append(
+            (list(joints), follow)
+        ),
+        read_joint_state=lambda: pytest.fail("query must not run"),
+        stop=lambda: pytest.fail("stop must not run"),
+    )
+    teleop = _timeout_teleop(adapter)
+
+    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
+
+
+def test_short_udp_timeout_without_active_target_stays_stopped() -> None:
+    stop_calls = []
+    adapter = SimpleNamespace(
+        send_joint_target=lambda joints, follow: pytest.fail(
+            "inactive control must not start CANFD output"
+        ),
+        read_joint_state=lambda: pytest.fail("query must not run"),
+        stop=lambda: stop_calls.append(True),
+    )
+    teleop = _timeout_teleop(adapter)
+    teleop._active = False
+
+    teleop._handle_stale_joint_feedback(0.2)
+
+    assert len(stop_calls) == 1
+
+
+def test_persistent_udp_timeout_queries_once_and_holds_actual_position() -> None:
+    sends = []
+    query_calls = []
+    adapter = SimpleNamespace(
+        send_joint_target=lambda joints, follow: sends.append(list(joints)),
+        read_joint_state=lambda: (
+            query_calls.append(True)
+            or JointStateSnapshot([0.2] * 7, time.monotonic())
+        ),
+        stop=lambda: None,
+    )
+    teleop = _timeout_teleop(adapter)
+
+    teleop._handle_stale_joint_feedback(0.5)
+    teleop._handle_stale_joint_feedback(0.6)
+
+    assert len(query_calls) == 1
+    assert sends == [[0.2] * 7, [0.2] * 7]
+    assert teleop._last_valid_joint_target == [0.2] * 7
+    assert teleop._last_joint_command_velocity == [0.0] * 7
+
+
+def test_persistent_udp_timeout_query_failure_latches_control() -> None:
+    stop_calls = []
+    adapter = SimpleNamespace(
+        send_joint_target=lambda joints, follow: pytest.fail(
+            "CANFD must stop after query failure"
+        ),
+        read_joint_state=lambda: (_ for _ in ()).throw(
+            RuntimeError("rm_get_joint_degree failed with code 7")
+        ),
+        stop=lambda: stop_calls.append(True),
+    )
+    teleop = _timeout_teleop(adapter)
+
+    teleop._handle_stale_joint_feedback(0.5)
+    teleop._handle_stale_joint_feedback(0.6)
+
+    assert teleop._control_fault_latched
+    assert len(stop_calls) == 1
+```
+
+删除旧的 `_fresh_joint_state()` 直接测试，并用
+`test_short_udp_timeout_without_active_target_stays_stopped` 替换旧的
+`test_stale_feedback_stops_before_active_control`。
+
+在 `test_feedback_fault_blocks_grip_until_release` 中补齐：
+
+```python
+teleop._control_fault_latched = False
+teleop._feedback_resync_attempted = False
+```
+
+- [x] **步骤2：运行测试并确认 RED**
+
+```bash
+source /opt/ros/humble/setup.bash
+python3 -m pytest src/xr_rm_teleop/test/test_joint_control.py \
+  -k 'udp_timeout or joint_snapshot' -v
+```
+
+预期：测试因超时处理和锁存状态尚不存在而失败。
+
+- [x] **步骤3：增加参数与最小状态**
+
+参数默认值：
+
+```python
+self.declare_parameter("control_rate_hz", 90.0)
+self.declare_parameter("command_timeout_sec", 0.12)
+self.declare_parameter("feedback_resync_timeout_sec", 0.5)
+```
+
+读取并初始化：
+
+```python
+self._feedback_resync_timeout_sec = float(
+    self.get_parameter("feedback_resync_timeout_sec").value
+)
+self._feedback_resync_attempted = False
+self._control_fault_latched = False
+```
+
+在 `_validate_parameters()` 中增加：
+
+```python
+if self._feedback_resync_timeout_sec <= self._command_timeout_sec:
+    raise ValueError(
+        "feedback_resync_timeout_sec must be greater than command_timeout_sec"
+    )
+```
+
+- [x] **步骤4：增加保持、重新同步和锁存逻辑**
+
+增加：
+
+```python
+def _handle_stale_joint_feedback(self, age: float) -> None:
+    if self._control_fault_latched:
+        return
+    self._grip_rearm_required = True
+    if self._joint_feedback_ready:
+        self.get_logger().warn(
+            f"{self._arm_name} UDP关节反馈超时，保持最后安全目标。"
+        )
+    self._joint_feedback_ready = False
+
+    if (
+        age >= self._feedback_resync_timeout_sec
+        and not self._feedback_resync_attempted
+    ):
+        self._feedback_resync_attempted = True
+        self.get_logger().warn(
+            f"{self._arm_name} UDP关节反馈持续超时，"
+            "尝试rm_get_joint_degree重新同步。"
+        )
+        try:
+            self._reset_joint_state(self._adapter.read_joint_state())
+        except Exception as exc:
+            self._latch_control_fault(
+                f"UDP关节反馈持续超时且重新同步失败：{exc}"
+            )
+            return
+        self.get_logger().info(
+            f"{self._arm_name} 已通过rm_get_joint_degree重新同步，"
+            "继续保持并等待UDP恢复。"
+        )
+
+    if self._active and self._last_joint_command_target is not None:
+        self._repeat_last_joint_target()
+    else:
+        self._safe_stop(reset_active=True)
+
+def _repeat_last_joint_target(self) -> None:
+    target = self._last_joint_command_target
+    if target is None:
+        return
+    self._adapter.send_joint_target(list(target), self._follow)
+    self._stop_sent = False
+
+def _latch_control_fault(self, message: str) -> None:
+    if self._control_fault_latched:
+        return
+    self._control_fault_latched = True
+    self._grip_rearm_required = True
+    self.get_logger().error(
+        f"{self._arm_name} 控制故障已锁存：{message}"
+    )
+    self._safe_stop(reset_active=True)
+```
+
+- [x] **步骤5：在 QP 之前处理反馈状态**
+
+在 `_control_tick()` 开头用以下逻辑替换现有 `_fresh_joint_state()` 分支：
+
+```python
+if self._control_fault_latched:
+    return
+
+snapshot = self._adapter.get_latest_joint_state()
+if not self._joint_snapshot_is_motion_ready(snapshot):
+    self._grip_rearm_required = True
+    if self._joint_feedback_ready:
+        self.get_logger().warn(
+            f"{self._arm_name} 关节反馈无效或机械臂未就绪，机械臂停止。"
+        )
+    self._joint_feedback_ready = False
+    self._safe_stop(reset_active=True)
+    return
+
+feedback_age = time.monotonic() - snapshot.received_at
+if feedback_age < 0.0:
+    self._grip_rearm_required = True
+    self._joint_feedback_ready = False
+    self._safe_stop(reset_active=True)
+    return
+if feedback_age > self._command_timeout_sec:
+    self._handle_stale_joint_feedback(feedback_age)
+    return
+
+self._feedback_resync_attempted = False
+```
+
+在成功执行 `_sync_joint_feedback(snapshot)` 后，用以下逻辑替换现有首次反馈日志：
+
+```python
+if not self._joint_feedback_ready:
+    if self._grip_rearm_required:
+        message = (
+            f"{self._arm_name} UDP关节反馈已恢复，"
+            "等待Grip松开后重新使能。"
+        )
+    else:
+        message = (
+            f"{self._arm_name} 已收到首帧有效关节反馈，QP可以启用。"
+        )
+    self.get_logger().info(message)
+    self._joint_feedback_ready = True
+```
+
+用以下静态校验替换 `_fresh_joint_state()`：
+
+```python
+@staticmethod
+def _joint_snapshot_is_motion_ready(
+    snapshot: JointStateSnapshot | None,
+) -> bool:
+    return (
+        snapshot is not None
+        and len(snapshot.positions) == 7
+        and all(math.isfinite(value) for value in snapshot.positions)
+        and snapshot.motion_ready
+    )
+```
+
+XR手柄消息超时、Grip逻辑、工作空间/圆柱限位、姿态限速和关节限速保持原样。
+
+- [x] **步骤6：运行超时及反馈安全测试**
+
+```bash
+source /opt/ros/humble/setup.bash
+python3 -m pytest src/xr_rm_teleop/test/test_joint_control.py \
+  -k 'udp_timeout or feedback_fault or joint_snapshot' -v
+```
+
+预期：所选测试通过；短暂超时不调用 QP 和同步查询，机械臂未就绪仍立即停止。
+
+- [x] **步骤7：检查本任务差异**
+
+```bash
+git diff --check
+git diff --stat
+```
+
+预期：没有新增状态机类、线程、依赖、连接或常态轮询。
+
+---
+
+## 任务四：CANFD 错误后停止、查询并等待人工恢复
+
+**修改文件：**
+
+- `src/xr_rm_teleop/test/test_joint_control.py`
+- `src/xr_rm_teleop/xr_rm_teleop/single_arm_velocity_teleop.py`
+
+- [x] **步骤1：先写 CANFD 恢复测试**
+
+用以下测试替换旧的 `test_joint_send_failure_requests_slow_stop_and_resets_control`：
+
+```python
+def test_canfd_error_stops_queries_and_requires_grip_rearm() -> None:
+    class RecoveringAdapter:
+        def __init__(self):
+            self.stop_calls = 0
+            self.read_calls = 0
+
+        def send_joint_target(self, joints, follow):
+            raise RuntimeError("rm_movej_canfd failed with code 9")
+
+        def stop(self):
+            self.stop_calls += 1
+
+        def read_joint_state(self):
+            self.read_calls += 1
+            return JointStateSnapshot([0.2] * 7, time.monotonic())
+
+    teleop = _timeout_teleop(RecoveringAdapter())
+    teleop._joint_command_max_speed = math.radians(180.0)
+    teleop._joint_command_max_acceleration = math.radians(300.0)
+    teleop._dt = 1.0 / 90.0
+
+    sent = teleop._send_joint_target([0.3] * 7)
+
+    assert not sent
+    assert teleop._adapter.stop_calls == 1
+    assert teleop._adapter.read_calls == 1
+    assert not teleop._control_fault_latched
+    assert teleop._grip_rearm_required
+    assert teleop._last_joint_command_target == [0.2] * 7
+
+
+def test_canfd_error_latches_when_joint_query_also_fails() -> None:
+    class FailingAdapter:
+        def __init__(self):
+            self.stop_calls = 0
+
+        def send_joint_target(self, joints, follow):
+            raise RuntimeError("rm_movej_canfd failed with code 9")
+
+        def stop(self):
+            self.stop_calls += 1
+
+        def read_joint_state(self):
+            raise RuntimeError("rm_get_joint_degree failed with code 7")
+
+    teleop = _timeout_teleop(FailingAdapter())
+    teleop._joint_command_max_speed = math.radians(180.0)
+    teleop._joint_command_max_acceleration = math.radians(300.0)
+    teleop._dt = 1.0 / 90.0
+
+    assert not teleop._send_joint_target([0.3] * 7)
+    assert teleop._control_fault_latched
+    assert teleop._adapter.stop_calls == 1
+```
+
+- [x] **步骤2：运行测试并确认 RED**
+
+```bash
+source /opt/ros/humble/setup.bash
+python3 -m pytest src/xr_rm_teleop/test/test_joint_control.py \
+  -k 'canfd_error' -v
+```
+
+预期：测试失败；当前发送错误只会 slow-stop，不会查询实际关节角或锁存查询失败。
+
+- [x] **步骤3：增加统一 CANFD 恢复路径**
+
+增加：
+
+```python
+def _recover_from_canfd_error(self, send_error: Exception) -> None:
+    self.get_logger().error(
+        f"{self._arm_name} rm_movej_canfd发送失败：{send_error}"
+    )
+    self._grip_rearm_required = True
+    self._send_stop_once()
+    self._safe_stop(reset_active=True)
+    try:
+        snapshot = self._adapter.read_joint_state()
+        self._reset_joint_state(snapshot)
+    except Exception as query_error:
+        self._latch_control_fault(
+            "CANFD错误后关节同步失败："
+            f"send={send_error}; query={query_error}"
+        )
+        return
+    self.get_logger().info(
+        f"{self._arm_name} CANFD错误后已同步实际关节角，"
+        "等待UDP恢复及Grip重新使能。"
+    )
+```
+
+把 `_send_joint_target()` 的异常分支替换为：
+
+```python
+except Exception as exc:
+    self._recover_from_canfd_error(exc)
+    return False
+```
+
+让短暂超时重发也走相同错误恢复：
+
+```python
+def _repeat_last_joint_target(self) -> None:
+    target = self._last_joint_command_target
+    if target is None:
+        return
+    try:
+        self._adapter.send_joint_target(list(target), self._follow)
+        self._stop_sent = False
+    except Exception as exc:
+        self._recover_from_canfd_error(exc)
+```
+
+- [x] **步骤4：运行全部关节控制测试**
+
+```bash
+source /opt/ros/humble/setup.bash
+python3 -m pytest src/xr_rm_teleop/test/test_joint_control.py -v
+```
+
+预期：全部关节控制测试通过。
+
+- [x] **步骤5：检查日志与差异**
+
+```bash
+rg -n "rm_movej_canfd发送失败|控制故障已锁存|rm_get_joint_degree" \
+  src/xr_rm_teleop/xr_rm_teleop
+git diff --check
+```
+
+预期：
+
+- CANFD错误和查询错误都包含机械臂名称及失败阶段。
+- 锁存分支不会每周期重复打印错误。
+- 正常QP输出和超时保持使用同一个CANFD恢复入口。
+
+---
+
+## 任务五：同步90 Hz配置并完成mock验证
+
+**修改文件：**
+
+- `src/xr_rm_bringup/config/dual_arm_rm75.yaml`
+- `src/xr_rm_bringup/config/left_arm_rm75.yaml`
+- `src/xr_rm_bringup/config/right_arm_rm75.yaml`
+
+- [x] **步骤1：只修改请求中的控制参数**
+
+三份配置的每个机械臂条目统一为：
+
+```yaml
+control_rate_hz: 90.0
+command_timeout_sec: 0.12
+feedback_resync_timeout_sec: 0.5
+```
+
+保留低跟随：
+
+```yaml
+follow: false
+```
+
+不得修改：
+
+- 工作空间与圆柱限位。
+- TCP线速度、角速度及关节速度/加速度限制。
+- `configure_safety_limits: true`。
+- `move_to_initial_pose_on_connect: false`。
+- 机械臂IP、端口、初始位姿和末端工具配置。
+- 双臂节点名 `left_arm_teleop`、`right_arm_teleop`。
+
+- [x] **步骤2：机械检查三份配置**
+
+```bash
+rg -n "control_rate_hz|command_timeout_sec|feedback_resync_timeout_sec|follow:" \
+  src/xr_rm_bringup/config/dual_arm_rm75.yaml \
+  src/xr_rm_bringup/config/left_arm_rm75.yaml \
+  src/xr_rm_bringup/config/right_arm_rm75.yaml
+```
+
+预期：共四个机械臂配置条目，每个条目均为90.0、0.12、0.5和`follow: false`；三份文件不再出现125.0。
+
+- [x] **步骤3：运行相关测试**
+
+```bash
+source /opt/ros/humble/setup.bash
+python3 -m pytest src/xr_rm_teleop/test/test_initial_joint_pose.py -v
+python3 -m pytest src/xr_rm_teleop/test/test_joint_control.py -v
+python3 -m pytest src/xr_rm_teleop/test/test_orientation_control.py -v
+```
+
+预期：三个命令均以0退出且无失败。
+
+- [x] **步骤4：构建ROS2工作空间**
+
+```bash
+source /opt/ros/humble/setup.bash
+colcon build --symlink-install
+```
+
+预期：`xr_rm_interfaces`、`xr_rm_input`、`xr_rm_teleop`、`xr_rm_bringup` 构建成功。
+
+- [x] **步骤5：通过统一入口进行mock启动验证**
+
+```bash
+source /opt/ros/humble/setup.bash
+source install/setup.bash
+if timeout --signal=INT 10s ros2 launch xr_rm_bringup arm_debug.launch.py \
+  arm:=right use_mock:=true udp_port:=15123
+then
+  true
+else
+  launch_status=$?
+  test "$launch_status" -eq 124
+fi
+```
+
+预期：
+
+- `udp_controller_receiver` 与 `single_arm_velocity_teleop` 正常启动。
+- 遥操作节点报告90 Hz、低跟随，并完成mock关节状态初始化。
+- 不导入厂商SDK，不建立RealMan连接，不发送CANFD，不移动机械臂，不操作夹爪。
+- 10秒后由`timeout`结束；仅该超时允许退出码124。
+
+- [x] **步骤6：最终范围与安全审计**
+
+```bash
+git diff --check
+git status --short
+git diff --stat
+git diff -- \
+  xr_rm_teleop/xr_rm_teleop/realman_adapter.py \
+  xr_rm_teleop/xr_rm_teleop/single_arm_velocity_teleop.py \
+  xr_rm_teleop/test/test_initial_joint_pose.py \
+  xr_rm_teleop/test/test_joint_control.py \
+  xr_rm_bringup/config/dual_arm_rm75.yaml \
+  xr_rm_bringup/config/left_arm_rm75.yaml \
+  xr_rm_bringup/config/right_arm_rm75.yaml
+```
+
+逐项确认：
+
+- 没有无关文件或格式化改动。
+- 没有提交、推送、分支、worktree、锁文件、CI、格式化规则或依赖变化。
+- 没有新增线程、ROS包、launch入口、并发RealMan连接或常态SDK轮询。
+- mock模式不导入、不依赖厂商SDK。
+- `configure_safety_limits` 默认仍为开启。
+- `move_to_initial_pose_on_connect` 默认仍为关闭。
+- `left_arm_teleop`、`right_arm_teleop` 节点名不变。
+- 工作空间/圆柱限位、TCP与关节限速、XR命令超时和slow-stop逻辑仍保留。
+- 验证期间未连接真机、移动机械臂或操作夹爪。

docs/superpowers/plans/2026-07-29-rm75-qp-convergence.md

@@ -0,0 +1,412 @@
+# RM75 QP 收敛优化实施计划
+
+> **执行要求：** 使用 `superpowers:executing-plans` 逐项执行。用户未授权
+> subagent、独立worktree或本地分支，因此本计划只允许当前会话内联实施。所有
+> 步骤使用复选框跟踪。
+
+**目标：** 将当前每周期单步QP改为有界迭代QP，使低跟随RM75在手柄移动10 cm
+后约1秒内稳定到位，并消除由近距离台阶目标造成的持续轻微晃动。
+
+**实现方式：** 每个正常控制周期仍先用UDP实际关节角同步Placo，然后在一次
+`PlacoIkSolver.solve()`内部最多迭代30次，提前达到1 mm位置误差和0.005 rad
+姿态误差即返回。最终关节解继续经过现有90 Hz关节速度与加速度限幅后，以
+`follow=false`发送；不增加预测状态、线程、连接、依赖或配置参数。
+
+**技术栈：** Python 3.10、ROS2 Humble、Placo 0.9.4、NumPy、pytest、
+ament/colcon。
+
+**设计文档：**
+`docs/superpowers/specs/2026-07-29-rm75-qp-convergence-design.md`
+
+---
+
+## 仓库与安全约束
+
+- 构建、测试和启动命令在 `/home/robot/WS_xr` 执行。
+- Git命令在 `/home/robot/WS_xr/src` 执行。
+- 每次构建、测试或启动前执行 `source /opt/ros/humble/setup.bash`。
+- 不自动提交、推送、创建分支或worktree。
+- 不连接真机，不发送真实CANFD，不移动机械臂，不操作夹爪。
+- 只通过 `arm_debug.launch.py arm:=right use_mock:=true`进行启动验证。
+- 不修改三份机械臂YAML、RealMan适配器、launch、UI、依赖或公开入口。
+- 保留工作空间、圆柱、TCP速度、姿态速度、关节速度、关节加速度、反馈超时、
+  CANFD恢复、Grip重新使能和安全停止逻辑。
+
+## 文件范围
+
+- 修改 `xr_rm_teleop/test/test_placo_transforms.py`
+  - 增加真实Placo 7 cm目标收敛回归测试。
+- 修改 `xr_rm_teleop/xr_rm_teleop/placo_ik_solver.py`
+  - 增加固定上限、提前收敛和逐步安全校验。
+
+不需要修改 `single_arm_velocity_teleop.py`；现有 `_solve_joint_target()` 已负责
+QP异常时打印限频警告并保持上一组安全关节目标，现有
+`_limit_joint_command_step()` 已负责最终90 Hz真实命令限速。
+
+---
+
+## 任务一：用真实Placo复现单步QP不收敛
+
+**修改文件：**
+
+- `xr_rm_teleop/test/test_placo_transforms.py`
+
+- [x] **步骤1：增加测试辅助函数**
+
+在文件顶部增加：
+
+```python
+import math
+```
+
+在现有URDF结构测试之后增加：
+
+```python
+def _rm75_placo_solver() -> tuple[PlacoIkSolver, list[float]]:
+    pytest.importorskip("placo")
+    urdf_path = (
+        Path(__file__).resolve().parents[1]
+        / "models"
+        / "rm75_omnipicker"
+        / "urdf"
+        / "RM75-B_OmniPicker_fixed.urdf"
+    )
+    joints = [
+        math.radians(value)
+        for value in [
+            -90.14,
+            3.76,
+            -86.89,
+            87.89,
+            -96.53,
+            -79.62,
+            -90.04,
+        ]
+    ]
+    return PlacoIkSolver(str(urdf_path), 1.0 / 90.0), joints
+```
+
+`importorskip()`只让没有Placo的普通系统Python跳过真模型用例；下面的RED/GREEN
+命令会显式加入项目现有Placo 0.9.4路径，因此该用例必须实际执行而不能跳过。
+
+- [x] **步骤2：增加7 cm目标收敛测试**
+
+增加：
+
+```python
+def test_qp_solve_converges_to_reachable_tcp_target() -> None:
+    solver, joints = _rm75_placo_solver()
+    start_pose = solver.update_joint_state(joints)
+    target_pose = start_pose.copy()
+    target_pose[0, 3] += 0.07
+
+    result = solver.solve(target_pose)
+    reached_pose = solver.update_joint_state(result)
+    position_error = np.linalg.norm(
+        target_pose[:3, 3] - reached_pose[:3, 3]
+    )
+    rotation_delta = (
+        target_pose[:3, :3] @ reached_pose[:3, :3].T
+    )
+    orientation_error = math.acos(
+        float(
+            np.clip(
+                (np.trace(rotation_delta) - 1.0) * 0.5,
+                -1.0,
+                1.0,
+            )
+        )
+    )
+
+    assert position_error <= 1e-3
+    assert orientation_error <= 5e-3
+```
+
+该测试验证一次公开 `solve()` 调用返回当前TCP目标对应的收敛关节解，而不是验证
+内部迭代次数。
+
+- [x] **步骤3：运行测试并确认RED**
+
+```bash
+source /opt/ros/humble/setup.bash
+export RM75_PLACO_TEST_PATH="/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_PLACO_TEST_PATH}:${PYTHONPATH:-}" \
+  python3 -m pytest \
+  src/xr_rm_teleop/test/test_placo_transforms.py::test_qp_solve_converges_to_reachable_tcp_target \
+  -v
+```
+
+预期：测试以位置误差约0.063 m大于0.001 m失败，证明当前单步QP确实不能在一次
+调用内给出收敛关节目标。测试不得因导入错误或跳过而结束。
+
+---
+
+## 任务二：实现有界迭代QP
+
+**修改文件：**
+
+- `xr_rm_teleop/xr_rm_teleop/placo_ik_solver.py`
+
+- [x] **步骤1：增加固定收敛常量**
+
+把模块说明改为：
+
+```python
+"""RM75 的 Placo 0.9.4 有界迭代 QP 逆解。"""
+```
+
+在现有常量后增加：
+
+```python
+QP_MAX_ITERATIONS = 30
+QP_POSITION_TOLERANCE_M = 1e-3
+QP_ORIENTATION_TOLERANCE_RAD = 5e-3
+```
+
+这些值是本次已确认的算法边界，不新增ROS参数。
+
+- [x] **步骤2：增加任务误差读取**
+
+在 `solve()` 前增加：
+
+```python
+def _target_errors(self) -> tuple[float, float]:
+    position_task = self._frame_task.position()
+    orientation_task = self._frame_task.orientation()
+    position_task.update()
+    orientation_task.update()
+    return (
+        float(position_task.error_norm()),
+        float(orientation_task.error_norm()),
+    )
+```
+
+Placo在 `solve(True)` 后只更新关节状态；先更新机器人运动学，再显式更新两个任务，
+确保 `error_norm()`对应当前迭代后的状态而不是前一迭代。
+
+- [x] **步骤3：把单步求解改为最多30次且提前收敛**
+
+用以下实现替换现有 `solve()`：
+
+```python
+def solve(self, target_tool_pose: np.ndarray) -> list[float]:
+    if self._actual_joints is None:
+        raise RuntimeError(
+            "joint state must be initialized before QP solve"
+        )
+    self._frame_task.T_world_frame = _validated_transform(
+        target_tool_pose
+    )
+    result = np.asarray(
+        self._robot.state.q[RM75_Q_SLICE],
+        dtype=float,
+    ).copy()
+    position_error, orientation_error = self._target_errors()
+    if (
+        position_error <= QP_POSITION_TOLERANCE_M
+        and orientation_error <= QP_ORIENTATION_TOLERANCE_RAD
+    ):
+        return result.tolist()
+
+    for _ in range(QP_MAX_ITERATIONS):
+        previous = result
+        self._solver.solve(True)
+        self._robot.update_kinematics()
+        result = np.asarray(
+            self._robot.state.q[RM75_Q_SLICE],
+            dtype=float,
+        ).copy()
+        self._validate_result(result, previous)
+        position_error, orientation_error = self._target_errors()
+        if (
+            position_error <= QP_POSITION_TOLERANCE_M
+            and orientation_error <= QP_ORIENTATION_TOLERANCE_RAD
+        ):
+            return result.tolist()
+
+    raise RuntimeError(
+        "QP did not converge after "
+        f"{QP_MAX_ITERATIONS} iterations: "
+        f"position_error={position_error:.6f} m, "
+        f"orientation_error={orientation_error:.6f} rad"
+    )
+```
+
+目标已到达时直接返回当前关节角，避免静止时进行不必要的数值迭代。
+
+- [x] **步骤4：让速度校验针对每次数值迭代**
+
+把 `_validate_result()` 签名改为：
+
+```python
+def _validate_result(
+    self,
+    result: np.ndarray,
+    reference: np.ndarray | None = None,
+) -> None:
+```
+
+保留现有有限值和关节位置检查，把速度检查替换为：
+
+```python
+if reference is None:
+    reference = self._actual_joints
+if reference is None:
+    raise RuntimeError("joint state has not been initialized")
+reference = np.asarray(reference, dtype=float)
+if reference.shape != (7,) or not np.isfinite(reference).all():
+    raise ValueError("QP reference must contain 7 finite values")
+max_step = self._velocity_limits * self._dt + 1e-9
+if np.any(np.abs(result - reference) > max_step):
+    raise ValueError(
+        "QP result violates RM75 one-cycle velocity limits"
+    )
+```
+
+这样每次内部数值迭代继续满足Placo的URDF关节速度边界；最终收敛解仍由节点现有
+`_limit_joint_command_step()`按真实90 Hz周期限制后才发送。
+
+- [x] **步骤5：运行目标测试并确认GREEN**
+
+```bash
+source /opt/ros/humble/setup.bash
+export RM75_PLACO_TEST_PATH="/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_PLACO_TEST_PATH}:${PYTHONPATH:-}" \
+  python3 -m pytest \
+  src/xr_rm_teleop/test/test_placo_transforms.py::test_qp_solve_converges_to_reachable_tcp_target \
+  src/xr_rm_teleop/test/test_placo_transforms.py::test_qp_result_rejects_nan_position_and_velocity_violations \
+  -v
+```
+
+预期：两个测试通过；真实Placo用例不被跳过。
+
+- [x] **步骤6：运行Placo变换测试文件**
+
+```bash
+source /opt/ros/humble/setup.bash
+export RM75_PLACO_TEST_PATH="/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_PLACO_TEST_PATH}:${PYTHONPATH:-}" \
+  python3 -m pytest \
+  src/xr_rm_teleop/test/test_placo_transforms.py \
+  -v
+```
+
+预期：全部通过，无失败或跳过。
+
+---
+
+## 任务三：回归、安全和mock验证
+
+**验证范围：**
+
+- `xr_rm_teleop`全部测试；
+- ROS2工作空间构建；
+- 统一launch的右臂mock启动；
+- 最终差异与安全配置审计。
+
+- [x] **步骤1：运行遥操作包全部测试**
+
+```bash
+source /opt/ros/humble/setup.bash
+export RM75_PLACO_TEST_PATH="/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_PLACO_TEST_PATH}:${PYTHONPATH:-}" \
+  python3 -m pytest src/xr_rm_teleop/test -v
+```
+
+预期：全部测试通过，真实Placo收敛用例被执行。
+
+- [x] **步骤2：按项目规则单独运行姿态控制测试**
+
+```bash
+source /opt/ros/humble/setup.bash
+python3 -m pytest \
+  src/xr_rm_teleop/test/test_orientation_control.py \
+  -v
+```
+
+预期：全部通过。
+
+- [x] **步骤3：构建ROS2工作空间**
+
+```bash
+source /opt/ros/humble/setup.bash
+colcon build --symlink-install
+```
+
+预期：`xr_rm_interfaces`、`xr_rm_input`、`xr_rm_teleop`和
+`xr_rm_bringup`全部构建成功。
+
+- [x] **步骤4：通过统一入口进行右臂mock启动验证**
+
+```bash
+source /opt/ros/humble/setup.bash
+source install/setup.bash
+if timeout --signal=INT 10s ros2 launch \
+  xr_rm_bringup arm_debug.launch.py \
+  arm:=right use_mock:=true udp_port:=15123
+then
+  true
+else
+  launch_status=$?
+  test "$launch_status" -eq 124
+fi
+```
+
+预期：
+
+- `udp_controller_receiver`和`single_arm_velocity_teleop`正常启动；
+- 节点报告 `dt=0.0111s`、`follow=False`；
+- mock关节初始化成功；
+- 不导入RealMan SDK，不建立真机连接，不发送CANFD；
+- 10秒后仅由 `timeout`结束。
+
+- [x] **步骤5：最终差异和安全审计**
+
+在 `/home/robot/WS_xr/src` 执行：
+
+```bash
+git diff --check
+git status --short
+git diff -- \
+  xr_rm_teleop/xr_rm_teleop/placo_ik_solver.py \
+  xr_rm_teleop/test/test_placo_transforms.py
+rg -n \
+  "control_rate_hz|follow:|configure_safety_limits|move_to_initial_pose_on_connect" \
+  xr_rm_bringup/config/dual_arm_rm75.yaml \
+  xr_rm_bringup/config/left_arm_rm75.yaml \
+  xr_rm_bringup/config/right_arm_rm75.yaml
+```
+
+预期：
+
+- 生产代码只修改Placo求解器；
+- 测试只增加真实模型收敛验证；
+- 三份配置继续使用90 Hz、`follow: false`、
+  `configure_safety_limits: true`和
+  `move_to_initial_pose_on_connect: false`；
+- 不改变此前由用户保留的 `AGENTS.md` 修改；
+- 不自动提交或推送。
+
+---
+
+## 真机交接验收
+
+Codex不执行本节。自动验证全部通过后，由用户在安全工作区使用：
+
+```bash
+ros2 launch xr_rm_bringup arm_debug.launch.py \
+  arm:=right use_mock:=false
+```
+
+验收步骤：
+
+1. 急停可用、Grip松开、工作区无人后启动。
+2. 按住Grip，快速移动手柄约10 cm后保持不动。
+3. 机械臂应在约1秒内稳定到位，无持续肉眼可见晃动。
+4. 连续观察四个5秒 timing 窗口，`total max`均低于11.111 ms。
+5. 不应出现QP未收敛、反馈超时、CANFD错误或故障锁存日志。
+6. 松开Grip后机械臂按现有逻辑安全停止。
+
+若任一窗口 `total max`达到或超过11.111 ms，或机械臂出现明显振荡，立即松开
+Grip并停止测试，把完整timing和错误日志返回后再调整；不得直接提高控制频率、
+关闭限速或改成高跟随。

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

@@ -0,0 +1,309 @@
+# RM75 关节命令提前制动实施计划
+
+> **供智能体执行者：** 必须使用 `superpowers:subagent-driven-development`
+>（推荐）或 `superpowers:executing-plans` 逐项实施；所有步骤使用复选框跟踪。
+
+**目标：** 修复 90 Hz 关节命令在稳定目标附近反复越界的问题，使 RM75 在保留
+现有速度、加速度限制和低跟随模式的前提下提前制动并稳定停止。
+
+**架构：** 保留当前 QP、反馈和故障恢复链路，只替换
+`SingleArmVelocityTeleop._limit_joint_command_step()` 内部的关节命令生成规则。
+每个关节根据离散制动距离决定继续加速或开始减速，最终命令仍由现有
+`_send_joint_target()` 发送。
+
+**技术栈：** Python 3.10、ROS2 Humble、NumPy、pytest、ament/colcon。
+
+---
+
+## 执行约束
+
+- 设计文档：
+  `docs/superpowers/specs/2026-07-30-rm75-joint-command-braking-design.md`。
+- 构建、测试和启动命令在 `/home/robot/WS_xr` 执行，并先运行
+  `source /opt/ros/humble/setup.bash`。
+- 不连接真机，不发送真实 CANFD，不移动机械臂，不操作夹爪。
+- 启动验证只使用
+  `xr_rm_bringup/launch/arm_debug.launch.py arm:=right use_mock:=true`。
+- 不修改 QP、YAML、RealMan 适配器、launch、UI、依赖和公开 API。
+- 保留工作空间、圆柱、TCP 速度、姿态速度、关节速度、关节加速度、超时保持、
+  CANFD 恢复、Grip 重新使能和安全停止逻辑。
+- 用户未要求 Git 提交，因此本计划不执行 `git commit` 或 `git push`。
+- 保留工作区中已有的其他修改，不回退、不覆盖：
+  `placo_ik_solver.py`、`test_placo_transforms.py` 及现有 Superpowers 文档。
+
+## 文件范围
+
+- 修改 `xr_rm_teleop/test/test_joint_control.py`
+  - 增加固定目标提前制动回归测试。
+- 修改 `xr_rm_teleop/xr_rm_teleop/single_arm_velocity_teleop.py`
+  - 在现有关节限幅入口实现离散制动距离判断。
+- 不创建新的运行时代码文件或配置项。
+
+### 任务一：增加持续振荡回归测试
+
+**文件：**
+
+- 修改：`xr_rm_teleop/test/test_joint_control.py:206`
+- 测试：`xr_rm_teleop/test/test_joint_control.py`
+
+- [x] **步骤 1：在现有首周期加速度测试后增加固定目标测试**
+
+增加以下测试：
+
+```python
+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_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 == pytest.approx(target, abs=1e-12)
+    assert velocity == pytest.approx([0.0] * 7, abs=1e-12)
+```
+
+该测试同时覆盖正负方向、不同目标距离、最大速度、最大加速度、禁止越过固定目标
+和最终停止。
+
+- [x] **步骤 2：运行新增测试并确认失败**
+
+运行：
+
+```bash
+source /opt/ros/humble/setup.bash
+PYTHONPATH="/home/robot/WS_xr/src/xr_rm_teleop:${PYTHONPATH:-}" \
+  python3 -m pytest \
+  src/xr_rm_teleop/test/test_joint_control.py::test_joint_command_step_brakes_before_fixed_target_without_overshoot \
+  -v
+```
+
+预期：`FAIL`，现有实现会让至少一个关节命令越过固定目标。失败原因必须来自新增
+越界断言，不能是导入或环境错误。
+
+### 任务二：实现离散提前制动
+
+**文件：**
+
+- 修改：
+  `xr_rm_teleop/xr_rm_teleop/single_arm_velocity_teleop.py:1232-1263`
+- 测试：`xr_rm_teleop/test/test_joint_control.py`
+
+- [x] **步骤 1：用离散制动逻辑替换现有限幅计算**
+
+保留方法签名和现有长度、参数校验，将
+`desired_velocity = np.clip(...)` 到返回值的部分替换为：
+
+```python
+        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
+        arrival_distance = velocity_step * dt
+        limited_target = []
+        limited_velocity = []
+        for desired_target, last_target, last_velocity in zip(
+            target,
+            previous_target,
+            previous_velocity,
+        ):
+            error = desired_target - last_target
+            if (
+                abs(last_velocity) <= 1e-12
+                and abs(error) <= arrival_distance
+            ):
+                velocity = error / dt
+                position = desired_target
+            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, limited_velocity
+```
+
+不要增加 ROS 参数或辅助类。制动距离直接使用当前方法已有的
+`max_acceleration`、`max_speed` 和 `dt`。
+
+- [x] **步骤 2：运行新增测试并确认通过**
+
+运行：
+
+```bash
+source /opt/ros/humble/setup.bash
+PYTHONPATH="/home/robot/WS_xr/src/xr_rm_teleop:${PYTHONPATH:-}" \
+  python3 -m pytest \
+  src/xr_rm_teleop/test/test_joint_control.py::test_joint_command_step_brakes_before_fixed_target_without_overshoot \
+  -v
+```
+
+预期：`PASS`。
+
+- [x] **步骤 3：运行关节控制测试文件**
+
+运行：
+
+```bash
+source /opt/ros/humble/setup.bash
+PYTHONPATH="/home/robot/WS_xr/src/xr_rm_teleop:${PYTHONPATH:-}" \
+  python3 -m pytest src/xr_rm_teleop/test/test_joint_control.py -v
+```
+
+预期：全部通过；现有
+`test_joint_command_step_limits_acceleration_from_rest` 继续通过，证明首周期
+加速度行为没有回归。
+
+### 任务三：完整验证
+
+**文件：**
+
+- 不修改文件。
+
+- [x] **步骤 1：运行 `xr_rm_teleop` 全部测试**
+
+运行：
+
+```bash
+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:-}" \
+  python3 -m pytest \
+  src/xr_rm_teleop/test -v
+```
+
+预期：全部测试通过，无失败；真实 Placo 回归测试必须执行，不能因缺少模块而跳过。
+
+- [x] **步骤 2：单独运行姿态控制测试**
+
+运行：
+
+```bash
+source /opt/ros/humble/setup.bash
+PYTHONPATH="/home/robot/WS_xr/src/xr_rm_teleop:${PYTHONPATH:-}" \
+  python3 -m pytest \
+  src/xr_rm_teleop/test/test_orientation_control.py -v
+```
+
+预期：全部通过。
+
+- [x] **步骤 3：构建工作空间**
+
+运行：
+
+```bash
+source /opt/ros/humble/setup.bash
+colcon build --symlink-install
+```
+
+预期：`xr_rm_input`、`xr_rm_interfaces`、`xr_rm_teleop` 和 `xr_rm_bringup`
+全部构建成功。
+
+- [x] **步骤 4：使用 mock 启动右臂统一 launch**
+
+运行：
+
+```bash
+source /opt/ros/humble/setup.bash
+source install/setup.bash
+timeout 10s ros2 launch xr_rm_bringup arm_debug.launch.py \
+  arm:=right use_mock:=true
+```
+
+预期：
+
+- 节点日志显示控制周期约 `dt=0.0111s`；
+- 日志显示 `follow=False`；
+- 不连接厂商 SDK，不发送真实 CANFD；
+- 除 `timeout` 主动结束产生的退出状态外，没有 Python 异常或 ROS 错误。
+
+- [x] **步骤 5：检查最终差异**
+
+运行：
+
+```bash
+git -C /home/robot/WS_xr/src diff --check
+git -C /home/robot/WS_xr/src status --short
+git -C /home/robot/WS_xr/src diff -- \
+  xr_rm_teleop/xr_rm_teleop/single_arm_velocity_teleop.py \
+  xr_rm_teleop/test/test_joint_control.py
+```
+
+预期：
+
+- `diff --check` 无输出；
+- 本次运行时代码改动只涉及上述两个文件；
+- 原有工作区修改仍保留；
+- 不存在提交或远程推送。
+
+## 用户真机验证边界
+
+自动验证完成后，只提供手动验证步骤，不由 Codex 操作真机：
+
+1. 保持低跟随，从安全姿态和小于 5 mm 的上下位移开始；
+2. 手柄停止后观察机械臂是否立即减振并稳定；
+3. 确认无持续 QP、UDP、CANFD 或故障锁存错误后，再测试 10 mm；
+4. 若不再振荡但仍有不可接受的整臂大幅构型变化，停止扩大位移，转入独立的奇异点
+   处理设计。

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/plans/2026-07-30-xrobotoolkit-controller-inputs.md

@@ -0,0 +1,650 @@
+# XRoboToolkit 手柄输入扩展 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:** 将 XRoboToolkit 左右手柄摇杆、主键和副键加入现有 `XrController` 链路，同时保持 Grip、Trigger、位姿和遥操作行为不变。
+
+**Architecture:** 直接扩展现有 ROS2 消息，继续使用当前 bridge 的嵌套 UDP `buttons` 结构，由 receiver 将按钮展平到消息字段。新增字段按可选输入解析，旧数据包或非法新增字段回退到安全默认值，不新增话题、依赖或控制分支。
+
+**Tech Stack:** Ubuntu 22.04、ROS2 Humble、Python 3.10、ament/colcon、pytest、XRoboToolkit PC-Service Python binding。
+
+## Global Constraints
+
+- 所有构建、测试和运行命令在 `/home/robot/WS_xr` 执行，并先运行 `source /opt/ros/humble/setup.bash`。
+- `XrController.msg` 的字段和顺序必须与批准的设计完全一致，不增加菜单键、摇杆按键、模拟 Grip/Trigger、SDK 时间戳或序号。
+- 保持现有 `grip`、`trigger`、`pose` 语义及控制行为不变。
+- 不新增依赖、ROS2 话题、节点或 LeRobot 录制实现。
+- 不修改 `xr_rm_teleop` 控制代码、机械臂 YAML、安全限位、超时或停止逻辑。
+- 启动验证只使用 `use_mock:=true`，不得连接真机、移动机械臂或操作夹爪。
+- 新增和修改的 Markdown 文档使用中文。
+- Superpowers 工作流只允许创建本地 Git 提交；禁止 push、合并本地分支、合并 PR 或执行任何远程写操作。
+- 每次提交只暂存当前任务列出的文件，不包含用户的其他工作区改动。
+
+---
+
+### Task 1: 扩展 `XrController` 消息接口
+
+**Files:**
+- Modify: `src/xr_rm_interfaces/msg/XrController.msg`
+
+**Interfaces:**
+- Consumes: 现有 `std_msgs/Header`、`geometry_msgs/Pose` 和 `hand/grip/trigger/pose` 字段。
+- Produces: `XrController.primary: bool`、`secondary: bool`、`axis: float32[2]`，供 Task 2 的 receiver 赋值。
+
+- [ ] **Step 1: 记录旧接口缺少新增字段**
+
+在工作空间根目录执行：
+
+```bash
+cd /home/robot/WS_xr
+source /opt/ros/humble/setup.bash
+source install/setup.bash
+python3 - <<'PY'
+from xr_rm_interfaces.msg import XrController
+
+expected = [
+    "header",
+    "hand",
+    "grip",
+    "trigger",
+    "primary",
+    "secondary",
+    "axis",
+    "pose",
+]
+actual = list(XrController.get_fields_and_field_types())
+assert actual == expected, actual
+PY
+```
+
+Expected: FAIL；旧接口输出
+`['header', 'hand', 'grip', 'trigger', 'pose']`。
+
+- [ ] **Step 2: 用批准的顺序修改消息定义**
+
+将 `src/xr_rm_interfaces/msg/XrController.msg` 完整替换为：
+
+```text
+std_msgs/Header header
+string hand
+
+bool grip
+float32 trigger
+bool primary
+bool secondary
+float32[2] axis
+
+geometry_msgs/Pose pose
+```
+
+- [ ] **Step 3: 构建消息包**
+
+```bash
+cd /home/robot/WS_xr
+source /opt/ros/humble/setup.bash
+colcon build --symlink-install --packages-select xr_rm_interfaces
+```
+
+Expected: `xr_rm_interfaces` 构建成功，无 rosidl 错误。
+
+- [ ] **Step 4: 验证生成接口字段和顺序**
+
+```bash
+cd /home/robot/WS_xr
+source /opt/ros/humble/setup.bash
+source install/setup.bash
+python3 - <<'PY'
+from xr_rm_interfaces.msg import XrController
+
+expected = [
+    "header",
+    "hand",
+    "grip",
+    "trigger",
+    "primary",
+    "secondary",
+    "axis",
+    "pose",
+]
+actual = list(XrController.get_fields_and_field_types())
+assert actual == expected, actual
+assert XrController.get_fields_and_field_types()["axis"] == "float[2]"
+PY
+```
+
+Expected: PASS，无输出。
+
+- [ ] **Step 5: 提交消息接口**
+
+```bash
+cd /home/robot/WS_xr/src
+git add xr_rm_interfaces/msg/XrController.msg
+git commit -m "feat: 扩展 XR 手柄消息"
+```
+
+Expected: 只提交 `XrController.msg`；不执行 push 或 merge。
+
+---
+
+### Task 2: 透传摇杆与主副按键
+
+**Files:**
+- Create: `src/xr_rm_input/test/test_controller_fields.py`
+- Modify: `src/xr_rm_input/xr_rm_input/xrobotoolkit_to_udp_bridge.py`
+- Modify: `src/xr_rm_input/xr_rm_input/udp_controller_receiver.py`
+
+**Interfaces:**
+- Consumes: Task 1 生成的 `XrController.primary`、`secondary`、`axis`。
+- Produces:
+  - `_buttons_payload(*, primary: Callable[[], Any], secondary: Callable[[], Any]) -> dict[str, bool]`
+  - `_controller_payload(*, hand: str, pose: Any, axis: Any, buttons: dict[str, bool], grip_pressed: bool, trigger_pressed: bool, pose_valid: bool = True) -> dict[str, Any]`
+  - `UdpControllerReceiver._optional_axis(value: Any) -> list[float]`
+  - `UdpControllerReceiver._optional_buttons(value: Any) -> tuple[bool, bool]`
+
+- [ ] **Step 1: 新增失败测试**
+
+创建 `src/xr_rm_input/test/test_controller_fields.py`：
+
+```python
+import math
+from types import SimpleNamespace
+
+from builtin_interfaces.msg import Time
+from xr_rm_input.udp_controller_receiver import UdpControllerReceiver
+from xr_rm_input.xrobotoolkit_to_udp_bridge import (
+    _buttons_payload,
+    _controller_payload,
+    _stop_controller_payload,
+)
+
+
+def _receiver_without_socket() -> UdpControllerReceiver:
+    receiver = object.__new__(UdpControllerReceiver)
+    receiver._quat_order = "xyzw"
+    receiver.get_clock = lambda: SimpleNamespace(
+        now=lambda: SimpleNamespace(to_msg=lambda: Time())
+    )
+    return receiver
+
+
+def test_bridge_payload_contains_only_selected_controller_inputs() -> None:
+    buttons = _buttons_payload(
+        primary=lambda: True,
+        secondary=lambda: False,
+    )
+    payload = _controller_payload(
+        hand="left",
+        pose=[1.0, 2.0, 3.0, 0.0, 0.0, 0.0, 1.0],
+        axis=[2.0, -2.0],
+        buttons=buttons,
+        grip_pressed=True,
+        trigger_pressed=False,
+    )
+
+    assert payload == {
+        "hand": "left",
+        "grip": True,
+        "trigger": 0.0,
+        "pos": [1.0, 2.0, 3.0],
+        "quat": [0.0, 0.0, 0.0, 1.0],
+        "pose_valid": True,
+        "pose_source": "xrobotoolkit",
+        "axis": [1.0, -1.0],
+        "buttons": {
+            "primary": True,
+            "secondary": False,
+        },
+    }
+
+
+def test_stop_payload_uses_neutral_selected_inputs() -> None:
+    payload = _stop_controller_payload("right")
+
+    assert payload["axis"] == [0.0, 0.0]
+    assert payload["buttons"] == {
+        "primary": False,
+        "secondary": False,
+    }
+    assert "grip_value" not in payload
+    assert "trigger_value" not in payload
+
+
+def test_receiver_publishes_selected_controller_inputs() -> None:
+    msg = _receiver_without_socket()._payload_to_msg(
+        {
+            "grip": True,
+            "trigger": 1.0,
+            "axis": [2.0, -2.0],
+            "buttons": {
+                "primary": True,
+                "secondary": False,
+            },
+            "pos": [1.0, 2.0, 3.0],
+            "quat": [0.0, 0.0, 0.0, 1.0],
+        },
+        "left",
+    )
+
+    assert msg.primary is True
+    assert msg.secondary is False
+    assert list(msg.axis) == [1.0, -1.0]
+
+
+def test_receiver_defaults_invalid_optional_inputs() -> None:
+    msg = _receiver_without_socket()._payload_to_msg(
+        {
+            "grip": True,
+            "trigger": 0.4,
+            "axis": [math.nan, 0.0],
+            "buttons": [],
+            "pos": [1.0, 2.0, 3.0],
+            "quat": [0.0, 0.0, 0.0, 1.0],
+        },
+        "right",
+    )
+
+    assert msg.primary is False
+    assert msg.secondary is False
+    assert list(msg.axis) == [0.0, 0.0]
+    assert msg.grip is True
+    assert abs(msg.trigger - 0.4) < 1e-6
+    assert msg.pose.position.x == 1.0
+    assert msg.pose.position.y == 2.0
+    assert msg.pose.position.z == 3.0
+
+
+def test_receiver_defaults_missing_legacy_optional_inputs() -> None:
+    msg = _receiver_without_socket()._payload_to_msg(
+        {
+            "grip": True,
+            "trigger": 0.0,
+            "pos": [0.0, 1.0, 0.0],
+            "quat": [0.0, 0.0, 0.0, 1.0],
+        },
+        "left",
+    )
+
+    assert msg.primary is False
+    assert msg.secondary is False
+    assert list(msg.axis) == [0.0, 0.0]
+    assert msg.grip is True
+```
+
+- [ ] **Step 2: 运行测试并确认失败**
+
+```bash
+cd /home/robot/WS_xr
+source /opt/ros/humble/setup.bash
+source install/setup.bash
+pytest src/xr_rm_input/test/test_controller_fields.py -v
+```
+
+Expected: FAIL；旧 `_buttons_payload` 仍要求 `grip/menu/axis_click`，且 receiver
+尚无 `_optional_axis` 和 `_optional_buttons`。
+
+- [ ] **Step 3: 精简 bridge payload**
+
+在 `xrobotoolkit_to_udp_bridge.py` 中将 `_controller_payload` 改为：
+
+```python
+def _controller_payload(
+    *,
+    hand: str,
+    pose: Any,
+    axis: Any,
+    buttons: dict[str, bool],
+    grip_pressed: bool,
+    trigger_pressed: bool,
+    pose_valid: bool = True,
+) -> dict[str, Any]:
+    pos, quat = (
+        _pose_to_pos_quat(pose)
+        if pose_valid
+        else (ZERO_POS.copy(), IDENTITY_QUAT.copy())
+    )
+    return {
+        "hand": hand,
+        "grip": pose_valid and grip_pressed,
+        "trigger": 1.0 if pose_valid and trigger_pressed else 0.0,
+        "pos": pos,
+        "quat": quat,
+        "pose_valid": pose_valid,
+        "pose_source": POSE_SOURCE,
+        "axis": _safe_axis(axis),
+        "buttons": buttons,
+    }
+```
+
+将 `_stop_controller_payload` 的输入部分改为：
+
+```python
+        "axis": [0.0, 0.0],
+        "buttons": {
+            "primary": False,
+            "secondary": False,
+        },
+```
+
+并删除 `grip_value`、`trigger_value`、`buttons.grip`、`buttons.menu` 和
+`buttons.axis_click` 输出。
+
+将 `_buttons_payload` 改为：
+
+```python
+def _buttons_payload(
+    *,
+    primary: Callable[[], Any],
+    secondary: Callable[[], Any],
+) -> dict[str, bool]:
+    return {
+        "primary": _safe_bool(primary),
+        "secondary": _safe_bool(secondary),
+    }
+```
+
+修改主循环的左手调用：
+
+```python
+                    "left": _controller_payload(
+                        hand="left",
+                        pose=xrt.get_left_controller_pose(),
+                        axis=xrt.get_left_axis(),
+                        buttons=_buttons_payload(
+                            primary=xrt.get_X_button,
+                            secondary=xrt.get_Y_button,
+                        ),
+                        grip_pressed=left_grip,
+                        trigger_pressed=left_trigger,
+                    ),
+```
+
+修改主循环的右手调用：
+
+```python
+                    "right": _controller_payload(
+                        hand="right",
+                        pose=xrt.get_right_controller_pose(),
+                        axis=xrt.get_right_axis(),
+                        buttons=_buttons_payload(
+                            primary=xrt.get_A_button,
+                            secondary=xrt.get_B_button,
+                        ),
+                        grip_pressed=right_grip,
+                        trigger_pressed=right_trigger,
+                    ),
+```
+
+保留主循环中 `get_left/right_grip()`、`get_left/right_trigger()` 和现有滞回
+开关；仅从 `_controller_payload` 参数及 UDP 输出中删除原始模拟量。
+
+- [ ] **Step 4: 为 receiver 增加容错解析**
+
+在 `udp_controller_receiver.py` 导入区增加：
+
+```python
+import math
+```
+
+在 `_payload_to_msg` 中读取新增可选字段：
+
+```python
+        axis = self._optional_axis(payload.get("axis"))
+        primary, secondary = self._optional_buttons(payload.get("buttons"))
+```
+
+在现有 `msg.grip` 和 `msg.trigger` 赋值后加入：
+
+```python
+        msg.primary = primary
+        msg.secondary = secondary
+        msg.axis = axis
+```
+
+在 `_vector3` 附近增加两个无状态解析方法：
+
+```python
+    @staticmethod
+    def _optional_axis(value: Any) -> list[float]:
+        try:
+            axis = [float(item) for item in value]
+        except (TypeError, ValueError):
+            return [0.0, 0.0]
+        if len(axis) != 2 or not all(math.isfinite(item) for item in axis):
+            return [0.0, 0.0]
+        return [
+            min(max(axis[0], -1.0), 1.0),
+            min(max(axis[1], -1.0), 1.0),
+        ]
+
+    @classmethod
+    def _optional_buttons(cls, value: Any) -> tuple[bool, bool]:
+        if not isinstance(value, Mapping):
+            return False, False
+        return (
+            cls._as_bool(value.get("primary", False)),
+            cls._as_bool(value.get("secondary", False)),
+        )
+```
+
+不要把新增字段加入现有 pose 诊断条件；它们无效时不得改变 `grip`。
+
+- [ ] **Step 5: 运行新增测试**
+
+```bash
+cd /home/robot/WS_xr
+source /opt/ros/humble/setup.bash
+source install/setup.bash
+pytest src/xr_rm_input/test/test_controller_fields.py -v
+```
+
+Expected: `5 passed`。
+
+- [ ] **Step 6: 运行 Python 语法检查**
+
+```bash
+cd /home/robot/WS_xr
+python3 -m py_compile \
+  src/xr_rm_input/xr_rm_input/xrobotoolkit_to_udp_bridge.py \
+  src/xr_rm_input/xr_rm_input/udp_controller_receiver.py \
+  src/xr_rm_input/test/test_controller_fields.py
+```
+
+Expected: PASS，无输出。
+
+- [ ] **Step 7: 提交 bridge、receiver 和测试**
+
+```bash
+cd /home/robot/WS_xr/src
+git add \
+  xr_rm_input/xr_rm_input/xrobotoolkit_to_udp_bridge.py \
+  xr_rm_input/xr_rm_input/udp_controller_receiver.py \
+  xr_rm_input/test/test_controller_fields.py
+git commit -m "feat: 发布 XR 手柄摇杆与按键"
+```
+
+Expected: 只提交列出的三个文件；不执行 push 或 merge。
+
+---
+
+### Task 3: 更新文档并完成工作空间验证
+
+**Files:**
+- Modify: `src/README.md`
+- Modify: `src/AGENTS.md`
+
+**Interfaces:**
+- Consumes: Task 1 的最终 `XrController` 格式和 Task 2 的 UDP JSON。
+- Produces: 当前手柄接口说明，以及对后续 Superpowers 任务生效的本地 Git 边界。
+
+- [ ] **Step 1: 更新 README 的 Git 约束**
+
+在 README 的环境准备之前增加：
+
+```markdown
+## Superpowers Git 约束
+
+使用 Superpowers 执行任务时，只允许按 skill 工作流创建本地 Git 提交。
+禁止执行 `git push`、合并本地分支、合并 PR 或其他远程写操作。skill 如需
+独立 worktree 或配套本地分支，可以创建，但不得将其合并到其他分支。
+```
+
+- [ ] **Step 2: 更新 README 的当前 UDP 示例**
+
+先修正 README 顶部的当前范围和项目结构：
+
+- 将“自定义 PICO 4 Ultra UDP Sender Unity 工程”完成项替换为
+  “XRoboToolkit bridge 读取左右手柄 pose、Grip、Trigger、摇杆和主副按键”。
+- 从项目结构树删除当前仓库中不存在的
+  `docs/pico_udp_sender_ubuntu22_setup.md` 和整个 `unity/` 子树。
+- 保留官方 XRoboToolkit APK、PC-Service、`PXREAClientUnity` 和
+  `RobotLinuxDemo` 的运行说明；这些是外部工具，不是仓库内已删除的 Unity 工程。
+
+将“UDP 数据格式”开头改为“当前 XRoboToolkit bridge 每个周期发送一个双手柄
+JSON 包”，并将示例替换为：
+
+```json
+{
+  "t": 12.345,
+  "source_time": 12.345,
+  "seq": 42,
+  "frame_id": "xr_world",
+  "controllers": {
+    "left": {
+      "hand": "left",
+      "grip": true,
+      "trigger": 0.0,
+      "axis": [0.2, -0.4],
+      "buttons": {
+        "primary": true,
+        "secondary": false
+      },
+      "pos": [-0.12, 1.05, 0.30],
+      "quat": [0.0, 0.0, 0.0, 1.0],
+      "pose_valid": true,
+      "pose_source": "xrobotoolkit"
+    },
+    "right": {
+      "hand": "right",
+      "grip": true,
+      "trigger": 1.0,
+      "axis": [-0.1, 0.3],
+      "buttons": {
+        "primary": false,
+        "secondary": true
+      },
+      "pos": [0.12, 1.05, 0.30],
+      "quat": [0.0, 0.0, 0.0, 1.0],
+      "pose_valid": true,
+      "pose_source": "xrobotoolkit"
+    }
+  }
+}
+```
+
+字段说明更新为：
+
+```markdown
+- `t` / `source_time`：bridge 的 PC 单调时间，用于诊断发送周期。
+- `seq`：bridge 递增的 UDP 包序号，bridge 重启后重新计数。
+- `frame_id`：默认 `xr_world`，会写入 `XrController.header.frame_id`。
+- `grip`：运动使能。`true` 时进入相对位姿控制，`false` 时停止。
+- `trigger`：经过 bridge 滞回处理的 `0.0/1.0` 值；上升沿切换对应夹爪状态。
+- `axis`：摇杆 `[x, y]`，每个分量限制在 `-1.0` 到 `1.0`。
+- `buttons.primary`：左手 X 键或右手 A 键。
+- `buttons.secondary`：左手 Y 键或右手 B 键。
+- `pos`：手柄位置，长度 3。
+- `quat`：手柄姿态四元数，默认按 `xyzw` 解析。
+- `pose_valid`：姿态是否可信；`false` 时接收端强制 `grip=false`。
+- `pose_source`：当前 bridge 使用 `xrobotoolkit`。
+```
+
+补充说明：`axis`、`buttons.primary` 和 `buttons.secondary` 会进入
+`XrController`；旧 UDP 包缺少这些字段时分别回退为 `[0,0]`、`false` 和
+`false`。删除已经不存在的自定义 Unity 工程和安装文档链接，但保留 receiver
+对旧格式字段的兼容说明。
+
+将故障排查中的旧 Unity HUD 提示替换为：
+
+```markdown
+- 确认 `xrobotoolkit_to_udp_bridge` 没有持续打印 SDK read failed；SDK
+  读取失败时 bridge 会发送 `pose_valid=false` 的停止包。
+```
+
+- [ ] **Step 3: 更新 AGENTS 的 Superpowers Git 规则**
+
+将 AGENTS“Git 与提交”中的 Superpowers 段落改为：
+
+```markdown
+使用 Superpowers 执行任务时，只允许按相关 skill 工作流创建本地 Git 提交；
+禁止执行 `git push`、合并本地分支、合并 PR 或其他远程写操作。相关 skill
+如需独立 worktree 或配套本地分支，可以创建，但不得将其合并到其他分支。
+其他情况下，除非用户明确要求，不要自动创建分支。
+```
+
+- [ ] **Step 4: 运行输入包测试**
+
+```bash
+cd /home/robot/WS_xr
+source /opt/ros/humble/setup.bash
+source install/setup.bash
+pytest src/xr_rm_input/test/test_controller_fields.py -v
+```
+
+Expected: `5 passed`。
+
+- [ ] **Step 5: 运行完整工作空间构建**
+
+```bash
+cd /home/robot/WS_xr
+source /opt/ros/humble/setup.bash
+colcon build --symlink-install
+```
+
+Expected: `xr_rm_interfaces`、`xr_rm_input`、`xr_rm_teleop` 和
+`xr_rm_bringup` 全部构建成功。
+
+- [ ] **Step 6: 重新 source 后验证接口和遥操作回归**
+
+```bash
+cd /home/robot/WS_xr
+source /opt/ros/humble/setup.bash
+source install/setup.bash
+ros2 interface show xr_rm_interfaces/msg/XrController
+pytest src/xr_rm_teleop/test/test_orientation_control.py -v
+```
+
+Expected: 接口按 `header/hand/grip/trigger/primary/secondary/axis/pose` 顺序
+显示；姿态控制测试全部通过。
+
+- [ ] **Step 7: 使用 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:=right use_mock:=true
+```
+
+Expected: `udp_controller_receiver` 和 `single_arm_velocity_teleop` 正常启动；
+不出现消息类型、Placo 或 traceback 错误。`timeout` 到期退出属于预期。
+
+- [ ] **Step 8: 检查 diff 和格式**
+
+```bash
+cd /home/robot/WS_xr/src
+git diff --check
+git status --short
+```
+
+Expected: `git diff --check` 无输出；只剩 README、AGENTS 的计划内文档改动。
+
+- [ ] **Step 9: 提交文档**
+
+```bash
+cd /home/robot/WS_xr/src
+git add README.md AGENTS.md
+git commit -m "docs: 更新手柄输入与 Superpowers 规则"
+```
+
+Expected: 只提交 README 和 AGENTS；不执行 push 或 merge。

docs/superpowers/specs/2026-07-29-rm75-feedback-recovery-design.md

@@ -0,0 +1,151 @@
+# RM75 关节反馈与故障恢复设计
+
+## 目标
+
+将当前 RM75 QP 遥操作链路调整为：
+
+- 启动时使用 `rm_get_joint_degree()` 获取实际关节角并初始化 QP。
+- 运行时只把 RealMan UDP `joint_position` 作为连续实际关节反馈。
+- UDP 短暂超时时保持最后一次已限速的安全关节目标，不生成新运动。
+- UDP 持续超时或 CANFD 发送错误时，按明确的同步、停止和人工恢复流程处理。
+- `rm_movej_canfd()` 保持低跟随，控制频率使用 `xr_rm_teleop` 分支的 90 Hz。
+
+不改变现有工作空间/圆柱限位、TCP与关节速度和加速度限制、XR命令超时、安全停止、外设控制及双臂节点名。
+
+## 控制数据源
+
+启动初始化与运行反馈使用不同的数据源：
+
+1. `RealManAdapter` 建立现有唯一厂商连接。
+2. 节点同步调用一次 `rm_get_joint_degree()`。
+3. 查询成功后把7个角度转换为弧度，用于初始化 Placo QP、最后安全目标和关节限速历史。
+4. 查询失败时关闭适配器、打印错误并使节点启动失败，不发送 CANFD。
+5. 正常运行后，QP 的连续实际状态只来自已校验且运动状态正常的 UDP `joint_position`。
+
+同步查询只用于启动、持续反馈超时恢复和 CANFD 错误恢复，不新增连接，不进行常态轮询。
+
+## 状态与转换
+
+### 正常运行
+
+控制定时器以 90 Hz 执行。每个周期读取最新 UDP 关节快照，同步 QP，执行现有目标生成、安全限位、单步 QP、关节速度/加速度限制，然后调用：
+
+```text
+rm_movej_canfd(target_degrees, follow=false, ...)
+```
+
+只有已经通过关节限速并成功发送的目标才能成为“最后安全目标”。
+
+### UDP 短暂超时
+
+UDP 快照年龄超过现有 `command_timeout_sec=0.12` 秒、但未达到 `feedback_resync_timeout_sec=0.5` 秒时：
+
+- 不使用过期反馈同步 QP。
+- 不运行目标生成和 QP。
+- 不更新任何目标、滤波器或限速历史。
+- 若超时前正在遥操作且已有成功发送的安全目标，以 90 Hz 原样重发该目标。
+- 若超时前未在遥操作或没有成功发送的目标，保持停止，不开始 CANFD 输出。
+- 首次进入时打印节流后的警告。
+
+机械臂报警、关节掉使能、关节错误或非有限关节值不是普通超时，仍立即执行安全停止。
+
+### UDP 持续超时
+
+UDP 快照年龄达到 0.5 秒时，每次中断只同步调用一次 `rm_get_joint_degree()`：
+
+- 查询成功：用实际角度重置 QP、最后安全目标和关节限速历史；不生成新运动，继续保持并等待 UDP 恢复。
+- 查询失败：调用 slow-stop，停止 CANFD，进入锁存故障并打印错误。
+
+同一次中断不会反复查询。收到新的有效 UDP 反馈后，查询标志才复位。
+
+### UDP 恢复
+
+UDP 恢复后先持续同步实际关节状态，但不能直接恢复运动：
+
+1. 当前 Grip 必须松开。
+2. 节点清除重新使能要求。
+3. 操作者再次按下 Grip，节点以新的手柄和机械臂实际位姿建立相对控制起点。
+
+### CANFD 错误
+
+`rm_movej_canfd()` 返回错误或抛出异常时：
+
+1. 立即停止后续 CANFD 发送。
+2. 调用 slow-stop。
+3. 打印包含机械臂名称、命令名称和原始错误的终端错误日志。
+4. 调用 `rm_get_joint_degree()` 查询实际关节角。
+5. 查询成功时重置 QP和关节命令历史，但不再发送保持命令；等待有效 UDP 和 Grip 松开后重新按下。
+6. 查询失败时进入锁存故障并打印查询错误。
+
+### 锁存故障
+
+锁存故障只作用于发生错误的机械臂节点：
+
+- 控制定时器不再查询、运行 QP或发送 CANFD。
+- slow-stop 只发送一次。
+- 后续 UDP 恢复或 Grip 操作不能自动解锁。
+- 终端保留明确错误信息，但不在每个周期重复刷屏。
+- 操作者检查后必须重启对应遥操作节点才能恢复。
+
+## 代码边界
+
+### `xr_rm_teleop/xr_rm_teleop/realman_adapter.py`
+
+- 给真实与 mock 适配器增加同步关节角查询能力。
+- 复用现有厂商连接。
+- 校验返回码、数量和有限值，统一返回弧度。
+- 保留 UDP 回调作为运行时快照来源。
+
+### `xr_rm_teleop/xr_rm_teleop/single_arm_velocity_teleop.py`
+
+- 启动时查询并初始化 QP。
+- 增加 `feedback_resync_timeout_sec` 参数，默认 0.5 秒。
+- 校验 `feedback_resync_timeout_sec > command_timeout_sec > 0`。
+- 在现有控制周期内加入保持、一次性重新同步、等待 Grip 重使能和锁存判断。
+- 复用现有 `_safe_stop()`、Grip 重使能和关节限速逻辑，不新增状态机类。
+
+### 配置
+
+以下配置的 `control_rate_hz` 从 125 Hz 改为 90 Hz，并增加相同的 0.5 秒持续超时参数：
+
+- `xr_rm_bringup/config/dual_arm_rm75.yaml`
+- `xr_rm_bringup/config/left_arm_rm75.yaml`
+- `xr_rm_bringup/config/right_arm_rm75.yaml`
+
+三份配置继续使用 `follow: false`，双臂节点名保持 `left_arm_teleop` 和 `right_arm_teleop`。
+
+## 错误日志
+
+以下转换必须写入 ROS2 终端日志：
+
+- 启动关节查询失败：`error`。
+- 首次进入 UDP 短暂超时：`warn`。
+- 持续超时查询开始及成功：`warn`/`info`。
+- 持续超时查询失败并锁存：`error`。
+- CANFD 发送失败：`error`。
+- CANFD 后关节查询失败并锁存：`error`。
+- UDP 恢复并等待 Grip 人工重使能：`info`。
+
+日志包含机械臂名称和失败阶段；周期性路径使用状态转换或节流避免刷屏。
+
+## 测试与验证
+
+使用现有 mock 和单元测试完成，不连接真机：
+
+1. 适配器正确解析 `rm_get_joint_degree()` 成功结果，并拒绝错误码、错误数量和 NaN/Inf。
+2. 启动查询结果初始化 QP 和安全目标；查询失败时节点不能进入控制。
+3. 0.12～0.5 秒反馈超时期间不调用 QP，只重发同一安全目标。
+4. 0.5 秒持续超时只查询一次；成功后等待 UDP 与 Grip，失败后锁存。
+5. CANFD 错误后停止发送并查询；查询成功要求 Grip 重使能，查询失败锁存。
+6. 机械臂报警或掉使能仍立即停止，不能进入保持路径。
+7. 三份配置均使用 90 Hz、0.5 秒持续超时和低跟随。
+
+在工作空间根目录 `/home/robot/WS_xr` 执行：
+
+```bash
+source /opt/ros/humble/setup.bash
+pytest src/xr_rm_teleop/test/test_initial_joint_pose.py
+pytest src/xr_rm_teleop/test/test_joint_control.py
+pytest src/xr_rm_teleop/test/test_orientation_control.py
+colcon build --symlink-install
+```

docs/superpowers/specs/2026-07-29-rm75-qp-convergence-design.md

@@ -0,0 +1,192 @@
+# RM75 QP 收敛与低跟随稳定性优化设计
+
+## 背景
+
+右臂真机以90 Hz、`follow: false`运行时，用户观察到：
+
+- 手柄移动约10 cm后，`target_pose`很快稳定；
+- `current_pose`仍需约3秒缓慢追赶；
+- 运动过程中机械臂存在肉眼可见的轻微晃动。
+
+现场 timing 日志同时表明：
+
+- 控制周期约11.111 ms；
+- 控制回调平均约2.6 ms，最大约6.0 ms；
+- QP平均约0.39 ms；
+- CANFD发送平均约0.21 ms；
+- UDP实际关节反馈平均约25 ms一帧，即约40 Hz。
+
+因此，控制线程、QP单次计算和CANFD调用本身没有耗尽90 Hz周期；慢速发生在
+`target_pose`生成之后。
+
+## 根因
+
+当前 `PlacoIkSolver.solve()` 每次只调用一次：
+
+```python
+self._solver.solve(True)
+```
+
+该调用把一次QP增量应用为 `q + Δq`。与此同时，90 Hz控制循环每次都会先用
+最新实际关节反馈重置Placo模型。由于实际反馈约40 Hz，同一帧反馈通常会被重复
+使用两到三次。
+
+结果是每次下发的关节目标只位于实际关节角前方一小步，而不是当前TCP目标对应的
+收敛关节解。低跟随控制器持续追逐这个短距离移动点，表现为：
+
+- 对稳定TCP目标呈缓慢的渐近追赶；
+- 实际反馈每约25 ms更新一次时，关节目标随反馈发生台阶式修正；
+- 低跟随内部平滑与台阶式关节目标叠加，形成轻微晃动。
+
+本地RM75模型对照结果支持该判断：从右臂初始姿态求解7 cm平移目标时，单次QP
+只产生约7.6 mm TCP位移；在同一次逆解中连续迭代30次后，目标误差可降至接近
+零，计算耗时约3.56 ms。
+
+## 目标
+
+保持现有安全基线并实现：
+
+- 手柄移动10 cm后，机械臂约1秒内稳定到位；
+- 运动和到位后无持续肉眼可见晃动；
+- 控制频率保持90 Hz；
+- `rm_movej_canfd()`保持低跟随；
+- 运行时仍以UDP `joint_position`作为实际关节反馈；
+- 保留工作空间、圆柱、TCP速度、姿态速度、关节速度与关节加速度限制；
+- 保留反馈超时、CANFD错误恢复、Grip重新使能和安全停止逻辑。
+
+## 不在本次范围
+
+- 不启用高跟随；
+- 不提高TCP或关节安全上限；
+- 不修改XR手柄滤波和坐标映射；
+- 不修改UDP反馈周期或增加反馈预测器；
+- 不新增线程、RealMan连接、依赖或状态机；
+- 不处理双臂碰撞检测。
+
+## 方案比较
+
+### 方案一：有限次数迭代QP
+
+每个控制周期仍从实际关节角开始，但在一次 `solve()` 调用内部迭代QP，直到TCP
+目标收敛或达到固定迭代上限。得到的完整关节目标继续经过现有关节速度与加速度
+限幅后才发送。
+
+优点：
+
+- 直接修复单步QP只生成近距离移动点的根因；
+- 不需要预测状态，不会在反馈中断时继续外推；
+- 复用现有限速、错误回退和CANFD发送路径；
+- 本地测量表明计算量可放入90 Hz周期。
+
+缺点：
+
+- 单周期QP耗时会高于当前单步求解；
+- 不可达目标需要明确的未收敛处理。
+
+### 方案二：反馈帧之间维护预测关节状态
+
+仅在新UDP反馈到达时校正模型，其余90 Hz周期从上一条关节命令继续积分QP。
+
+优点：
+
+- 每周期仍只求解一次QP；
+- 可避免同一反馈帧反复重置模型。
+
+缺点：
+
+- 引入预测状态、反馈校正和漂移处理；
+- 反馈与预测偏差可能在校正时产生新的关节跳动；
+- 超时与恢复逻辑需要同时管理实际状态和预测状态。
+
+### 方案三：只调整滤波、速度或高跟随参数
+
+`target_pose`已经快速稳定，继续提高 `max_linear_speed` 或减小目标滤波不能解决
+下游渐近追赶。启用高跟随则违反本次低跟随约束。
+
+## 决策
+
+采用方案一。它在不引入预测状态的情况下直接修复根因，改动范围只涉及Placo
+求解器及其测试。
+
+## 控制数据流
+
+正常运行时的数据流调整为：
+
+```text
+UDP实际关节反馈
+→ 更新Placo实际关节状态和current_pose
+→ 现有XR相对位姿、工作空间、圆柱、滤波和TCP限速
+→ 有限次数迭代QP，得到收敛关节目标
+→ 现有关节速度与加速度限幅
+→ rm_movej_canfd(..., follow=false)
+```
+
+反馈短暂超时仍只以90 Hz重发最后一次已通过限速的关节目标，不运行QP。反馈持续
+超时和CANFD错误仍沿用现有同步、停止与故障锁存逻辑。
+
+## QP迭代规则
+
+`PlacoIkSolver.solve()`按以下规则执行：
+
+1. 校验目标变换。
+2. 记录本次内部迭代前的关节状态。
+3. 调用一次 `self._solver.solve(True)`。
+4. 更新Placo运动学。
+5. 校验本次候选关节状态：
+   - 7个有限数值；
+   - 不违反RM75关节位置限制；
+   - 本次数值迭代步长不超过Placo按 `dt=1/90` 应用的URDF关节速度限制。
+6. 使用位置任务与姿态任务的 `error_norm()`检查收敛：
+   - 位置误差不超过1 mm；
+   - 姿态误差不超过0.005 rad。
+7. 未收敛则继续迭代，最多30次。
+
+30次后仍未收敛，或任一迭代产生非法结果时，抛出异常。节点复用现有
+`_solve_joint_target()`错误路径，在终端限频打印QP失败原因，并保持上一组安全
+关节目标。
+
+内部迭代得到的是逆解目标，不会直接绕过发送限速。最终下发仍必须经过
+`_limit_joint_command_step()`，因此每个90 Hz真实命令继续满足现有
+`joint_max_speed` 和 `joint_max_acc`。
+
+## 晃动抑制
+
+本次不再叠加新的低通滤波器。晃动通过两层现有机制抑制：
+
+1. QP先收敛到当前TCP目标对应的关节解，避免关节目标随40 Hz反馈只前进一小步；
+2. 最终关节目标由现有关节速度与加速度限幅器生成连续90 Hz命令。
+
+若真机仍存在晃动，再根据“目标关节角与实际关节角误差”追加诊断；本次不预先
+引入预测器或额外滤波。
+
+## 性能与安全验收
+
+自动验证：
+
+- 7 cm可达TCP平移目标在一次 `solve()` 后位置误差不超过1 mm；
+- 姿态误差满足0.005 rad阈值；
+- 非法结果和未收敛目标继续触发现有安全回退；
+- 关节命令速度与加速度限幅测试继续通过；
+- `xr_rm_teleop`全部pytest通过；
+- `colcon build --symlink-install`通过；
+- `arm_debug.launch.py arm:=right use_mock:=true`正常启动。
+
+真机由用户验证：
+
+- 手柄快速移动10 cm并保持不动，机械臂约1秒内稳定；
+- 无持续肉眼可见晃动；
+- 连续四个5秒 timing 窗口中 `total` 最大值低于11.111 ms；
+- 无QP失败、反馈超时、CANFD错误或故障锁存日志；
+- 松开Grip后仍立即退出遥操作并执行安全停止。
+
+若单周期 `total` 达到或超过11.111 ms，停止真机运动并降低最大QP迭代次数，
+不得通过提高控制频率或关闭安全检查规避计算超时。
+
+## 文件范围
+
+- 修改 `xr_rm_teleop/xr_rm_teleop/placo_ik_solver.py`。
+- 修改 `xr_rm_teleop/test/test_placo_transforms.py`。
+- 如现有QP失败测试需要补充未收敛原因断言，只精确修改
+  `xr_rm_teleop/test/test_joint_control.py`。
+
+不修改三份机械臂YAML、RealMan适配器、launch、UI、依赖或公开入口。

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

@@ -0,0 +1,99 @@
+# RM75 关节命令提前制动设计
+
+## 背景
+
+右臂真机保持 90 Hz 和 `follow: false`。有界迭代 QP 提高跟随速度后，手柄上下
+移动约 10 mm 时出现整臂剧烈晃动，并且到达目标后仍持续振荡。
+
+现场日志表明控制计算未超时，但存在少量 QP 未收敛警告。离线检查确认：
+
+- 手柄上下移动按现有映射对应机器人 X 方向；
+- 当前初始姿态的关节雅可比条件数约为 116，该方向的逆解对关节运动较敏感；
+- 机器人 X 方向 10 mm 的收敛逆解可能包含最大约 17° 的关节变化；
+- 现有关节命令限幅器只限制速度和加速度，没有根据剩余距离提前制动。
+
+固定 10° 关节目标的离线复现中，现有限幅器运行 3 秒后仍处于约 10.56°、
+-20°/s，证明稳定目标本身也会被反复越过。这与真机“到位后继续晃动”的现象
+一致。
+
+## 目标
+
+- 关节目标稳定后，90 Hz 关节命令提前减速并停止在目标上；
+- 不再因命令限幅器反复越过目标而持续振荡；
+- 保留当前较快的有界迭代 QP；
+- 保留现有关节最大速度和最大加速度限制；
+- 保持 `rm_movej_canfd(..., follow=false)`；
+- 不改变 UDP 反馈、超时保持、CANFD 错误恢复和安全停止行为。
+
+## 不在本次范围
+
+- 不修改 QP 迭代次数、收敛阈值或失败回退；
+- 不修改初始姿态、XR 坐标映射或姿态控制；
+- 不增加奇异点阻尼、预测器、新线程、新依赖或新 ROS 参数；
+- 不修改三份机械臂 YAML、RealMan 适配器、launch 或 UI；
+- 不连接真机，不由 Codex 发送运动命令。
+
+若修复制动后机械臂运动已经平稳，但上下运动仍伴随不可接受的整臂大幅构型变化，
+再单独设计奇异点处理；本次不把两个问题混在同一改动中。
+
+## 方案
+
+只修改 `SingleArmVelocityTeleop._limit_joint_command_step()`。
+
+现有逻辑在目标仍位于运动方向前方时持续加速，只有到达或越过目标后才开始反向
+减速。新逻辑对每个关节使用同一组现有状态：
+
+- 上一次已发送的关节目标；
+- 上一次关节命令速度；
+- 当前 QP 关节目标；
+- 现有关节最大速度、最大加速度和控制周期。
+
+每周期按以下规则生成命令：
+
+1. 计算关节剩余距离和单周期最大速度变化
+   `velocity_step = max_acceleration * dt`。
+2. 根据 90 Hz 离散积分规则，计算“本周期再加速一次、随后以最大允许减速度制动”
+   所需的总距离。
+3. 若关节正在朝目标运动，并且剩余距离已经不大于该制动距离，则本周期开始减速；
+   否则继续朝目标加速，但不超过现有最大速度。
+4. 使用 `velocity_step` 限制本周期速度变化，保持现有加速度上限。
+5. 使用新速度积分得到本周期关节目标。
+6. 当关节已经停下且剩余距离不超过一个最大加速度位移
+   `max_acceleration * dt²` 时，在不违反单周期加速度限制的前提下精确落到目标，
+   避免离散步长形成极小往复振荡。
+
+该逻辑只负责命令轨迹制动，不改变 QP 输出。对于持续移动的目标，若目标突然越过
+当前命令位置，控制器仍优先遵守加速度限制，以最大允许减速度反向；不会为禁止
+瞬时越界而跳变速度。
+
+## 安全行为
+
+- 每周期命令速度绝对值不超过 `joint_max_speed`；
+- 相邻周期间速度变化不超过 `joint_max_acc * dt`；
+- 输出继续校验 NaN 和 Inf；
+- QP 异常仍保持上一组安全目标并在终端限频打印警告；
+- UDP 短暂超时仍以 90 Hz 重发最后一次已限速目标，不运行 QP；
+- UDP 持续超时、CANFD 错误、Grip 重新使能和安全停止逻辑保持不变。
+
+## 测试
+
+先增加失败测试，再修改实现：
+
+1. 对固定 10° 关节目标连续运行限幅器，验证命令不越过目标并最终停止；
+2. 在整个序列中验证速度不超过现有上限；
+3. 验证相邻命令速度变化不超过现有加速度上限；
+4. 保留现有“从静止开始限制首周期加速度”测试；
+5. 运行 `xr_rm_teleop` 全部测试；
+6. 按项目规则运行 `colcon build --symlink-install`；
+7. 使用 `arm_debug.launch.py arm:=right use_mock:=true`验证 90 Hz、低跟随和启动路径。
+
+真机只由用户分阶段验证：先小位移、低风险姿态，确认不再到位后持续振荡，再逐步
+增加位移。若仍有明显整臂构型变化但不再振荡，应停止扩大位移并转入奇异点处理，
+不得通过提高速度、加速度或启用高跟随规避。
+
+## 文件范围
+
+- 修改 `xr_rm_teleop/xr_rm_teleop/single_arm_velocity_teleop.py`；
+- 修改 `xr_rm_teleop/test/test_joint_control.py`；
+- 新增本中文设计文档；
+- 后续新增一份中文实施计划。

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 或依赖。

docs/superpowers/specs/2026-07-30-xrobotoolkit-controller-inputs-design.md

@@ -0,0 +1,140 @@
+# XRoboToolkit 手柄输入扩展设计
+
+## 背景
+
+当前 `xrobotoolkit_to_udp_bridge` 已从 XRoboToolkit PC-Service SDK 读取左右
+手柄摇杆、主键和副键，但 `udp_controller_receiver` 只把 `grip`、`trigger`
+和位姿写入 `XrController`，其余信息在 UDP 到 ROS2 的转换中丢失。
+
+后续项目会使用 LeRobot 同时记录相机、RM75 状态和手柄输入。本次只补齐当前
+明确需要的手柄字段，不实现 LeRobot 录制，不改变现有机械臂控制逻辑。
+
+## 目标
+
+- 将左右手柄摇杆、主键和副键发布到现有 `XrController` 话题。
+- 保持现有 `grip`、`trigger` 和 `pose` 的语义及控制行为不变。
+- 兼容不包含新增字段的旧 UDP 数据包。
+- 使用现有节点、消息和 UDP 协议，不增加依赖或新话题。
+- 更新 README 和 AGENTS，记录接口及 Superpowers 的 Git 操作边界。
+
+## 不在本次范围
+
+- Grip 和 Trigger 原始模拟量。
+- 菜单键、摇杆按键、SDK 时间戳和 bridge 序号。
+- 头显位姿、26 点手部骨骼、身体追踪和 Motion Tracker。
+- LeRobot 数据集录制、相机同步和 RM75 状态采集。
+- 任何机械臂控制参数、安全逻辑或真机行为修改。
+
+## 方案选择
+
+采用直接扩展 `XrController` 的方案。相比新增 `sensor_msgs/Joy` 话题，该方案
+不需要额外同步左右手柄话题；相比继续只保留 UDP JSON，它能让 ROS2 和后续
+LeRobot 适配层直接读取类型明确的数据。
+
+修改消息定义后必须重新构建并重启相关节点。重新构建后的现有遥操作代码仍只读取
+原字段，不需要修改控制逻辑。
+
+## ROS2 消息格式
+
+`XrController.msg` 使用以下固定顺序：
+
+```text
+std_msgs/Header header
+string hand
+
+bool grip
+float32 trigger
+bool primary
+bool secondary
+float32[2] axis
+
+geometry_msgs/Pose pose
+```
+
+字段语义：
+
+- `primary`：左手 X 键，右手 A 键。
+- `secondary`：左手 Y 键，右手 B 键。
+- `axis`：对应手柄摇杆的 `[x, y]`，每个分量限制在 `[-1.0, 1.0]`。
+
+## 数据流
+
+正常链路保持不变：
+
+```text
+XRoboToolkit PC-Service SDK
+→ xrobotoolkit_to_udp_bridge
+→ UDP JSON
+→ udp_controller_receiver
+→ /xr/left_controller、/xr/right_controller
+→ single_arm_velocity_teleop
+```
+
+bridge 继续读取 Grip 和 Trigger 模拟量并应用现有滞回，只是不再把未使用的
+`grip_value`、`trigger_value`、`menu` 和 `axis_click` 放入 UDP JSON。
+
+UDP 中的按钮继续使用现有嵌套结构：
+
+```json
+{
+  "grip": true,
+  "trigger": 0.0,
+  "axis": [0.2, -0.4],
+  "buttons": {
+    "primary": true,
+    "secondary": false
+  },
+  "pos": [0.0, 1.0, 0.0],
+  "quat": [0.0, 0.0, 0.0, 1.0]
+}
+```
+
+`udp_controller_receiver` 将嵌套按钮展平到 ROS2 消息字段。现有遥操作节点忽略
+新增字段，因此目标位姿、夹爪触发和安全停止路径均不变化。
+
+## 兼容与异常处理
+
+- 旧 UDP 包缺少 `axis` 或 `buttons` 时，发布
+  `axis=[0.0, 0.0]`、`primary=false`、`secondary=false`。
+- 新增可选字段格式错误时使用上述默认值，不丢弃有效的 Grip、Trigger 和位姿。
+- bridge 和 receiver 均将摇杆分量限制在 `[-1.0, 1.0]`。
+- 旧包中存在 `menu`、`axis_click` 或其他按钮字段时忽略，不报错。
+- `sample_udp_sender` 保持旧格式，用它验证向后兼容，不为本次需求增加新参数。
+
+## 文件范围
+
+- `xr_rm_interfaces/msg/XrController.msg`
+- `xr_rm_input/xr_rm_input/xrobotoolkit_to_udp_bridge.py`
+- `xr_rm_input/xr_rm_input/udp_controller_receiver.py`
+- `xr_rm_input/test/` 下的一份最小兼容性测试
+- `README.md`
+- `AGENTS.md`
+
+不修改 `xr_rm_teleop` 控制实现及三个机械臂 YAML。
+
+## README 与 AGENTS 规则
+
+README 增加新的手柄字段、UDP 格式和兼容行为说明。
+
+AGENTS 和 README 同时明确：使用 Superpowers 执行任务时，只允许按 skill
+工作流创建本地 Git 提交；不得推送、合并或执行其他远程写操作。skill 如需本地
+worktree 或配套分支，可以创建，但不得将其合并到其他分支。
+
+## 验证
+
+自动验证包括：
+
+- bridge 生成的 UDP payload 只包含确认保留的按钮和摇杆字段。
+- 左手 X/Y 与右手 A/B 正确映射到 `primary/secondary`。
+- receiver 正确发布新增字段。
+- 旧 UDP 包继续发布，新增字段使用默认值。
+- 非法新增字段不会阻断现有 Grip、Trigger 和位姿。
+- 在 `/home/robot/WS_xr` source ROS2 Humble 后运行相关 pytest。
+- 运行 `colcon build --symlink-install`。
+
+运行验证只使用 mock，不连接真机、不移动机械臂、不操作夹爪。
+
+## Git 边界
+
+本设计和后续实现可以按 Superpowers 流程创建本地提交。禁止执行 `git push`、
+创建或合并 PR、合并本地分支以及任何远程写操作。

xr_rm_bringup/config/dual_arm_rm75.yaml

@@ -11,8 +11,9 @@ left_arm_teleop:
   ros__parameters:
     arm_name: left_rm75
     controller_topic: /xr/left_controller
-    control_rate_hz: 125.0
+    control_rate_hz: 90.0
     command_timeout_sec: 0.12
+    feedback_resync_timeout_sec: 0.5
 
     # 位姿目标生成与平滑参数。
     scale: 0.75
@@ -68,8 +69,9 @@ right_arm_teleop:
   ros__parameters:
     arm_name: right_rm75
     controller_topic: /xr/right_controller
-    control_rate_hz: 125.0
+    control_rate_hz: 90.0
     command_timeout_sec: 0.12
+    feedback_resync_timeout_sec: 0.5
 
     scale: 0.75
     deadband_m: 0.001

xr_rm_bringup/config/left_arm_rm75.yaml

@@ -5,8 +5,9 @@ single_arm_velocity_teleop:
   ros__parameters:
     arm_name: left_rm75
     controller_topic: /xr/left_controller
-    control_rate_hz: 125.0
+    control_rate_hz: 90.0
     command_timeout_sec: 0.12
+    feedback_resync_timeout_sec: 0.5
 
     # 手柄相对位姿 -> 目标 TCP 位姿；随后做目标低通、姿态低通和单帧步长限制。
     scale: 1.0

xr_rm_bringup/config/right_arm_rm75.yaml

@@ -5,8 +5,9 @@ single_arm_velocity_teleop:
   ros__parameters:
     arm_name: right_rm75
     controller_topic: /xr/right_controller
-    control_rate_hz: 125.0
+    control_rate_hz: 90.0
     command_timeout_sec: 0.12
+    feedback_resync_timeout_sec: 0.5
 
     scale: 0.7
     deadband_m: 0.001

xr_rm_input/test/test_controller_fields.py

@@ -0,0 +1,122 @@
+import math
+from types import SimpleNamespace
+
+from builtin_interfaces.msg import Time
+from xr_rm_input.udp_controller_receiver import UdpControllerReceiver
+from xr_rm_input.xrobotoolkit_to_udp_bridge import (
+    _buttons_payload,
+    _controller_payload,
+    _stop_controller_payload,
+)
+
+
+def _receiver_without_socket() -> UdpControllerReceiver:
+    receiver = object.__new__(UdpControllerReceiver)
+    receiver._quat_order = "xyzw"
+    receiver.get_clock = lambda: SimpleNamespace(
+        now=lambda: SimpleNamespace(to_msg=lambda: Time())
+    )
+    return receiver
+
+
+def test_bridge_payload_contains_only_selected_controller_inputs() -> None:
+    buttons = _buttons_payload(
+        primary=lambda: True,
+        secondary=lambda: False,
+    )
+    payload = _controller_payload(
+        hand="left",
+        pose=[1.0, 2.0, 3.0, 0.0, 0.0, 0.0, 1.0],
+        axis=[2.0, -2.0],
+        buttons=buttons,
+        grip_pressed=True,
+        trigger_pressed=False,
+    )
+
+    assert payload == {
+        "hand": "left",
+        "grip": True,
+        "trigger": 0.0,
+        "pos": [1.0, 2.0, 3.0],
+        "quat": [0.0, 0.0, 0.0, 1.0],
+        "pose_valid": True,
+        "pose_source": "xrobotoolkit",
+        "axis": [1.0, -1.0],
+        "buttons": {
+            "primary": True,
+            "secondary": False,
+        },
+    }
+
+
+def test_stop_payload_uses_neutral_selected_inputs() -> None:
+    payload = _stop_controller_payload("right")
+
+    assert payload["axis"] == [0.0, 0.0]
+    assert payload["buttons"] == {
+        "primary": False,
+        "secondary": False,
+    }
+    assert "grip_value" not in payload
+    assert "trigger_value" not in payload
+
+
+def test_receiver_publishes_selected_controller_inputs() -> None:
+    msg = _receiver_without_socket()._payload_to_msg(
+        {
+            "grip": True,
+            "trigger": 1.0,
+            "axis": [2.0, -2.0],
+            "buttons": {
+                "primary": True,
+                "secondary": False,
+            },
+            "pos": [1.0, 2.0, 3.0],
+            "quat": [0.0, 0.0, 0.0, 1.0],
+        },
+        "left",
+    )
+
+    assert msg.primary is True
+    assert msg.secondary is False
+    assert list(msg.axis) == [1.0, -1.0]
+
+
+def test_receiver_defaults_invalid_optional_inputs() -> None:
+    msg = _receiver_without_socket()._payload_to_msg(
+        {
+            "grip": True,
+            "trigger": 0.4,
+            "axis": [math.nan, 0.0],
+            "buttons": [],
+            "pos": [1.0, 2.0, 3.0],
+            "quat": [0.0, 0.0, 0.0, 1.0],
+        },
+        "right",
+    )
+
+    assert msg.primary is False
+    assert msg.secondary is False
+    assert list(msg.axis) == [0.0, 0.0]
+    assert msg.grip is True
+    assert abs(msg.trigger - 0.4) < 1e-6
+    assert msg.pose.position.x == 1.0
+    assert msg.pose.position.y == 2.0
+    assert msg.pose.position.z == 3.0
+
+
+def test_receiver_defaults_missing_legacy_optional_inputs() -> None:
+    msg = _receiver_without_socket()._payload_to_msg(
+        {
+            "grip": True,
+            "trigger": 0.0,
+            "pos": [0.0, 1.0, 0.0],
+            "quat": [0.0, 0.0, 0.0, 1.0],
+        },
+        "left",
+    )
+
+    assert msg.primary is False
+    assert msg.secondary is False
+    assert list(msg.axis) == [0.0, 0.0]
+    assert msg.grip is True

xr_rm_input/xr_rm_input/udp_controller_receiver.py

@@ -1,10 +1,11 @@
 """XR 手柄 UDP 接收节点。
 
-从 UDP JSON 数据包中解析左右手柄位姿、握持键和扳机值，并发布为
-`xr_rm_interfaces/XrController` 消息，供遥操作和夹爪节点订阅。
+从 UDP JSON 数据包中解析左右手柄位姿、Grip、Trigger、摇杆和主副按键，
+并发布为 `xr_rm_interfaces/XrController` 消息，供遥操作和夹爪节点订阅。
 """
 
 import json
+import math
 import socket
 from collections.abc import Iterable, Mapping
 from typing import Any
@@ -124,6 +125,8 @@ class UdpControllerReceiver(Node):
         pos, quat = self._extract_pose(payload)
         if len(pos) != 3 or len(quat) != 4:
             raise ValueError("expected pos[3] and quat[4]")
+        axis = self._optional_axis(payload.get("axis"))
+        primary, secondary = self._optional_buttons(payload.get("buttons"))
 
         msg = XrController()
         msg.header.stamp = self.get_clock().now().to_msg()
@@ -144,6 +147,9 @@ class UdpControllerReceiver(Node):
 
         msg.grip = grip
         msg.trigger = self._clamp_float(payload.get("trigger", 0.0), 0.0, 1.0)
+        msg.primary = primary
+        msg.secondary = secondary
+        msg.axis = axis
         msg.pose.position.x = float(pos[0])
         msg.pose.position.y = float(pos[1])
         msg.pose.position.z = float(pos[2])
@@ -213,6 +219,28 @@ class UdpControllerReceiver(Node):
             raise ValueError("expected 3D position")
         return [float(item) for item in vector]
 
+    @staticmethod
+    def _optional_axis(value: Any) -> list[float]:
+        try:
+            axis = [float(item) for item in value]
+        except (TypeError, ValueError):
+            return [0.0, 0.0]
+        if len(axis) != 2 or not all(math.isfinite(item) for item in axis):
+            return [0.0, 0.0]
+        return [
+            min(max(axis[0], -1.0), 1.0),
+            min(max(axis[1], -1.0), 1.0),
+        ]
+
+    @classmethod
+    def _optional_buttons(cls, value: Any) -> tuple[bool, bool]:
+        if not isinstance(value, Mapping):
+            return False, False
+        return (
+            cls._as_bool(value.get("primary", False)),
+            cls._as_bool(value.get("secondary", False)),
+        )
+
     def _quaternion(self, value: Any) -> list[float]:
         if isinstance(value, Mapping):
             if self._quat_order == "wxyz":

xr_rm_input/xr_rm_input/xrobotoolkit_to_udp_bridge.py

@@ -1,8 +1,8 @@
 """XRoboToolkit SDK 到当前 UDP controller JSON 协议的桥接脚本。
 
 该脚本运行在安装了 `xrobotoolkit_sdk` 的 Python 环境中，从官方
-XRoboToolkit PC-Service SDK 读取 PICO 左右手柄 pose / grip / trigger，
-再发送现有 `udp_controller_receiver` 已兼容的 UDP JSON 包。
+XRoboToolkit PC-Service SDK 读取 PICO 左右手柄 pose、Grip、Trigger、
+摇杆和主副按键，再发送 `udp_controller_receiver` 兼容的 UDP JSON 包。
 """
 
 import argparse
@@ -94,8 +94,6 @@ def _controller_payload(
     *,
     hand: str,
     pose: Any,
-    grip_value: Any,
-    trigger_value: Any,
     axis: Any,
     buttons: dict[str, bool],
     grip_pressed: bool,
@@ -103,8 +101,6 @@ def _controller_payload(
     pose_valid: bool = True,
 ) -> dict[str, Any]:
     pos, quat = _pose_to_pos_quat(pose) if pose_valid else (ZERO_POS.copy(), IDENTITY_QUAT.copy())
-    grip_float = _clamp_float(grip_value, 0.0, 1.0)
-    trigger_float = _clamp_float(trigger_value, 0.0, 1.0)
     return {
         "hand": hand,
         "grip": pose_valid and grip_pressed,
@@ -113,8 +109,6 @@ def _controller_payload(
         "quat": quat,
         "pose_valid": pose_valid,
         "pose_source": POSE_SOURCE,
-        "grip_value": grip_float,
-        "trigger_value": trigger_float,
         "axis": _safe_axis(axis),
         "buttons": buttons,
     }
@@ -129,15 +123,10 @@ def _stop_controller_payload(hand: str) -> dict[str, Any]:
         "quat": IDENTITY_QUAT.copy(),
         "pose_valid": False,
         "pose_source": POSE_SOURCE,
-        "grip_value": 0.0,
-        "trigger_value": 0.0,
         "axis": [0.0, 0.0],
         "buttons": {
-            "grip": False,
             "primary": False,
             "secondary": False,
-            "menu": False,
-            "axis_click": False,
         },
     }
 
@@ -181,18 +170,12 @@ def _send_stop_packets(
 
 def _buttons_payload(
     *,
-    grip: bool,
     primary: Callable[[], Any],
     secondary: Callable[[], Any],
-    menu: Callable[[], Any],
-    axis_click: Callable[[], Any],
 ) -> dict[str, bool]:
     return {
-        "grip": grip,
         "primary": _safe_bool(primary),
         "secondary": _safe_bool(secondary),
-        "menu": _safe_bool(menu),
-        "axis_click": _safe_bool(axis_click),
     }
 
 
@@ -321,15 +304,10 @@ def main(argv: Sequence[str] | None = None) -> None:
                     "left": _controller_payload(
                         hand="left",
                         pose=xrt.get_left_controller_pose(),
-                        grip_value=left_grip_value,
-                        trigger_value=left_trigger_value,
                         axis=xrt.get_left_axis(),
                         buttons=_buttons_payload(
-                            grip=left_grip,
                             primary=xrt.get_X_button,
                             secondary=xrt.get_Y_button,
-                            menu=xrt.get_left_menu_button,
-                            axis_click=xrt.get_left_axis_click,
                         ),
                         grip_pressed=left_grip,
                         trigger_pressed=left_trigger,
@@ -337,15 +315,10 @@ def main(argv: Sequence[str] | None = None) -> None:
                     "right": _controller_payload(
                         hand="right",
                         pose=xrt.get_right_controller_pose(),
-                        grip_value=right_grip_value,
-                        trigger_value=right_trigger_value,
                         axis=xrt.get_right_axis(),
                         buttons=_buttons_payload(
-                            grip=right_grip,
                             primary=xrt.get_A_button,
                             secondary=xrt.get_B_button,
-                            menu=xrt.get_right_menu_button,
-                            axis_click=xrt.get_right_axis_click,
                         ),
                         grip_pressed=right_grip,
                         trigger_pressed=right_trigger,

xr_rm_interfaces/msg/XrController.msg

@@ -1,5 +1,10 @@
 std_msgs/Header header
 string hand
+
 bool grip
 float32 trigger
+bool primary
+bool secondary
+float32[2] axis
+
 geometry_msgs/Pose pose

xr_rm_teleop/test/test_initial_joint_pose.py

@@ -202,6 +202,60 @@ def _install_fake_sdk(monkeypatch, *, push_return=0, send_feedback=True):
     return SimpleNamespace(RoboticArm=FakeArm)
 
 
+def test_joint_degree_query_returns_validated_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,
+        "127.0.0.1",
+        8090,
+    )
+    adapter._arm = FakeArm()
+
+    snapshot = adapter.read_joint_state()
+
+    assert snapshot.positions == pytest.approx(
+        [math.radians(value) for value in [0, 10, -20, 30, -40, 50, -60]]
+    )
+    assert snapshot.read_duration_ms is not None
+
+
+@pytest.mark.parametrize(
+    "result",
+    [
+        (7, [0.0] * 7),
+        (0, [0.0] * 6),
+        (0, [0.0, 0.0, 0.0, math.nan, 0.0, 0.0, 0.0]),
+    ],
+)
+def test_joint_degree_query_rejects_sdk_errors_and_invalid_values(result) -> None:
+    adapter = RealManAdapter(
+        "127.0.0.1",
+        8080,
+        0,
+        "127.0.0.1",
+        8090,
+    )
+    adapter._arm = SimpleNamespace(rm_get_joint_degree=lambda: result)
+
+    with pytest.raises((RuntimeError, ValueError)):
+        adapter.read_joint_state()
+
+
+def test_mock_joint_query_uses_current_mock_positions() -> None:
+    adapter = MockRealManAdapter([1.0, 2.0, 3.0, 4.0, 5.0, 6.0, 7.0])
+
+    snapshot = adapter.read_joint_state()
+
+    assert snapshot.positions == pytest.approx(
+        [math.radians(value) for value in [1, 2, 3, 4, 5, 6, 7]]
+    )
+
+
 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))
@@ -253,7 +307,12 @@ def test_invalid_udp_feedback_does_not_replace_snapshot(state) -> None:
 
     adapter._on_realtime_arm_state(state)
 
-    assert adapter.get_latest_joint_state() == before
+    after = adapter.get_latest_joint_state()
+    assert after is not None
+    assert before is not None
+    assert after.positions == before.positions
+    assert after.received_at == before.received_at
+    assert after.motion_ready is False
 
 
 def test_udp_joint_fault_marks_snapshot_unready() -> None:
@@ -353,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",
@@ -361,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,
     )
 
@@ -368,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

@@ -30,34 +30,184 @@ class FakeTime:
         return SimpleNamespace(nanoseconds=0)
 
 
-def test_missing_or_stale_feedback_does_not_enable_qp() -> None:
+def test_startup_joint_query_initializes_qp_and_command_history() -> None:
+    positions = [0.1] * 7
+    pose = np.eye(4)
     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._arm_name = "right_rm75"
     teleop._adapter = SimpleNamespace(
-        get_latest_joint_state=lambda: JointStateSnapshot(
-            [0.0] * 7,
-            time.monotonic() - 1.0,
+        read_joint_state=lambda: JointStateSnapshot(
+            positions,
+            time.monotonic(),
         )
     )
-    assert teleop._fresh_joint_state() is None
+    teleop._ik_solver = SimpleNamespace(
+        update_joint_state=lambda joints: pose
+    )
+    teleop.get_logger = lambda: FakeLogger()
+
+    teleop._initialize_joint_state()
+
+    assert teleop._latest_joint_positions == positions
+    assert teleop._last_valid_joint_target == positions
+    assert teleop._last_joint_command_target == positions
+    assert teleop._last_joint_command_velocity == [0.0] * 7
+    assert teleop._last_current_pose is pose
 
 
-def test_disabled_joint_feedback_does_not_enable_qp() -> None:
+def test_startup_joint_query_failure_closes_adapter() -> None:
+    class FailingAdapter:
+        def __init__(self):
+            self.close_calls = 0
+
+        def read_joint_state(self):
+            raise RuntimeError("rm_get_joint_degree failed with code 7")
+
+        def close(self):
+            self.close_calls += 1
+
+    errors = []
     teleop = object.__new__(SingleArmVelocityTeleop)
-    teleop._command_timeout_sec = 0.12
-    teleop._adapter = SimpleNamespace(
-        get_latest_joint_state=lambda: JointStateSnapshot(
+    teleop._arm_name = "left_rm75"
+    teleop._adapter = FailingAdapter()
+    teleop.get_logger = lambda: SimpleNamespace(
+        error=lambda message: errors.append(message)
+    )
+
+    with pytest.raises(RuntimeError, match="code 7"):
+        teleop._initialize_joint_state()
+
+    assert teleop._adapter.close_calls == 1
+    assert "left_rm75" in errors[0]
+    assert "启动关节同步失败" in errors[0]
+
+
+def _timeout_teleop(adapter) -> SingleArmVelocityTeleop:
+    teleop = object.__new__(SingleArmVelocityTeleop)
+    teleop._adapter = adapter
+    teleop._arm_name = "right_rm75"
+    teleop._follow = False
+    teleop._active = True
+    teleop._joint_feedback_ready = True
+    teleop._grip_rearm_required = False
+    teleop._feedback_resync_attempted = False
+    teleop._control_fault_latched = False
+    teleop._last_joint_command_target = [0.1] * 7
+    teleop._last_joint_command_velocity = [0.0] * 7
+    teleop._latest_joint_positions = [0.1] * 7
+    teleop._last_valid_joint_target = [0.1] * 7
+    teleop._last_current_pose = np.eye(4)
+    teleop._controller_start = None
+    teleop._controller_orientation_start = None
+    teleop._robot_start_transform = None
+    teleop._filtered_target = None
+    teleop._filtered_orientation_target = None
+    teleop._last_sent_target = None
+    teleop._last_sent_orientation = None
+    teleop._last_command_time = None
+    teleop._ik_solver = SimpleNamespace(
+        update_joint_state=lambda joints: np.eye(4)
+    )
+    teleop._stop_sent = False
+    teleop._feedback_resync_timeout_sec = 0.5
+    teleop._publish_stop_debug = lambda: None
+    teleop.get_logger = lambda: FakeLogger()
+    return teleop
+
+
+def test_missing_or_disabled_joint_snapshot_is_not_motion_ready() -> None:
+    assert not SingleArmVelocityTeleop._joint_snapshot_is_motion_ready(None)
+    assert not SingleArmVelocityTeleop._joint_snapshot_is_motion_ready(
+        JointStateSnapshot(
             [0.0] * 7,
             time.monotonic(),
             motion_ready=False,
         )
     )
 
-    assert teleop._fresh_joint_state() is None
+
+def test_short_udp_timeout_repeats_last_limited_target_without_query() -> None:
+    sends = []
+    adapter = SimpleNamespace(
+        send_joint_target=lambda joints, follow: sends.append(
+            (list(joints), follow)
+        ),
+        read_joint_state=lambda: pytest.fail("query must not run"),
+        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:
+    stop_calls = []
+    adapter = SimpleNamespace(
+        send_joint_target=lambda joints, follow: pytest.fail(
+            "inactive control must not start CANFD output"
+        ),
+        read_joint_state=lambda: pytest.fail("query must not run"),
+        stop=lambda: stop_calls.append(True),
+    )
+    teleop = _timeout_teleop(adapter)
+    teleop._active = False
+
+    teleop._handle_stale_joint_feedback(0.2)
+
+    assert len(stop_calls) == 1
+
+
+def test_persistent_udp_timeout_queries_once_and_holds_actual_position() -> None:
+    sends = []
+    query_calls = []
+    adapter = SimpleNamespace(
+        send_joint_target=lambda joints, follow: sends.append(list(joints)),
+        read_joint_state=lambda: (
+            query_calls.append(True)
+            or JointStateSnapshot([0.2] * 7, time.monotonic())
+        ),
+        stop=lambda: None,
+    )
+    teleop = _timeout_teleop(adapter)
+
+    teleop._handle_stale_joint_feedback(0.5)
+    teleop._handle_stale_joint_feedback(0.6)
+
+    assert len(query_calls) == 1
+    assert sends == [[0.2] * 7, [0.2] * 7]
+    assert teleop._last_valid_joint_target == [0.2] * 7
+    assert teleop._last_joint_command_velocity == [0.0] * 7
+
+
+def test_persistent_udp_timeout_query_failure_latches_control() -> None:
+    stop_calls = []
+    adapter = SimpleNamespace(
+        send_joint_target=lambda joints, follow: pytest.fail(
+            "CANFD must stop after query failure"
+        ),
+        read_joint_state=lambda: (_ for _ in ()).throw(
+            RuntimeError("rm_get_joint_degree failed with code 7")
+        ),
+        stop=lambda: stop_calls.append(True),
+    )
+    teleop = _timeout_teleop(adapter)
+
+    teleop._handle_stale_joint_feedback(0.5)
+    teleop._handle_stale_joint_feedback(0.6)
+
+    assert teleop._control_fault_latched
+    assert len(stop_calls) == 1
 
 
 def test_joint_command_step_limits_acceleration_from_rest() -> None:
@@ -75,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):
@@ -106,6 +364,8 @@ def test_feedback_fault_blocks_grip_until_release() -> None:
         update_joint_state=lambda joints: np.eye(4)
     )
     teleop._grip_rearm_required = True
+    teleop._control_fault_latched = False
+    teleop._feedback_resync_attempted = False
     teleop.get_clock = lambda: FakeClock()
     teleop.get_logger = lambda: FakeLogger()
     stopped = []
@@ -125,40 +385,6 @@ def test_feedback_fault_blocks_grip_until_release() -> None:
     assert len(entered) == 1
 
 
-def test_stale_feedback_stops_before_active_control() -> None:
-    stopped = []
-    entered = []
-    teleop = object.__new__(SingleArmVelocityTeleop)
-    teleop._adapter = SimpleNamespace(
-        get_latest_joint_state=lambda: JointStateSnapshot(
-            [0.0] * 7,
-            time.monotonic() - 1.0,
-        )
-    )
-    teleop._command_timeout_sec = 0.12
-    teleop._joint_feedback_ready = True
-    teleop._arm_name = "right_rm75"
-    teleop._last_msg = SimpleNamespace(
-        grip=True,
-        pose=SimpleNamespace(
-            position=SimpleNamespace(x=0.0, y=0.0, z=0.0),
-            orientation=SimpleNamespace(x=0.0, y=0.0, z=0.0, w=1.0),
-        ),
-    )
-    teleop._last_msg_time = FakeTime()
-    teleop._active = False
-    teleop._enable_orientation_control = False
-    teleop.get_clock = lambda: SimpleNamespace(now=lambda: FakeTime())
-    teleop.get_logger = lambda: FakeLogger()
-    teleop._safe_stop = lambda reset_active: stopped.append(reset_active)
-    teleop._enter_active_control = lambda *args: entered.append(args)
-
-    teleop._control_tick()
-
-    assert stopped == [True]
-    assert entered == []
-
-
 def test_first_feedback_initializes_last_valid_target_without_solving() -> None:
     class FakeSolver:
         def __init__(self) -> None:
@@ -318,34 +544,58 @@ def test_timing_stats_logs_summary_and_clears_window() -> None:
     assert all(not samples for samples in teleop._timing_samples.values())
 
 
-def test_joint_send_failure_requests_slow_stop_and_resets_control() -> None:
-    class FailingAdapter:
-        def __init__(self) -> None:
+def test_canfd_error_stops_queries_and_requires_grip_rearm() -> None:
+    class RecoveringAdapter:
+        def __init__(self):
             self.stop_calls = 0
+            self.read_calls = 0
 
         def send_joint_target(self, joints, follow):
             del joints, follow
-            raise RuntimeError("send failed")
+            raise RuntimeError("rm_movej_canfd failed with code 9")
 
         def stop(self):
             self.stop_calls += 1
 
-    reset_calls = []
-    teleop = object.__new__(SingleArmVelocityTeleop)
-    teleop._adapter = FailingAdapter()
-    teleop._follow = False
-    teleop._arm_name = "left_rm75"
-    teleop._stop_sent = False
-    teleop._last_joint_command_target = [0.0] * 7
-    teleop._last_joint_command_velocity = [0.0] * 7
+        def read_joint_state(self):
+            self.read_calls += 1
+            return JointStateSnapshot([0.2] * 7, time.monotonic())
+
+    teleop = _timeout_teleop(RecoveringAdapter())
     teleop._joint_command_max_speed = math.radians(180.0)
     teleop._joint_command_max_acceleration = math.radians(300.0)
-    teleop._dt = 1.0 / 125.0
-    teleop.get_logger = lambda: FakeLogger()
-    teleop._safe_stop = lambda reset_active: reset_calls.append(reset_active)
+    teleop._dt = 1.0 / 90.0
 
-    sent = teleop._send_joint_target([0.1] * 7)
+    sent = teleop._send_joint_target([0.3] * 7)
 
     assert not sent
     assert teleop._adapter.stop_calls == 1
-    assert reset_calls == [True]
+    assert teleop._adapter.read_calls == 1
+    assert not teleop._control_fault_latched
+    assert teleop._grip_rearm_required
+    assert teleop._last_joint_command_target == [0.2] * 7
+
+
+def test_canfd_error_latches_when_joint_query_also_fails() -> None:
+    class FailingAdapter:
+        def __init__(self):
+            self.stop_calls = 0
+
+        def send_joint_target(self, joints, follow):
+            del joints, follow
+            raise RuntimeError("rm_movej_canfd failed with code 9")
+
+        def stop(self):
+            self.stop_calls += 1
+
+        def read_joint_state(self):
+            raise RuntimeError("rm_get_joint_degree failed with code 7")
+
+    teleop = _timeout_teleop(FailingAdapter())
+    teleop._joint_command_max_speed = math.radians(180.0)
+    teleop._joint_command_max_acceleration = math.radians(300.0)
+    teleop._dt = 1.0 / 90.0
+
+    assert not teleop._send_joint_target([0.3] * 7)
+    assert teleop._control_fault_latched
+    assert teleop._adapter.stop_calls == 1

xr_rm_teleop/test/test_orientation_control.py

@@ -179,6 +179,8 @@ def test_invalid_controller_quaternion_stops_current_tick() -> None:
     teleop._last_valid_joint_target = None
     teleop._last_current_pose = None
     teleop._joint_feedback_ready = True
+    teleop._control_fault_latched = False
+    teleop._feedback_resync_attempted = False
     stopped = []
     teleop.get_clock = lambda: FakeClock()
     teleop.get_logger = lambda: FakeLogger()

xr_rm_teleop/test/test_placo_transforms.py

@@ -1,10 +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,
 )
@@ -45,6 +48,88 @@ def test_fixed_urdf_has_seven_moving_joints_and_omnipicker_tcp() -> None:
     assert tcp_joint.find("origin").attrib["rpy"] == "0 0 0"
 
 
+def _rm75_placo_solver() -> tuple[PlacoIkSolver, list[float]]:
+    pytest.importorskip("placo")
+    urdf_path = (
+        Path(__file__).resolve().parents[1]
+        / "models"
+        / "rm75_omnipicker"
+        / "urdf"
+        / "RM75-B_OmniPicker_fixed.urdf"
+    )
+    joints = [
+        math.radians(value)
+        for value in [
+            -90.14,
+            3.76,
+            -86.89,
+            87.89,
+            -96.53,
+            -79.62,
+            -90.04,
+        ]
+    ]
+    return PlacoIkSolver(str(urdf_path), 1.0 / 90.0), joints
+
+
+def test_qp_solve_converges_to_reachable_tcp_target() -> None:
+    solver, joints = _rm75_placo_solver()
+    start_pose = solver.update_joint_state(joints)
+    target_pose = start_pose.copy()
+    target_pose[0, 3] += 0.07
+
+    result = solver.solve(target_pose)
+    reached_pose = solver.update_joint_state(result)
+    position_error = np.linalg.norm(
+        target_pose[:3, 3] - reached_pose[:3, 3]
+    )
+    rotation_delta = (
+        target_pose[:3, :3] @ reached_pose[:3, :3].T
+    )
+    orientation_error = math.acos(
+        float(
+            np.clip(
+                (np.trace(rotation_delta) - 1.0) * 0.5,
+                -1.0,
+                1.0,
+            )
+        )
+    )
+
+    assert 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

@@ -1,4 +1,4 @@
-"""RM75 的 Placo 0.9.4 单步 QP 逆解。"""
+"""RM75 的 Placo 0.9.4 有界迭代 QP 逆解。"""
 
 from __future__ import annotations
 
@@ -10,6 +10,9 @@ import numpy as np
 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 = 2e-3
+QP_ORIENTATION_TOLERANCE_RAD = 5e-3
 
 
 def _validated_transform(transform: np.ndarray) -> np.ndarray:
@@ -118,25 +121,74 @@ class PlacoIkSolver:
             self._frame_task.T_world_frame = base_to_tool.copy()
         return base_to_tool.copy()
 
+    def _target_errors(self) -> tuple[float, float]:
+        position_task = self._frame_task.position()
+        orientation_task = self._frame_task.orientation()
+        position_task.update()
+        orientation_task.update()
+        return (
+            float(position_task.error_norm()),
+            float(orientation_task.error_norm()),
+        )
+
     def solve(self, target_tool_pose: np.ndarray) -> list[float]:
         if self._actual_joints is None:
             raise RuntimeError("joint state must be initialized before QP solve")
-        self._frame_task.T_world_frame = _validated_transform(target_tool_pose)
-        self._solver.solve(True)
+        self._frame_task.T_world_frame = _validated_transform(
+            target_tool_pose
+        )
         result = np.asarray(
             self._robot.state.q[RM75_Q_SLICE],
             dtype=float,
         ).copy()
-        self._validate_result(result)
-        return result.tolist()
+        position_error, orientation_error = self._target_errors()
+        if (
+            position_error <= QP_POSITION_TOLERANCE_M
+            and orientation_error <= QP_ORIENTATION_TOLERANCE_RAD
+        ):
+            return result.tolist()
 
-    def _validate_result(self, result: np.ndarray) -> None:
+        for _ in range(QP_MAX_ITERATIONS):
+            previous = result
+            self._solver.solve(True)
+            self._robot.update_kinematics()
+            result = np.asarray(
+                self._robot.state.q[RM75_Q_SLICE],
+                dtype=float,
+            ).copy()
+            self._validate_result(result, previous)
+            position_error, orientation_error = self._target_errors()
+            if (
+                position_error <= QP_POSITION_TOLERANCE_M
+                and orientation_error <= QP_ORIENTATION_TOLERANCE_RAD
+            ):
+                return result.tolist()
+
+        raise RuntimeError(
+            "QP did not converge after "
+            f"{QP_MAX_ITERATIONS} iterations: "
+            f"position_error={position_error:.6f} m, "
+            f"orientation_error={orientation_error:.6f} rad"
+        )
+
+    def _validate_result(
+        self,
+        result: np.ndarray,
+        reference: np.ndarray | None = None,
+    ) -> 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")
+        if reference is None:
+            reference = self._actual_joints
+        if reference is None:
+            raise RuntimeError("joint state has not been initialized")
+        reference = np.asarray(reference, dtype=float)
+        if reference.shape != (7,) or not np.isfinite(reference).all():
+            raise ValueError("QP reference must contain 7 finite values")
         max_step = self._velocity_limits * self._dt + 1e-9
-        if np.any(np.abs(result - self._actual_joints) > max_step):
+        if np.any(np.abs(result - reference) > max_step):
             raise ValueError("QP result violates RM75 one-cycle velocity limits")

xr_rm_teleop/xr_rm_teleop/realman_adapter.py

@@ -59,6 +59,9 @@ class MockRealManAdapter:
             time.monotonic(),
         )
 
+    def read_joint_state(self) -> JointStateSnapshot:
+        return self.get_latest_joint_state()
+
     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):
@@ -185,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,
@@ -228,6 +231,25 @@ class RealManAdapter:
                 self._latest_joint_state.motion_ready,
             )
 
+    def read_joint_state(self) -> JointStateSnapshot:
+        self._require_arm()
+        started_at = time.monotonic()
+        result = self._arm.rm_get_joint_degree()
+        finished_at = time.monotonic()
+        if not isinstance(result, tuple) or len(result) != 2:
+            raise RuntimeError(
+                f"rm_get_joint_degree returned invalid result: {result!r}"
+            )
+        self._check_return(result, "rm_get_joint_degree")
+        return JointStateSnapshot(
+            self._joint_positions_from_degrees(
+                result[1],
+                "rm_get_joint_degree",
+            ),
+            finished_at,
+            (finished_at - started_at) * 1000.0,
+        )
+
     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):
@@ -305,17 +327,10 @@ class RealManAdapter:
                 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")
+            positions = self._joint_positions_from_degrees(
+                data.joint_status.joint_position,
+                "RM75 UDP feedback",
+            )
             joint_enabled = [
                 bool(value) for value in data.joint_status.joint_en_flag
             ]
@@ -367,12 +382,44 @@ class RealManAdapter:
             self._feedback_fault_logged = False
             self._feedback_ready.set()
         except Exception as exc:
+            with self._joint_state_lock:
+                if self._latest_joint_state is not None:
+                    current = self._latest_joint_state
+                    self._latest_joint_state = JointStateSnapshot(
+                        list(current.positions),
+                        current.received_at,
+                        current.read_duration_ms,
+                        current.update_interval_ms,
+                        False,
+                    )
             if not self._feedback_fault_logged:
                 self._log_warn(
                     f"RealMan UDP realtime feedback invalid: {exc}"
                 )
                 self._feedback_fault_logged = True
 
+    @staticmethod
+    def _joint_positions_from_degrees(
+        values: Any,
+        source: str,
+    ) -> list[float]:
+        try:
+            degrees = list(values)
+        except TypeError as exc:
+            raise ValueError(
+                f"{source} must contain 7 numeric joints"
+            ) from exc
+        if len(degrees) != 7 or not all(
+            isinstance(value, Number) for value in degrees
+        ):
+            raise ValueError(
+                f"{source} 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(f"{source} contains NaN/Inf")
+        return positions
+
     def _log_motion_status_transition(
         self,
         status: tuple[Any, ...],

xr_rm_teleop/xr_rm_teleop/single_arm_velocity_teleop.py

@@ -177,8 +177,9 @@ class SingleArmVelocityTeleop(Node):
 
         self.declare_parameter("arm_name", "rm75")
         self.declare_parameter("controller_topic", "/xr/right_controller")
-        self.declare_parameter("control_rate_hz", 125.0)
+        self.declare_parameter("control_rate_hz", 90.0)
         self.declare_parameter("command_timeout_sec", 0.12)
+        self.declare_parameter("feedback_resync_timeout_sec", 0.5)
         self.declare_parameter("scale", 1.0)
         self.declare_parameter("deadband_m", 0.001)
         self.declare_parameter("target_filter_alpha", 0.65)
@@ -234,6 +235,9 @@ class SingleArmVelocityTeleop(Node):
             raise ValueError("control_rate_hz must be > 0")
         self._dt = 1.0 / control_rate_hz
         self._command_timeout_sec = float(self.get_parameter("command_timeout_sec").value)
+        self._feedback_resync_timeout_sec = float(
+            self.get_parameter("feedback_resync_timeout_sec").value
+        )
         self._scale = float(self.get_parameter("scale").value)
         self._deadband_m = float(self.get_parameter("deadband_m").value)
         self._target_filter_alpha = float(self.get_parameter("target_filter_alpha").value)
@@ -287,6 +291,8 @@ class SingleArmVelocityTeleop(Node):
         self._last_joint_command_velocity: list[float] | None = None
         self._joint_feedback_ready = False
         self._grip_rearm_required = False
+        self._feedback_resync_attempted = False
+        self._control_fault_latched = False
         self._stop_sent = True
         self._trigger_tool_open = True
         self._last_trigger_pressed: bool | None = None
@@ -321,6 +327,7 @@ class SingleArmVelocityTeleop(Node):
         )
         self._adapter = self._make_adapter()
         self._adapter.connect()
+        self._initialize_joint_state()
         self._setup_tool_control()
 
         debug_ns = f"{self._debug_topic_prefix}/{self._arm_name}"
@@ -374,6 +381,31 @@ class SingleArmVelocityTeleop(Node):
             canfd_radio=int(self.get_parameter("canfd_radio").value),
         )
 
+    def _initialize_joint_state(self) -> None:
+        try:
+            self._reset_joint_state(self._adapter.read_joint_state())
+        except Exception as exc:
+            self.get_logger().error(
+                f"{self._arm_name} 启动关节同步失败：{exc}"
+            )
+            self._adapter.close()
+            raise
+
+    def _reset_joint_state(
+        self,
+        snapshot: JointStateSnapshot,
+    ) -> np.ndarray:
+        current_pose = self._ik_solver.update_joint_state(
+            snapshot.positions
+        )
+        positions = list(snapshot.positions)
+        self._latest_joint_positions = positions
+        self._last_current_pose = current_pose
+        self._last_valid_joint_target = list(positions)
+        self._last_joint_command_target = list(positions)
+        self._last_joint_command_velocity = [0.0] * 7
+        return current_pose
+
     def _setup_tool_control(self) -> None:
         peripheral_arm = self._peripheral_arm_name()
         if self._bool_parameter("configure_peripheral_on_connect"):
@@ -515,17 +547,32 @@ class SingleArmVelocityTeleop(Node):
             else (tick_started_ns - last_tick_started_ns) * 1e-6
         )
         now = self.get_clock().now()
-        snapshot = self._fresh_joint_state()
-        if snapshot is None:
+        if self._control_fault_latched:
+            return
+
+        snapshot = self._adapter.get_latest_joint_state()
+        if not self._joint_snapshot_is_motion_ready(snapshot):
             self._grip_rearm_required = True
             if self._joint_feedback_ready:
                 self.get_logger().warn(
-                    f"{self._arm_name} 关节反馈缺失、过期或机械臂未就绪，机械臂停止。",
+                    f"{self._arm_name} 关节反馈无效或机械臂未就绪，机械臂停止。",
                     throttle_duration_sec=1.0,
                 )
             self._joint_feedback_ready = False
             self._safe_stop(reset_active=True)
             return
+        assert snapshot is not None
+        feedback_age = time.monotonic() - snapshot.received_at
+        if feedback_age < 0.0:
+            self._grip_rearm_required = True
+            self._joint_feedback_ready = False
+            self._safe_stop(reset_active=True)
+            return
+        if feedback_age > self._command_timeout_sec:
+            self._handle_stale_joint_feedback(feedback_age)
+            return
+
+        self._feedback_resync_attempted = False
         try:
             current_pose = self._sync_joint_feedback(snapshot)
         except Exception as exc:
@@ -538,9 +585,17 @@ class SingleArmVelocityTeleop(Node):
             self._safe_stop(reset_active=True)
             return
         if not self._joint_feedback_ready:
-            self.get_logger().info(
-                f"{self._arm_name} 已收到首帧有效关节反馈，QP 可以启用。"
-            )
+            if self._grip_rearm_required:
+                message = (
+                    f"{self._arm_name} UDP关节反馈已恢复，"
+                    "等待Grip松开后重新使能。"
+                )
+            else:
+                message = (
+                    f"{self._arm_name} 已收到首帧有效关节反馈，"
+                    "QP可以启用。"
+                )
+            self.get_logger().info(message)
             self._joint_feedback_ready = True
 
         if self._last_msg is None or self._last_msg_time is None:
@@ -592,7 +647,7 @@ class SingleArmVelocityTeleop(Node):
             )
             return
 
-        feedback_age_ms = (time.monotonic() - snapshot.received_at) * 1000.0
+        feedback_age_ms = feedback_age * 1000.0
         assert self._controller_start is not None
         assert self._robot_start_transform is not None
 
@@ -978,20 +1033,103 @@ class SingleArmVelocityTeleop(Node):
             samples.clear()
         self.get_logger().info(message)
 
-    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
+    def _handle_stale_joint_feedback(self, age: float) -> None:
+        if self._control_fault_latched:
+            return
+        self._grip_rearm_required = True
+        if self._joint_feedback_ready:
+            self.get_logger().warn(
+                f"{self._arm_name} UDP关节反馈超时"
+                f"（age={age * 1000.0:.1f} ms），保持最后安全目标。"
+            )
+        self._joint_feedback_ready = False
+
         if (
-            len(snapshot.positions) != 7
-            or not all(math.isfinite(value) for value in snapshot.positions)
-            or not snapshot.motion_ready
+            age >= self._feedback_resync_timeout_sec
+            and not self._feedback_resync_attempted
         ):
-            return None
-        return snapshot
+            self._feedback_resync_attempted = True
+            self.get_logger().warn(
+                f"{self._arm_name} UDP关节反馈持续超时，"
+                "尝试rm_get_joint_degree重新同步。"
+            )
+            try:
+                self._reset_joint_state(
+                    self._adapter.read_joint_state()
+                )
+            except Exception as exc:
+                self._latch_control_fault(
+                    f"UDP关节反馈持续超时且重新同步失败：{exc}"
+                )
+                return
+            self.get_logger().info(
+                f"{self._arm_name} 已通过rm_get_joint_degree重新同步，"
+                "继续保持并等待UDP恢复。"
+            )
+
+        if self._active and self._last_joint_command_target is not None:
+            self._repeat_last_joint_target()
+        else:
+            self._safe_stop(reset_active=True)
+
+    def _repeat_last_joint_target(self) -> None:
+        target = self._last_joint_command_target
+        if target is None:
+            return
+        try:
+            self._adapter.send_joint_target(list(target), self._follow)
+            self._stop_sent = False
+        except Exception as exc:
+            self._recover_from_canfd_error(exc)
+
+    def _recover_from_canfd_error(
+        self,
+        send_error: Exception,
+    ) -> None:
+        self.get_logger().error(
+            f"{self._arm_name} rm_movej_canfd发送失败：{send_error}"
+        )
+        self._grip_rearm_required = True
+        self._send_stop_once()
+        self._safe_stop(reset_active=True)
+        try:
+            self._reset_joint_state(
+                self._adapter.read_joint_state()
+            )
+        except Exception as query_error:
+            self._latch_control_fault(
+                "CANFD错误后关节同步失败："
+                f"send={send_error}; query={query_error}"
+            )
+            return
+        self.get_logger().info(
+            f"{self._arm_name} CANFD错误后已同步实际关节角，"
+            "等待UDP恢复及Grip重新使能。"
+        )
+
+    def _latch_control_fault(self, message: str) -> None:
+        if self._control_fault_latched:
+            return
+        self._control_fault_latched = True
+        self._grip_rearm_required = True
+        self.get_logger().error(
+            f"{self._arm_name} 控制故障已锁存：{message}"
+        )
+        self._safe_stop(reset_active=True)
+
+    @staticmethod
+    def _joint_snapshot_is_motion_ready(
+        snapshot: JointStateSnapshot | None,
+    ) -> bool:
+        return (
+            snapshot is not None
+            and len(snapshot.positions) == 7
+            and all(
+                math.isfinite(value)
+                for value in snapshot.positions
+            )
+            and snapshot.motion_ready
+        )
 
     def _sync_joint_feedback(
         self,
@@ -1086,12 +1224,7 @@ class SingleArmVelocityTeleop(Node):
         try:
             self._adapter.send_joint_target(limited_target, self._follow)
         except Exception as exc:
-            self.get_logger().error(
-                f"{self._arm_name} 发送关节透传命令失败：{exc}",
-                throttle_duration_sec=1.0,
-            )
-            self._send_stop_once()
-            self._safe_stop(reset_active=True)
+            self._recover_from_canfd_error(exc)
             return False
         self._last_joint_command_target = limited_target
         self._last_joint_command_velocity = limited_velocity
@@ -1112,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:
-            raise ValueError("joint command limits and dt must be positive")
-        desired_velocity = np.clip(
-            (np.asarray(target) - np.asarray(previous_target)) / dt,
-            -max_speed,
-            max_speed,
-        )
+        if not all(
+            math.isfinite(value) and value > 0.0
+            for value in (max_speed, max_acceleration, dt)
+        ):
+            raise ValueError("joint command limits and dt must be finite and positive")
+        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(
-            desired_velocity,
-            np.asarray(previous_velocity) - velocity_step,
-            np.asarray(previous_velocity) + velocity_step,
-        )
-        limited_target = np.asarray(previous_target) + velocity * dt
+        arrival_distance = velocity_step * dt
+        limited_target = []
+        limited_velocity = []
+        for desired_target, last_target, last_velocity in zip(
+            target, previous_target, previous_velocity
+        ):
+            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,
@@ -1205,6 +1366,11 @@ class SingleArmVelocityTeleop(Node):
     def _validate_parameters(self) -> None:
         if self._command_timeout_sec <= 0.0:
             raise ValueError("command_timeout_sec must be > 0")
+        if self._feedback_resync_timeout_sec <= self._command_timeout_sec:
+            raise ValueError(
+                "feedback_resync_timeout_sec must be greater than "
+                "command_timeout_sec"
+            )
         if self._deadband_m < 0.0:
             raise ValueError("deadband_m must be >= 0")
         if not 0.0 <= self._target_filter_alpha <= 1.0: