feat: Implement RM75 joint feedback and fault recovery design

2026-07-29 19:39:12 +08:00
parent f795c06d44
commit 6d22d5600a
12 changed files with 1800 additions and 108 deletions
@@ -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逻辑仍保留。
+- 验证期间未连接真机、移动机械臂或操作夹爪。