import math import pytest from xr_rm_teleop import realman_adapter from xr_rm_teleop.realman_adapter import RealManAdapter from xr_rm_teleop.realman_adapter import MockRealManAdapter from xr_rm_teleop.fun_peripheral import PeripheralConfig, _configure_tool_frame def test_initial_pose_uses_joint_move_only() -> None: class FakeArm: def __init__(self) -> None: self.calls = [] def rm_movej(self, *args): self.calls.append(args) 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._arm = FakeArm() adapter._move_to_initial_pose() assert adapter._arm.calls == [(joints, 20, 0, 0, 1)] def test_peripheral_config_exposes_selected_tool() -> None: config = PeripheralConfig( scissorgripper=1, tools_in_ee={ "first": [[0.0] * 7, [0.0] * 7], "second": [[0.0, 0.0, 0.16, 0.0, 0.0, 0.0, 1.0], [0.0] * 7], }, ) assert config.tool_name == "second" assert config.tool_pose == [0.0, 0.0, 0.16, 0.0, 0.0, 0.0, 1.0] @pytest.mark.parametrize( ("existing", "expected_operation"), [(False, "create"), (True, "update")], ) def test_tool_frame_is_created_or_updated(existing, expected_operation) -> None: class FakeArm: def __init__(self) -> None: self.calls = [] def rm_get_total_tool_frame(self): self.calls.append(("get",)) names = ["omnipic"] if existing else [] return {"return_code": 0, "tool_names": names} def rm_set_manual_tool_frame(self, *, frame): self.calls.append(("create", frame)) return 0 def rm_update_tool_frame(self, *, frame): self.calls.append(("update", frame)) return 0 def rm_change_tool_frame(self, tool_name): self.calls.append(("change", tool_name)) return 0 arm = FakeArm() frame = object() _configure_tool_frame(arm, frame, "omnipic") assert arm.calls == [ ("get",), (expected_operation, frame), ("change", "omnipic"), ] @pytest.mark.parametrize( ("existing", "failure", "operation"), [ (False, "query", "rm_get_total_tool_frame"), (False, "create", "rm_set_manual_tool_frame"), (True, "update", "rm_update_tool_frame"), (False, "change", "rm_change_tool_frame"), ], ) def test_tool_frame_sdk_failures_are_reported(existing, failure, operation) -> None: class FakeArm: def rm_get_total_tool_frame(self): names = ["omnipic"] if existing else [] return { "return_code": 1 if failure == "query" else 0, "tool_names": names, } def rm_set_manual_tool_frame(self, *, frame): del frame return 1 if failure == "create" else 0 def rm_update_tool_frame(self, *, frame): del frame return 1 if failure == "update" else 0 def rm_change_tool_frame(self, tool_name): del tool_name return 1 if failure == "change" else 0 with pytest.raises(RuntimeError, match=operation): _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)), }, ), ) adapter = RealManAdapter("127.0.0.1", 8080, 0, 0.01) adapter._arm = FakeArm() adapter._read_joint_state_once() first = adapter.get_latest_joint_state() adapter._read_joint_state_once() 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.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) 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))}, ), ) 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._feedback_loop() assert len(reads) == 3 assert stop_event.waits == pytest.approx([0.003, 0.003]) def test_joint_target_uses_movej_canfd_in_degrees() -> None: class FakeArm: def __init__(self) -> None: self.calls = [] def rm_movej_canfd(self, *args): self.calls.append(args) return 0 adapter = RealManAdapter("127.0.0.1", 8080, 0, 0.01) adapter._arm = FakeArm() target = [math.radians(value) for value in [1, 2, 3, 4, 5, 6, 7]] adapter.send_joint_target(target, follow=False) assert len(adapter._arm.calls) == 1 degrees, follow, expand, trajectory_mode, radio = adapter._arm.calls[0] assert degrees == pytest.approx([1.0, 2.0, 3.0, 4.0, 5.0, 6.0, 7.0]) assert (follow, expand, trajectory_mode, radio) == (False, 0, 2, 0) def test_mock_joint_feedback_is_available_without_vendor_sdk() -> None: adapter = MockRealManAdapter([1.0, 2.0, 3.0, 4.0, 5.0, 6.0, 7.0]) adapter.connect() snapshot = adapter.get_latest_joint_state() assert snapshot is not None assert snapshot.positions == pytest.approx( [math.radians(value) for value in [1, 2, 3, 4, 5, 6, 7]] )