YikaiFu-cart/acRealman_xr
1
1
Fork 1
Code Issues Pull Requests Packages Projects Releases Wiki Activity

feat: Implement UDP feedback for RM75 robot arms

Browse Source
This commit is contained in:
YikaiFu-cart
2026-07-29 15:26:59 +08:00
parent 687a0b401a
commit 08996434e5
16 changed files with 1600 additions and 329 deletions
Download Patch File Download Diff File Expand all files Collapse all files
xr_rm_teleop/test/test_initial_joint_pose.py
+293 -59
View File
@@ -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]]