529 lines
14 KiB
Python
529 lines
14 KiB
Python
import math
|
|
import sys
|
|
from types import ModuleType, SimpleNamespace
|
|
|
|
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,
|
|
"127.0.0.1",
|
|
8090,
|
|
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 _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,
|
|
)
|
|
|
|
|
|
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_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))
|
|
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._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 is None
|
|
assert first.update_interval_ms is None
|
|
assert second is not None
|
|
assert second.read_duration_ms is None
|
|
assert second.update_interval_ms == pytest.approx(5.0)
|
|
|
|
|
|
@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._accept_realtime_feedback = True
|
|
adapter._on_realtime_arm_state(_udp_state(joints=[1.0] * 7))
|
|
before = adapter.get_latest_joint_state()
|
|
|
|
adapter._on_realtime_arm_state(state)
|
|
|
|
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:
|
|
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
|
|
|
|
|
|
@pytest.mark.parametrize(
|
|
("cycle_ms", "sdk_cycle"),
|
|
[(5, 1), (10, 2)],
|
|
)
|
|
def test_connect_converts_udp_feedback_cycle_to_sdk_units(
|
|
monkeypatch,
|
|
cycle_ms,
|
|
sdk_cycle,
|
|
) -> None:
|
|
fake_sdk = _install_fake_sdk(monkeypatch)
|
|
adapter = RealManAdapter(
|
|
"127.0.0.1",
|
|
8080,
|
|
0,
|
|
"192.168.192.148",
|
|
8090,
|
|
realtime_push_cycle_ms=cycle_ms,
|
|
configure_safety_limits=False,
|
|
)
|
|
|
|
adapter.connect()
|
|
|
|
arm = fake_sdk.RoboticArm.instance
|
|
assert arm is not None
|
|
assert arm.config.args == (
|
|
sdk_cycle,
|
|
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:
|
|
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,
|
|
"127.0.0.1",
|
|
8090,
|
|
)
|
|
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]]
|
|
)
|