feat: Implement RM75 joint feedback and fault recovery design

AGENTS.md

@@ -205,6 +205,7 @@
 * 类型标注方式。
 * 注释风格。
 * 测试组织方式。
+* 新生成的 Markdown（`.md`）文档统一使用中文撰写。
 
 如果项目已有 lint、format 或 test 命令，优先使用项目已有命令，不要擅自更换工具链。
 

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/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、依赖或公开入口。

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_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:

xr_rm_teleop/test/test_joint_control.py

@@ -30,34 +30,177 @@ 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._arm_name = "right_rm75"
-    teleop._adapter = SimpleNamespace(get_latest_joint_state=lambda: None)
-
-    assert teleop._fresh_joint_state() is None
-
     teleop._adapter = SimpleNamespace(
-        get_latest_joint_state=lambda: JointStateSnapshot(
+        read_joint_state=lambda: JointStateSnapshot(
-            [0.0] * 7,
+            positions,
-            time.monotonic() - 1.0,
+            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._arm_name = "left_rm75"
-    teleop._adapter = SimpleNamespace(
+    teleop._adapter = FailingAdapter()
-        get_latest_joint_state=lambda: JointStateSnapshot(
+    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)
+
+    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
 
 
 def test_joint_command_step_limits_acceleration_from_rest() -> None:
@@ -106,6 +249,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 +270,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 +429,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:
+def test_canfd_error_stops_queries_and_requires_grip_rearm() -> None:
-    class FailingAdapter:
+    class RecoveringAdapter:
-        def __init__(self) -> None:
+        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 = []
+        def read_joint_state(self):
-    teleop = object.__new__(SingleArmVelocityTeleop)
+            self.read_calls += 1
-    teleop._adapter = FailingAdapter()
+            return JointStateSnapshot([0.2] * 7, time.monotonic())
-    teleop._follow = False
+
-    teleop._arm_name = "left_rm75"
+    teleop = _timeout_teleop(RecoveringAdapter())
-    teleop._stop_sent = False
-    teleop._last_joint_command_target = [0.0] * 7
-    teleop._last_joint_command_velocity = [0.0] * 7
     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._dt = 1.0 / 90.0
-    teleop.get_logger = lambda: FakeLogger()
-    teleop._safe_stop = lambda reset_active: reset_calls.append(reset_active)
 
-    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/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):
@@ -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)
+            positions = self._joint_positions_from_degrees(
-            if (
+                data.joint_status.joint_position,
-                len(degrees) != 7
+                "RM75 UDP feedback",
-                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")
             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 self._control_fault_latched:
-        if snapshot is None:
+            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(
+            if self._grip_rearm_required:
-                f"{self._arm_name} 已收到首帧有效关节反馈，QP 可以启用。"
+                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,102 @@ class SingleArmVelocityTeleop(Node):
             samples.clear()
         self.get_logger().info(message)
 
-    def _fresh_joint_state(self) -> JointStateSnapshot | None:
+    def _handle_stale_joint_feedback(self, age: float) -> None:
-        snapshot = self._adapter.get_latest_joint_state()
+        if self._control_fault_latched:
-        if snapshot is None:
+            return
-            return None
+        self._grip_rearm_required = True
-        age = time.monotonic() - snapshot.received_at
+        if self._joint_feedback_ready:
-        if age < 0.0 or age > self._command_timeout_sec:
+            self.get_logger().warn(
-            return None
+                f"{self._arm_name} UDP关节反馈超时，保持最后安全目标。"
+            )
+        self._joint_feedback_ready = False
+
         if (
-            len(snapshot.positions) != 7
+            age >= self._feedback_resync_timeout_sec
-            or not all(math.isfinite(value) for value in snapshot.positions)
+            and not self._feedback_resync_attempted
-            or not snapshot.motion_ready
         ):
-            return None
+            self._feedback_resync_attempted = True
-        return snapshot
+            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 +1223,7 @@ class SingleArmVelocityTeleop(Node):
         try:
             self._adapter.send_joint_target(limited_target, self._follow)
         except Exception as exc:
-            self.get_logger().error(
+            self._recover_from_canfd_error(exc)
-                f"{self._arm_name} 发送关节透传命令失败：{exc}",
-                throttle_duration_sec=1.0,
-            )
-            self._send_stop_once()
-            self._safe_stop(reset_active=True)
             return False
         self._last_joint_command_target = limited_target
         self._last_joint_command_velocity = limited_velocity
@@ -1205,6 +1337,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: