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: