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

feat: Implement absolute scheduling for feedback loop and enhance tool frame configuration

Browse Source
This commit is contained in:
YikaiFu-cart
2026-07-29 09:44:51 +08:00
parent 2a12eea4d5
commit 687a0b401a
11 changed files with 876 additions and 21 deletions
Download Patch File Download Diff File Expand all files Collapse all files
xr_rm_teleop/test/test_initial_joint_pose.py
+137 -5
View File
@@ -2,9 +2,10 @@ 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
from xr_rm_teleop.fun_peripheral import PeripheralConfig, _configure_tool_frame
def test_initial_pose_uses_joint_move_only() -> None:
@@ -38,21 +39,152 @@ def test_peripheral_config_exposes_selected_tool() -> None:
assert config.tool_pose == [0.0, 0.0, 0.16, 0.0, 0.0, 0.0, 1.0]
def test_joint_feedback_is_cached_in_radians() -> None:
@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()
snapshot = adapter.get_latest_joint_state()
first = adapter.get_latest_joint_state()
adapter._read_joint_state_once()
second = adapter.get_latest_joint_state()
assert snapshot is not None
assert snapshot.positions == pytest.approx(
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:
xr_rm_teleop/test/test_joint_control.py
+24 -10
View File
@@ -194,33 +194,47 @@ def test_timing_stats_logs_summary_and_clears_window() -> None:
teleop = object.__new__(SingleArmVelocityTeleop)
teleop._arm_name = "right_rm75"
teleop._dt = 0.008
teleop._timing_stats_window = 2
teleop._timing_stats_window = 3
teleop._timing_samples = {
name: []
for name in ("period", "total", "qp", "send", "feedback_age")
for name in (
"period",
"total",
"qp",
"send",
"feedback_age",
"feedback_read",
"feedback_interval",
)
}
teleop._last_timing_feedback_received_at = None
teleop.get_logger = lambda: SimpleNamespace(
info=lambda message: messages.append(message)
)
first_feedback = JointStateSnapshot([0.0] * 7, 10.0, 2.0, None)
second_feedback = JointStateSnapshot([0.0] * 7, 10.011, 3.0, 11.0)
teleop._record_timing_sample(7.0, 6.0, 1.0, 0.5, 3.0)
teleop._record_timing_sample(7.0, 6.0, 1.0, 0.5, 3.0, first_feedback)
teleop._record_timing_sample(8.0, 8.0, 1.5, 0.6, 3.5, first_feedback)
assert messages == []
teleop._record_timing_sample(9.0, 10.0, 2.0, 0.7, 4.0)
teleop._record_timing_sample(9.0, 10.0, 2.0, 0.7, 4.0, second_feedback)
assert len(messages) == 1
assert "right_rm75 timing n=2 deadline=8.000 ms" in messages[0]
assert "right_rm75 timing n=3 deadline=8.000 ms" in messages[0]
assert (
"period[n=2 mean=8.000 p95=8.900 p99=8.980 "
"period[n=3 mean=8.000 p95=8.900 p99=8.980 "
"max=9.000 ms overruns=1]"
) in messages[0]
assert (
"total[n=2 mean=8.000 p95=9.800 p99=9.960 "
"total[n=3 mean=8.000 p95=9.800 p99=9.960 "
"max=10.000 ms overruns=1]"
) in messages[0]
assert "qp[n=2" in messages[0]
assert "send[n=2" in messages[0]
assert "feedback_age[n=2" in messages[0]
assert "qp[n=3" in messages[0]
assert "send[n=3" in messages[0]
assert "feedback_age[n=3" in messages[0]
assert "feedback_read[n=2" in messages[0]
assert "feedback_interval[n=1" in messages[0]
assert all(not samples for samples in teleop._timing_samples.values())