Files
acRealman_xr/xr_rm_teleop/test/test_initial_joint_pose.py
T

654 lines
18 KiB
Python

import math
import sys
from pathlib import Path
from types import ModuleType, SimpleNamespace
import pytest
import yaml
from xr_rm_teleop import fun_peripheral, 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,
load_peripheral_config,
peripheral_cfg,
)
CONFIG_DIR = Path(__file__).resolve().parents[2] / "xr_rm_bringup" / "config"
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_mock_initial_pose_restores_configured_joints() -> None:
initial_degrees = [-78.81, 3.22, 67.96, 97.12, 95.08, -81.11, -74.55]
adapter = MockRealManAdapter(initial_degrees)
adapter.send_joint_target([0.0] * 7, follow=False)
adapter.move_to_initial_pose()
assert adapter.read_joint_state().positions == pytest.approx(
[math.radians(value) for value in initial_degrees]
)
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]
def test_deployed_peripheral_config_selects_left_and_right_tools() -> None:
config_file = CONFIG_DIR / "peripherals_rm75.yaml"
left = load_peripheral_config(str(config_file), "left")
right = load_peripheral_config(str(config_file), "right")
assert left.scissorgripper == 2
assert left.tool_name == "minisci"
assert left.tool_pose == [0.0, 0.0, 0.165, 0.0, 0.0, 0.0, 1.0]
assert right.scissorgripper == 1
assert right.tool_name == "omnipic"
assert right.tool_pose == [0.0, 0.0, 0.14, 0.0, 0.0, 0.0, 1.0]
def test_right_tool_initializes_open_only_for_right_arm() -> None:
config_file = CONFIG_DIR / "peripherals_rm75.yaml"
left = load_peripheral_config(str(config_file), "left")
right = load_peripheral_config(str(config_file), "right")
assert not left.set_initial_tool_state
assert right.set_initial_tool_state
def test_omnipic_initial_state_opens_fully(monkeypatch) -> None:
calls = []
class FakeArm:
def rm_set_voltage(self, *args):
del args
def rm_set_io_mode(self, *args):
del args
def rm_algo_quaternion2euler(self, quaternion):
del quaternion
return [0.0, 0.0, 0.0]
def rm_get_total_tool_frame(self):
return {"return_code": 0, "tool_names": []}
def rm_set_manual_tool_frame(self, *, frame):
del frame
return 0
def rm_change_tool_frame(self, tool_name):
del tool_name
return 0
def rm_set_modbus_mode(self, **kwargs):
del kwargs
return 0
def rm_write_single_register(self, params, value):
del params, value
return 0
sdk = ModuleType("Robotic_Arm.rm_robot_interface")
sdk.rm_frame_t = lambda *args: object()
sdk.rm_peripheral_read_write_params_t = lambda *args: object()
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)
monkeypatch.setattr(fun_peripheral.time, "sleep", lambda seconds: None)
monkeypatch.setattr(
fun_peripheral,
"set_tool_position",
lambda robot, percent, device, scissorgripper: calls.append(
(percent, device, scissorgripper)
),
)
tools = {
"scissor": [[0.0] * 7, [0.0] * 7],
"omnipic": [[0.0] * 7, [0.0] * 7],
}
peripheral_cfg(
FakeArm(),
1,
tools,
set_initial_tool_state=True,
)
assert calls == [(1.0, 1, 1)]
@pytest.mark.parametrize(
("config_name", "node_names"),
[
("left_arm_rm75.yaml", ("single_arm_velocity_teleop",)),
("right_arm_rm75.yaml", ("single_arm_velocity_teleop",)),
("dual_arm_rm75.yaml", ("left_arm_teleop", "right_arm_teleop")),
],
)
def test_deployed_workspace_is_in_front_of_robot(config_name, node_names) -> None:
with (CONFIG_DIR / config_name).open(encoding="utf-8") as stream:
config = yaml.safe_load(stream)
for node_name in node_names:
parameters = config[node_name]["ros__parameters"]
assert parameters["workspace_min"] == [-0.70, -0.70, 0.10]
assert parameters["workspace_max"] == [0.70, 0.10, 0.75]
@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]]
)