From c0e950f4f18c5136e5b2938bc2648651cd03c5b3 Mon Sep 17 00:00:00 2001 From: ChenYuhan <2514158309@qq.com> Date: Mon, 17 Aug 2026 10:01:29 +0800 Subject: [PATCH] Add guarded GELLO latency experiment --- README.md | 19 + README_ZH.md | 18 + .../xarm7_gello_cartesian_record_config.yaml | 42 + ..._experiment_1_baseline_20260817_094737.csv | 2588 ++++++++++++ ..._experiment_1_baseline_20260817_094957.csv | 3572 +++++++++++++++++ .../robots/uf_robot/uf_robot.py | 75 + .../scripts/uf_lerobot_record.py | 21 + .../scripts/uf_robot_teleop.py | 176 +- tests/test_guard_latency.py | 81 + tests/test_manual_mode.py | 6 + tests/test_tcp_z_safety.py | 6 + 11 files changed, 6596 insertions(+), 8 deletions(-) create mode 100644 config/gello/xarm7_gello_cartesian_record_config.yaml create mode 100644 logs/gello_experiment_1_baseline_20260817_094737.csv create mode 100644 logs/gello_experiment_1_baseline_20260817_094957.csv create mode 100644 tests/test_guard_latency.py diff --git a/README.md b/README.md index c2a6aec..6b29089 100644 --- a/README.md +++ b/README.md @@ -125,6 +125,25 @@ uv run uf-robot-teleop --config_path config/gello/xarm7_gello_record_config.yaml `Space` reset & start, `←` reset, `Esc` exit. +#### Guard latency experiment + +This command runs for 60 seconds with the `min_tcp_z_mm` guard enabled and +records the loop period, GELLO read, safety guard, ServoJ, and complete +`send_action` latency: + +```bash +uv run uf-robot-teleop \ + --config_path config/gello/xarm7_gello_record_config.yaml \ + --fps 60 \ + --guard_latency_experiment=true \ + --experiment_duration_s 60 +``` + +Press `Space` to reset and start. While staying safe, include motion both far +from and near the configured height floor. The CSV `guard_path` column marks +`rt_fast_path`, `fk_safe`, `fk_ik_clamp`, or `fallback`, and the terminal prints +per-path summaries. Results are written to `logs/gello_guard_latency_