feat: Implement UDP feedback for RM75 robot arms

xr_rm_teleop/test/placo_ik_smoke.py

@@ -50,6 +50,9 @@ def main() -> None:
         drift_degrees = float(
             np.max(np.abs(np.rad2deg(np.asarray(joints) - initial_joints)))
         )
+        assert drift_degrees <= 0.05, (
+            f"{arm} stationary target drifted {drift_degrees:.3f}deg"
+        )
 
         solver = PlacoIkSolver(str(urdf_path), 1.0 / 125.0)
         joints = initial_joints.tolist()

xr_rm_teleop/test/test_initial_joint_pose.py

@@ -1,4 +1,6 @@
 import math
+import sys
+from types import ModuleType, SimpleNamespace
 
 import pytest
 
@@ -18,7 +20,14 @@ def test_initial_pose_uses_joint_move_only() -> None:
             return 0
 
     joints = [-167.21, 28.48, 28.21, 61.35, -14.40, 84.49, -124.51]
-    adapter = RealManAdapter("127.0.0.1", 8080, 0, 1, initial_joint_pose=joints)
+    adapter = RealManAdapter(
+        "127.0.0.1",
+        8080,
+        0,
+        "127.0.0.1",
+        8090,
+        initial_joint_pose=joints,
+    )
     adapter._arm = FakeArm()
 
     adapter._move_to_initial_pose()
@@ -111,80 +120,299 @@ def test_tool_frame_sdk_failures_are_reported(existing, failure, operation) -> N
         _configure_tool_frame(FakeArm(), object(), "omnipic")
 
 
-def test_joint_feedback_is_cached_in_radians(monkeypatch) -> 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]
-
-    perf_counter_ns = iter(
-        [1_000_000_000, 1_002_000_000, 2_000_000_000, 2_003_000_000]
-    )
-    monotonic = iter([10.0, 10.011])
-    monkeypatch.setattr(
-        realman_adapter,
-        "time",
-        type(
-            "FakeTime",
-            (),
-            {
-                "perf_counter_ns": staticmethod(lambda: next(perf_counter_ns)),
-                "monotonic": staticmethod(lambda: next(monotonic)),
-            },
+def _udp_state(
+    *,
+    robot_ip: str = "127.0.0.1",
+    joints=None,
+    error_code: int = 0,
+    joint_enabled=None,
+    joint_error_codes=None,
+    arm_error_codes=None,
+    arm_current_status: int = 0,
+):
+    if joints is None:
+        joints = [0.0, 10.0, -20.0, 30.0, -40.0, 50.0, -60.0]
+    if joint_enabled is None:
+        joint_enabled = [True] * 7
+    if joint_error_codes is None:
+        joint_error_codes = [0] * 7
+    if arm_error_codes is None:
+        arm_error_codes = []
+    return SimpleNamespace(
+        errCode=error_code,
+        arm_ip=robot_ip.encode(),
+        joint_status=SimpleNamespace(
+            joint_position=joints,
+            joint_en_flag=joint_enabled,
+            joint_err_code=joint_error_codes,
         ),
+        err=SimpleNamespace(
+            err_len=len(arm_error_codes),
+            err=list(arm_error_codes),
+        ),
+        arm_current_status=arm_current_status,
     )
-    adapter = RealManAdapter("127.0.0.1", 8080, 0, 0.01)
-    adapter._arm = FakeArm()
 
-    adapter._read_joint_state_once()
+
+def _install_fake_sdk(monkeypatch, *, push_return=0, send_feedback=True):
+    class FakeThreadMode:
+        RM_TRIPLE_MODE_E = 2
+
+    class FakePushConfig:
+        def __init__(self, *args):
+            self.args = args
+
+    class FakeArm:
+        instance = None
+
+        def __init__(self, mode):
+            self.mode = mode
+            self.callback = None
+            self.config = None
+            self.delete_calls = 0
+            FakeArm.instance = self
+
+        def rm_create_robot_arm(self, robot_ip, robot_port):
+            self.robot_ip = robot_ip
+            self.robot_port = robot_port
+            return SimpleNamespace(id=1)
+
+        def rm_realtime_arm_state_call_back(self, callback):
+            self.callback = callback
+
+        def rm_set_realtime_push(self, config):
+            self.config = config
+            if push_return == 0 and send_feedback:
+                self.callback(_udp_state())
+            return push_return
+
+        def rm_delete_robot_arm(self):
+            self.delete_calls += 1
+            return 0
+
+    sdk = ModuleType("Robotic_Arm.rm_robot_interface")
+    sdk.RoboticArm = FakeArm
+    sdk.rm_thread_mode_e = FakeThreadMode
+    sdk.rm_realtime_push_config_t = FakePushConfig
+    sdk.rm_realtime_arm_state_callback_ptr = lambda callback: callback
+    package = ModuleType("Robotic_Arm")
+    package.rm_robot_interface = sdk
+    monkeypatch.setitem(sys.modules, "Robotic_Arm", package)
+    monkeypatch.setitem(sys.modules, "Robotic_Arm.rm_robot_interface", sdk)
+    return SimpleNamespace(RoboticArm=FakeArm)
+
+
+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))
+    adapter = RealManAdapter(
+        "127.0.0.1",
+        8080,
+        0,
+        "127.0.0.1",
+        8090,
+    )
+    adapter._accept_realtime_feedback = True
+
+    adapter._on_realtime_arm_state(_udp_state())
     first = adapter.get_latest_joint_state()
