221 lines
6.5 KiB
Python
221 lines
6.5 KiB
Python
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]]
|
|
)
|