82 lines
2.5 KiB
Python
82 lines
2.5 KiB
Python
import csv
|
|
import math
|
|
from types import SimpleNamespace
|
|
|
|
import pytest
|
|
|
|
from lerobot_robot_ufactory.scripts.uf_robot_teleop import (
|
|
GuardLatencyTiming,
|
|
TeleopConfig,
|
|
_percentile,
|
|
_validate_guard_latency_config,
|
|
_write_guard_latency_timings,
|
|
)
|
|
|
|
|
|
def _config(**robot_overrides):
|
|
robot_values = {
|
|
"cameras": {"camera": object()},
|
|
"control_space": "joint",
|
|
"joint_command_mode": 1,
|
|
"min_tcp_z_mm": 100.0,
|
|
}
|
|
robot_values.update(robot_overrides)
|
|
return TeleopConfig(
|
|
robot=SimpleNamespace(**robot_values),
|
|
teleop=SimpleNamespace(),
|
|
fps=60,
|
|
guard_latency_experiment=True,
|
|
)
|
|
|
|
|
|
def test_guard_latency_requires_guarded_servoj():
|
|
_validate_guard_latency_config(_config())
|
|
|
|
with pytest.raises(ValueError, match="control_space='joint'"):
|
|
_validate_guard_latency_config(_config(control_space="cartesian"))
|
|
with pytest.raises(ValueError, match="joint_command_mode=1"):
|
|
_validate_guard_latency_config(_config(joint_command_mode=6))
|
|
with pytest.raises(ValueError, match="min_tcp_z_mm"):
|
|
_validate_guard_latency_config(_config(min_tcp_z_mm=None))
|
|
|
|
|
|
def test_guard_latency_config_rejects_invalid_rates():
|
|
with pytest.raises(ValueError, match="fps"):
|
|
TeleopConfig(robot=SimpleNamespace(cameras={}), teleop=SimpleNamespace(), fps=0)
|
|
with pytest.raises(ValueError, match="experiment_duration_s"):
|
|
TeleopConfig(
|
|
robot=SimpleNamespace(cameras={}),
|
|
teleop=SimpleNamespace(),
|
|
experiment_duration_s=float("nan"),
|
|
)
|
|
|
|
|
|
def test_percentile_ignores_non_finite_values():
|
|
assert _percentile([1.0, 2.0, float("nan"), 3.0, 4.0], 50) == pytest.approx(2.5)
|
|
assert math.isnan(_percentile([], 95))
|
|
|
|
|
|
def test_write_guard_latency_timings_creates_parseable_csv(tmp_path):
|
|
sample = GuardLatencyTiming(
|
|
iteration=0,
|
|
elapsed_s=0.0,
|
|
period_ms=None,
|
|
gello_read_ms=1.0,
|
|
safety_guard_ms=2.0,
|
|
guard_path="fk_safe",
|
|
servo_j_ms=3.0,
|
|
send_action_ms=6.0,
|
|
work_ms=7.0,
|
|
cycle_ms=16.7,
|
|
)
|
|
|
|
output_path = _write_guard_latency_timings([sample], str(tmp_path), fps=60)
|
|
|
|
with output_path.open(newline="", encoding="utf-8") as stream:
|
|
rows = list(csv.DictReader(stream))
|
|
assert len(rows) == 1
|
|
assert rows[0]["period_ms"] == ""
|
|
assert rows[0]["guard_path"] == "fk_safe"
|
|
assert float(rows[0]["safety_guard_ms"]) == 2.0
|
|
assert float(rows[0]["servo_j_ms"]) == 3.0
|