-    adapter._read_joint_state_once()
+    adapter._on_realtime_arm_state(_udp_state())
     second = adapter.get_latest_joint_state()
 
     assert first is not None
     assert first.positions == pytest.approx(
         [math.radians(value) for value in [0, 10, -20, 30, -40, 50, -60]]
     )
-    assert first.read_duration_ms == pytest.approx(2.0)
+    assert first.read_duration_ms is None
     assert first.update_interval_ms is None
     assert second is not None
-    assert second.read_duration_ms == pytest.approx(3.0)
-    assert second.update_interval_ms == pytest.approx(11.0)
+    assert second.read_duration_ms is None
+    assert second.update_interval_ms == pytest.approx(5.0)
 
 
-def test_feedback_loop_uses_absolute_schedule_without_catch_up(monkeypatch) -> None:
-    class FakeStopEvent:
-        def __init__(self) -> None:
-            self.checks = 0
-            self.waits = []
-
-        def is_set(self):
-            self.checks += 1
-            return self.checks > 3
-
-        def wait(self, timeout):
-            self.waits.append(timeout)
-            return False
-
-    monotonic = iter([0.0, 0.005, 0.018, 0.018, 0.023])
-    monkeypatch.setattr(
-        realman_adapter,
-        "time",
-        type(
-            "FakeTime",
-            (),
-            {"monotonic": staticmethod(lambda: next(monotonic))},
-        ),
+@pytest.mark.parametrize(
+    "state",
+    [
+        _udp_state(error_code=-3),
+        _udp_state(robot_ip="192.168.192.18"),
+        _udp_state(joints=[0.0] * 6),
+        _udp_state(joints=[0.0, 0.0, 0.0, math.nan, 0.0, 0.0, 0.0]),
+    ],
+)
+def test_invalid_udp_feedback_does_not_replace_snapshot(state) -> None:
+    adapter = RealManAdapter(
+        "127.0.0.1",
+        8080,
+        0,
+        "127.0.0.1",
+        8090,
     )
-    adapter = RealManAdapter("127.0.0.1", 8080, 0, 0.008)
-    stop_event = FakeStopEvent()
-    reads = []
-    adapter._feedback_stop = stop_event
-    adapter._read_joint_state_once = lambda: reads.append(None)
+    adapter._accept_realtime_feedback = True
+    adapter._on_realtime_arm_state(_udp_state(joints=[1.0] * 7))
+    before = adapter.get_latest_joint_state()
 
-    adapter._feedback_loop()
+    adapter._on_realtime_arm_state(state)
 
-    assert len(reads) == 3
-    assert stop_event.waits == pytest.approx([0.003, 0.003])
+    assert adapter.get_latest_joint_state() == before
+
+
+def test_udp_joint_fault_marks_snapshot_unready() -> None:
+    adapter = RealManAdapter(
+        "127.0.0.1",
+        8080,
+        0,
+        "127.0.0.1",
+        8090,
+    )
+    adapter._accept_realtime_feedback = True
+
+    adapter._on_realtime_arm_state(
+        _udp_state(
+            joint_enabled=[True, True, False, True, True, True, True],
+            joint_error_codes=[0, 0, 17, 0, 0, 0, 0],
+            arm_error_codes=[42],
+            arm_current_status=9,
+        )
+    )
+
+    snapshot = adapter.get_latest_joint_state()
+    assert snapshot is not None
+    assert snapshot.motion_ready is False
+
+
+def test_udp_stop_status_marks_snapshot_unready_without_joint_error() -> None:
+    adapter = RealManAdapter(
+        "127.0.0.1",
+        8080,
+        0,
+        "127.0.0.1",
+        8090,
+    )
+    adapter._accept_realtime_feedback = True
+
+    adapter._on_realtime_arm_state(_udp_state(arm_current_status=9))
+
+    snapshot = adapter.get_latest_joint_state()
+    assert snapshot is not None
+    assert snapshot.motion_ready is False
+
+
+def test_udp_zero_arm_error_code_is_motion_ready() -> None:
+    adapter = RealManAdapter(
+        "127.0.0.1",
+        8080,
+        0,
+        "127.0.0.1",
+        8090,
+    )
+    adapter._accept_realtime_feedback = True
+
+    adapter._on_realtime_arm_state(_udp_state(arm_error_codes=[0]))
+
+    snapshot = adapter.get_latest_joint_state()
+    assert snapshot is not None
+    assert snapshot.motion_ready is True
+
+
+def test_udp_fault_and_recovery_are_logged_once_per_transition() -> None:
+    class FakeLogger:
+        def __init__(self) -> None:
+            self.infos = []
+            self.warnings = []
+
+        def info(self, message):
+            self.infos.append(message)
+
+        def warn(self, message):
+            self.warnings.append(message)
+
+    logger = FakeLogger()
+    adapter = RealManAdapter(
+        "127.0.0.1",
+        8080,
+        0,
+        "127.0.0.1",
+        8090,
+        logger=logger,
+    )
+    adapter._accept_realtime_feedback = True
+    adapter._on_realtime_arm_state(_udp_state())
+    fault = _udp_state(
+        joint_enabled=[False] * 7,
+        joint_error_codes=[17, 0, 0, 0, 0, 0, 0],
+        arm_error_codes=[42],
+        arm_current_status=9,
+    )
+
+    adapter._on_realtime_arm_state(fault)
+    adapter._on_realtime_arm_state(fault)
+    adapter._on_realtime_arm_state(_udp_state())
+
+    assert len(logger.warnings) == 1
+    assert "joint_errors=[17, 0, 0, 0, 0, 0, 0]" in logger.warnings[0]
+    assert len([message for message in logger.infos if "恢复正常" in message]) == 1
+
+
+def test_connect_configures_udp_feedback_and_waits_for_first_frame(monkeypatch) -> None:
+    fake_sdk = _install_fake_sdk(monkeypatch)
+    adapter = RealManAdapter(
+        "127.0.0.1",
+        8080,
+        0,
+        "192.168.192.148",
+        8090,
+        configure_safety_limits=False,
+    )
+
+    adapter.connect()
+
+    arm = fake_sdk.RoboticArm.instance
+    assert arm is not None
+    assert arm.config.args == (5, True, 8090, 0, "192.168.192.148")
+    assert arm.callback is adapter._realtime_callback
+    assert adapter.get_latest_joint_state() is not None
+    assert not hasattr(adapter, "_feedback_thread")
+
+
+def test_udp_configuration_failure_cleans_up_robot_handle(monkeypatch) -> None:
+    fake_sdk = _install_fake_sdk(monkeypatch, push_return=1)
+    adapter = RealManAdapter(
+        "127.0.0.1",
+        8080,
+        0,
+        "192.168.192.148",
+        8090,
+        configure_safety_limits=False,
+    )
+
+    with pytest.raises(RuntimeError, match="rm_set_realtime_push"):
+        adapter.connect()
+
+    assert fake_sdk.RoboticArm.instance.delete_calls == 1
+    assert adapter._arm is None
+
+
+def test_udp_first_frame_timeout_cleans_up_robot_handle(monkeypatch) -> None:
+    fake_sdk = _install_fake_sdk(monkeypatch, send_feedback=False)
+    adapter = RealManAdapter(
+        "127.0.0.1",
+        8080,
+        0,
+        "192.168.192.148",
+        8090,
+        configure_safety_limits=False,
+    )
+    adapter._feedback_ready = SimpleNamespace(
+        clear=lambda: None,
+        set=lambda: None,
+        wait=lambda timeout: False,
+    )
+
+    with pytest.raises(RuntimeError, match="within 2 seconds"):
+        adapter.connect()
+
+    assert fake_sdk.RoboticArm.instance.delete_calls == 1
+    assert adapter._arm is None
 
 
 def test_joint_target_uses_movej_canfd_in_degrees() -> None:
@@ -196,7 +424,13 @@ def test_joint_target_uses_movej_canfd_in_degrees() -> None:
             self.calls.append(args)
             return 0
 
-    adapter = RealManAdapter("127.0.0.1", 8080, 0, 0.01)
+    adapter = RealManAdapter(
+        "127.0.0.1",
+        8080,
+        0,
+        "127.0.0.1",
+        8090,
+    )
     adapter._arm = FakeArm()
     target = [math.radians(value) for value in [1, 2, 3, 4, 5, 6, 7]]
 

xr_rm_teleop/test/test_joint_control.py

@@ -1,3 +1,4 @@
+import math
 import time
 from types import SimpleNamespace
 
@@ -45,6 +46,85 @@ def test_missing_or_stale_feedback_does_not_enable_qp() -> None:
     assert teleop._fresh_joint_state() is None
 
 
+def test_disabled_joint_feedback_does_not_enable_qp() -> None:
+    teleop = object.__new__(SingleArmVelocityTeleop)
+    teleop._command_timeout_sec = 0.12
+    teleop._adapter = SimpleNamespace(
+        get_latest_joint_state=lambda: JointStateSnapshot(
+            [0.0] * 7,
+            time.monotonic(),
+            motion_ready=False,
+        )
+    )
+
+    assert teleop._fresh_joint_state() is None
+
+
+def test_joint_command_step_limits_acceleration_from_rest() -> None:
+    dt = 1.0 / 125.0
+    target, velocity = SingleArmVelocityTeleop._limit_joint_command_step(
+        target=[0.2] * 7,
+        previous_target=[0.0] * 7,
+        previous_velocity=[0.0] * 7,
+        max_speed=math.radians(180.0),
+        max_acceleration=math.radians(300.0),
+        dt=dt,
+    )
+
+    assert velocity == pytest.approx([math.radians(2.4)] * 7)
+    assert target == pytest.approx([math.radians(0.0192)] * 7)
+
+
+def test_feedback_fault_blocks_grip_until_release() -> None:
+    class FakeClock:
+        def now(self):
+            return FakeTime()
+
+    teleop = object.__new__(SingleArmVelocityTeleop)
+    teleop._adapter = SimpleNamespace(
+        get_latest_joint_state=lambda: JointStateSnapshot(
+            [0.1] * 7,
+            time.monotonic(),
+        )
+    )
+    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._last_valid_joint_target = None
+    teleop._last_current_pose = None
+    teleop._ik_solver = SimpleNamespace(
+        update_joint_state=lambda joints: np.eye(4)
+    )
+    teleop._grip_rearm_required = True
+    teleop.get_clock = lambda: FakeClock()
+    teleop.get_logger = lambda: FakeLogger()
+    stopped = []
+    entered = []
+    teleop._safe_stop = lambda reset_active: stopped.append(reset_active)
+    teleop._enter_active_control = lambda *args: entered.append(args)
+
+    teleop._control_tick()
+    assert entered == []
+
+    teleop._last_msg.grip = False
+    teleop._control_tick()
+    assert teleop._grip_rearm_required is False
+
+    teleop._last_msg.grip = True
+    teleop._control_tick()
+    assert len(entered) == 1
+
+
 def test_stale_feedback_stops_before_active_control() -> None:
     stopped = []
     entered = []
@@ -256,6 +336,11 @@ def test_joint_send_failure_requests_slow_stop_and_resets_control() -> None:
     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
+    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)