feat: Implement RM75 joint feedback and fault recovery design

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._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)
+
+    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:
-    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()