Add guarded GELLO latency experiment

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_<time>.csv`.
+
 #### Configure the GELLO TCP height floor
 
 Stop other robot control processes, move the TCP to its lowest safe pose, then read its height:

README_ZH.md

@@ -116,6 +116,24 @@ uv run uf-camera-view -l -T realsense     # 列出每台相机的序列号
 
 `Space` 复位并开始，`←` 复位，`Esc` 退出。
 
+#### Guard 延迟实验
+
+以下命令在启用 `min_tcp_z_mm` 安全检测的情况下运行 60 秒，并记录控制周期、
+GELLO 读取、安全检测、ServoJ 和完整 `send_action` 耗时：
+
+```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
+```
+
+按 `Space` 复位并开始。实验期间可在确保安全的前提下分别经过远离高度下限和接近
+高度下限的区域。CSV 的 `guard_path` 会标记 `rt_fast_path`、`fk_safe`、
+`fk_ik_clamp` 或 `fallback`，终端也会按路径输出分组统计。结果写入
+`logs/gello_guard_latency_<时间>.csv`。
+
 #### 设置 GELLO TCP 最低高度
 
 先停止其他控制程序，将机械臂 TCP 移到最低安全位置，然后只读当前高度：

config/gello/xarm7_gello_cartesian_record_config.yaml

@@ -0,0 +1,42 @@
+robot:
+  type: uf::robot
+  id: "uf_robot_cartesian"
+  robot_dof: 7
+  control_space: "cartesian"
+  robot_ip: "192.168.1.245"
+  gripper_type: 1
+  max_linear_velocity: 200
+  min_tcp_z_mm: 50
+  gripper_error_log_path: "logs/xarm7_gripper_errors.log"
+  cameras:
+    camera:
+      type: intelrealsense
+      serial_number_or_name: "242622070583"
+      width: 640
+      height: 480
+      fps: 30
+    camera2:
+      type: intelrealsense
+      serial_number_or_name: "148522072685"
+      width: 640
+      height: 480
+      fps: 30
+
+teleop:
+  type: uf::gello_teleop
+  id: "gello_teleop"
+  port: "/dev/serial/by-id/usb-FTDI_USB__-__Serial_Converter_FTB9HYVD-if00-port0"
+  joint_ids: [1, 2, 3, 4, 5, 6, 7]
+  joint_signs: [1, 1, 1, 1, 1, 1, 1]
+  gripper_id: 8
+  gripper_open_deg: 198.28125
+  gripper_close_deg: 155.75
+
+dataset:
+  root: "datasets/xarm7_gello_cartesian_datas"
+  repo_id: "ufactory/xarm7_gello_cartesian_datas"
+  single_task: "Pick up the purple grape and drop into the box on the left."
+  fps: 60
+  episode_time_s: 60
+  reset_time_s: 20
+  push_to_hub: False

src/lerobot_robot_ufactory/robots/uf_robot/uf_robot.py

@@ -106,6 +106,7 @@ class UFRobot(Robot, Thread):
         self._tcp_z_guard_activation_margin_mm = self.config.tcp_z_guard_activation_margin_mm
         self._last_safe_joint_target = None
         self._last_safe_cartesian_target = None
+        self._last_guard_path = "not_run"
         self._tcp_z_is_clamped = False
         self._tcp_z_last_log_time = 0.0
         self._tcp_z_last_error_log_time = 0.0
@@ -322,6 +323,7 @@ class UFRobot(Robot, Thread):
         """Return a safe joint target, or None when motion must be skipped."""
         desired = np.asarray(command, dtype=np.float64)
         if self._min_tcp_z_mm is None:
+            self._last_guard_path = "disabled"
             return desired
 
         fallback = self._last_safe_joint_target
@@ -334,6 +336,7 @@ class UFRobot(Robot, Thread):
             # normal GELLO path keeps a stable command cadence. A large
             # activation margin absorbs ordinary per-cycle motion changes.
             if self._rt_actual_tcp_is_far_above_floor():
+                self._last_guard_path = "rt_fast_path"
                 self._last_safe_joint_target = desired.copy()
                 self._log_tcp_z_clamp(False)
                 return desired
@@ -347,6 +350,7 @@ class UFRobot(Robot, Thread):
 
             requested_z = float(pose[2])
             if requested_z >= self._min_tcp_z_mm:
+                self._last_guard_path = "fk_safe"
                 self._validate_guard_joint_target(desired, fallback, "GELLO target")
                 self._last_safe_joint_target = desired.copy()
                 self._log_tcp_z_clamp(False)
@@ -367,6 +371,7 @@ class UFRobot(Robot, Thread):
                 raise RuntimeError(f"inverse kinematics failed, code={code}")
 
             safe_target = inverse[:self._dof].copy()
+            self._last_guard_path = "fk_ik_clamp"
             self._validate_guard_joint_target(safe_target, fallback, "clamped IK target")
             code, verified_pose = self.real_arm.get_forward_kinematics(
                 safe_target.tolist(), input_is_radian=True, return_is_radian=True
@@ -384,6 +389,7 @@ class UFRobot(Robot, Thread):
             self._log_tcp_z_clamp(True, requested_z)
             return safe_target
         except Exception as exc:
+            self._last_guard_path = "fallback"
             self._log_tcp_z_guard_error(str(exc))
             if fallback is None:
                 return None
@@ -562,6 +568,21 @@ class UFRobot(Robot, Thread):
         self._is_calibrated = True
         pass # CHECK! currently No-op
 
+    def get_joint_observation(self) -> dict[str, float]:
+        """Read joint positions for teleoperator alignment in Cartesian mode."""
+        if self.real_arm is None or not self._is_connected:
+            raise ConnectionError("UF Robot is not connected")
+        code, states = self.real_arm.get_joint_states(is_radian=True, num=1)
+        if code != 0 or not states or len(states[0]) < self._dof:
+            raise RuntimeError(f"Failed to read xArm joint states, code={code}")
+        positions = np.asarray(states[0][:self._dof], dtype=np.float64)
+        if not np.all(np.isfinite(positions)):
+            raise RuntimeError("xArm joint states contain NaN or Inf")
+        return {
+            f"{self.prefix}J{i + 1}.pos": float(position)
+            for i, position in enumerate(positions)
+        }
+
     def get_observation(self) -> dict[str, np.ndarray]:
         obs_dict = {}
         self._log_controller_error_if_changed("get_observation")
@@ -721,6 +742,48 @@ class UFRobot(Robot, Thread):
         if code is not None and code != 0:
             raise RuntimeError(f"{command} failed, code={code}, {self._motion_status()}")
 
+    def joint_action_to_cartesian(self, action: dict) -> dict:
+        """Convert an absolute joint action to an xArm axis-angle pose.
+
+        GELLO reports absolute joint positions while Cartesian control expects
+        ``pose.x/y/z/rx/ry/rz``. The xArm controller's FK is used so the
+        configured robot model and tool frame stay authoritative. The returned
+        action is passed through ``send_action`` for Cartesian safety checks.
+        """
+        if self.real_arm is None or not self._is_connected:
+            raise ConnectionError("UF Robot is not connected")
+
+        joint_keys = [f"{self.prefix}J{i}.pos" for i in range(1, self._dof + 1)]
+        try:
+            joints = [float(action[key]) for key in joint_keys]
+        except (KeyError, TypeError, ValueError) as exc:
+            raise ValueError(f"Invalid joint action; expected keys {joint_keys}") from exc
+
+        if not np.all(np.isfinite(joints)):
+            raise ValueError("Joint action contains NaN or Inf")
+
+        code, pose = self.real_arm.get_forward_kinematics(
+            joints,
+            input_is_radian=True,
+            return_is_radian=True,
+        )
+        pose = np.asarray(pose, dtype=np.float64)
+        if code != 0 or pose.shape[0] < 6 or not np.all(np.isfinite(pose[:6])):
+            raise RuntimeError(f"xArm forward kinematics failed, code={code}, pose={pose}")
+
+        cartesian = {
+            f"{self.prefix}pose.x": float(pose[0]),
+            f"{self.prefix}pose.y": float(pose[1]),
+            f"{self.prefix}pose.z": float(pose[2]),
+            f"{self.prefix}pose.rx": float(pose[3]),
+            f"{self.prefix}pose.ry": float(pose[4]),
+            f"{self.prefix}pose.rz": float(pose[5]),
+        }
+        gripper_key = f"{self.prefix}gripper.pos"
+        if gripper_key in action:
+            cartesian[gripper_key] = float(action[gripper_key])
+        return cartesian
+
     def send_action(self, action: dict) -> np.ndarray:
         if not self._is_connected:
             raise ConnectionError()
@@ -741,6 +804,9 @@ class UFRobot(Robot, Thread):
             return action
 
         before_write_t = time.perf_counter()
+        self.logs["safety_guard_dt_s"] = 0.0
+        self.logs["servo_j_dt_s"] = 0.0
+        self.logs["safety_guard_path"] = "not_run"
         safe_action = dict(action)
         if self._control_space == "joint":
             # first sync with gello or other control device SLOWLY!
@@ -750,7 +816,10 @@ class UFRobot(Robot, Thread):
             cmd_list = [0]*(self._dof)
             for i in range(self._dof):
                 cmd_list[i] = action[f"{self.prefix}J{i+1}.pos"]
+            guard_start_t = time.perf_counter()
             safe_cmd = self._guard_joint_target(cmd_list)
+            self.logs["safety_guard_dt_s"] = time.perf_counter() - guard_start_t
+            self.logs["safety_guard_path"] = self._last_guard_path
             if safe_cmd is None:
                 # Do not send an unverified arm target. Gripper handling below
                 # remains independent and can continue safely.
@@ -769,9 +838,11 @@ class UFRobot(Robot, Thread):
                     code = self.real_arm.set_state(0)
                     self._check_motion_code("set_state(0)", code)
                     time.sleep(0.1)
+                servo_j_start_t = time.perf_counter()
                 code = self.real_arm.set_servo_angle_j(
                     safe_cmd[:self._dof].tolist(), speed=jnt_spd, is_radian=True
                 )
+                self.logs["servo_j_dt_s"] = time.perf_counter() - servo_j_start_t
                 self._check_motion_code("set_servo_angle_j", code)
             elif safe_cmd is not None:
                 # The legacy mode-6 path uses the absolute move_joint API.
@@ -822,6 +893,8 @@ class UFRobot(Robot, Thread):
         pass
 
     def disconnect(self) -> None:
+        if not self._is_connected:
+            return
         self.real_arm.set_state(4) # stop
         self.real_arm.set_mode(0)
         if self._use_rt_report:
@@ -835,10 +908,12 @@ class UFRobot(Robot, Thread):
 
         self._is_connected = False
 
+    @property
     def is_calibrated(self) -> bool:
         """Whether the robot is currently calibrated or not. Should be always `True` if not applicable"""
         return self._is_calibrated
 
+    @property
     def is_connected(self) -> bool:
         """Whether the robot is currently calibrated or not. Should be always `True` if not applicable"""
         return self._is_connected

src/lerobot_robot_ufactory/scripts/uf_lerobot_record.py

@@ -409,6 +409,22 @@ def record_loop(
         elif policy is None and isinstance(teleop, Teleoperator):
             act = teleop.get_action()
 
+            # GELLO reports absolute joint positions. In Cartesian robot mode,
+            # convert them with the xArm FK before the normal action pipeline;
+            # send_action() then applies the Cartesian safety guard and sends
+            # the resulting pose with set_position_aa().
+            joint_action_keys = [
+                f"{getattr(robot, 'prefix', '')}J{i}.pos"
+                for i in range(1, getattr(robot, "_dof", 0) + 1)
+            ]
+            if (
+                getattr(robot, "_control_space", None) == "cartesian"
+                and hasattr(robot, "joint_action_to_cartesian")
+                and joint_action_keys
+                and all(key in act for key in joint_action_keys)
+            ):
+                act = robot.joint_action_to_cartesian(act)
+
             # (space mouse) from delta Cartesian cmd to absolute command
             if "pose.dx" in act:
                 last_robot_cmd.update({"pose.x": last_robot_cmd["pose.x"] + act["pose.dx"], "pose.y": last_robot_cmd["pose.y"] + act["pose.dy"], "pose.z": last_robot_cmd["pose.z"] + act["pose.dz"]})
@@ -482,6 +498,11 @@ def _prepare_recording_episode(robot, teleop, is_uf_teleop, manual_mode):
 
     if is_uf_teleop:
         obs = robot.get_observation()
+        # Cartesian observations expose TCP pose, while GELLO alignment needs
+        # the current absolute joint positions.
+        joint_observation = getattr(robot, "get_joint_observation", None)
+        if joint_observation is not None:
+            obs.update(joint_observation())
         teleop.set_teleop_enabled(True, obs)
 
 

src/lerobot_robot_ufactory/scripts/uf_robot_teleop.py

@@ -1,10 +1,13 @@
 import sys
 import argparse
 import atexit
+import csv
 import logging
+import math
 import time
 from pathlib import Path
 from dataclasses import asdict, dataclass
+from datetime import datetime
 from pprint import pformat
 import lerobot_robot_ufactory # patch
 from lerobot.scripts.lerobot_record import register_third_party_plugins
@@ -34,8 +37,15 @@ class TeleopConfig:
     robot: RobotConfig
     teleop: TeleoperatorConfig
     fps: int = 30
+    guard_latency_experiment: bool = False
+    experiment_duration_s: float = 60.0
+    timing_log_dir: str = "logs"
 
     def __post_init__(self):
+        if self.fps <= 0:
+            raise ValueError("fps must be positive")
+        if not math.isfinite(self.experiment_duration_s) or self.experiment_duration_s <= 0:
+            raise ValueError("experiment_duration_s must be finite and positive")
         if hasattr(self.robot, 'robots'):
             for _, robot in self.robot.robots.items():
                 robot.cameras = {}
@@ -43,10 +53,120 @@ class TeleopConfig:
             self.robot.cameras = {}
 
 
+@dataclass
+class GuardLatencyTiming:
+    iteration: int
+    elapsed_s: float
+    period_ms: float | None
+    gello_read_ms: float
+    safety_guard_ms: float
+    guard_path: str
+    servo_j_ms: float
+    send_action_ms: float
+    work_ms: float
+    cycle_ms: float
+
+
+def _percentile(values: list[float], percentile: float) -> float:
+    values = [value for value in values if math.isfinite(value)]
+    if not values:
+        return float("nan")
+    ordered = sorted(values)
+    index = (len(ordered) - 1) * percentile / 100
+    lower = math.floor(index)
+    upper = math.ceil(index)
+    if lower == upper:
+        return ordered[lower]
+    return ordered[lower] + (ordered[upper] - ordered[lower]) * (index - lower)
+
+
+def _write_guard_latency_timings(
+    samples: list[GuardLatencyTiming], log_dir: str, fps: int
+) -> Path:
+    output_dir = Path(log_dir).expanduser()
+    output_dir.mkdir(parents=True, exist_ok=True)
+    timestamp = datetime.now().astimezone().strftime("%Y%m%d_%H%M%S")
+    output_path = output_dir / f"gello_guard_latency_{timestamp}.csv"
+    fieldnames = list(GuardLatencyTiming.__dataclass_fields__)
+    with output_path.open("w", newline="", encoding="utf-8") as stream:
+        writer = csv.DictWriter(stream, fieldnames=fieldnames)
+        writer.writeheader()
+        for sample in samples:
+            writer.writerow(asdict(sample))
+
+    target_ms = 1000 / fps
+    period_values = [sample.period_ms for sample in samples if sample.period_ms is not None]
+    overruns = sum(sample.work_ms > target_ms for sample in samples)
+    logging.info("Guard latency timing written to %s", output_path)
+    for name, values in (
+        ("loop period", period_values),
+        ("GELLO read", [sample.gello_read_ms for sample in samples]),
+        ("safety guard", [sample.safety_guard_ms for sample in samples]),
+        ("ServoJ", [sample.servo_j_ms for sample in samples]),
+        ("send_action", [sample.send_action_ms for sample in samples]),
+        ("loop work", [sample.work_ms for sample in samples]),
+    ):
+        logging.info(
+            "%s: p50=%.3f ms, p95=%.3f ms, p99=%.3f ms, max=%.3f ms",
+            name,
+            _percentile(values, 50),
+            _percentile(values, 95),
+            _percentile(values, 99),
+            max(values, default=float("nan")),
+        )
+    path_counts = {}
+    for sample in samples:
+        path_counts[sample.guard_path] = path_counts.get(sample.guard_path, 0) + 1
+    logging.info("guard paths: %s", path_counts)
+    for path in sorted(path_counts):
+        path_samples = [sample for sample in samples if sample.guard_path == path]
+        guard_values = [sample.safety_guard_ms for sample in path_samples]
+        send_values = [sample.send_action_ms for sample in path_samples]
+        path_overruns = sum(sample.work_ms > target_ms for sample in path_samples)
+        logging.info(
+            "guard path %s: n=%d, guard p50/p95/p99=%.3f/%.3f/%.3f ms, "
+            "send p50/p95/p99=%.3f/%.3f/%.3f ms, overruns=%d",
+            path,
+            len(path_samples),
+            _percentile(guard_values, 50),
+            _percentile(guard_values, 95),
+            _percentile(guard_values, 99),
+            _percentile(send_values, 50),
+            _percentile(send_values, 95),
+            _percentile(send_values, 99),
+            path_overruns,
+        )
+    logging.info(
+        "deadline overruns (> %.3f ms work): %d/%d (%.2f%%)",
+        target_ms,
+        overruns,
+        len(samples),
+        100 * overruns / len(samples) if samples else 0,
+    )
+    return output_path
+
+
+def _validate_guard_latency_config(cfg: TeleopConfig) -> None:
+    if getattr(cfg.robot, "control_space", None) != "joint":
+        raise ValueError("Guard latency experiment requires robot.control_space='joint'")
+    if getattr(cfg.robot, "joint_command_mode", None) != 1:
+        raise ValueError("Guard latency experiment requires robot.joint_command_mode=1 (ServoJ)")
+    if getattr(cfg.robot, "min_tcp_z_mm", None) is None:
+        raise ValueError("Guard latency experiment requires min_tcp_z_mm to be enabled")
+
+
 def teleop_loop(cfg: TeleopConfig):
     init_logging()
     logging.info(pformat(asdict(cfg)))
 
+    if cfg.guard_latency_experiment:
+        _validate_guard_latency_config(cfg)
+        logging.warning(
+            "Guard latency experiment enabled: measuring GELLO, safety guard, ServoJ, "
+            "and total send latency for %.1f active seconds",
+            cfg.experiment_duration_s,
+        )
+
     teleop = make_teleoperator_from_config(cfg.teleop)
     if hasattr(cfg.robot, "teleop"):
         cfg.robot.teleop = teleop
@@ -160,6 +280,9 @@ def teleop_loop(cfg: TeleopConfig):
 
     key_space_pressed = False
     key_left_pressed = False
+    latency_samples: list[GuardLatencyTiming] = []
+    experiment_start_t = None
+    previous_command_t = None
 
     while not events["exit"]:
         start_loop_t = time.perf_counter()
@@ -200,19 +323,56 @@ def teleop_loop(cfg: TeleopConfig):
             if is_reset or is_paused:
                 continue
 
-        # Get robot observation
+        if cfg.guard_latency_experiment:
-        obs = robot.get_observation()
+            if experiment_start_t is None:
+                experiment_start_t = start_loop_t
+            period_ms = None
+            if previous_command_t is not None:
+                period_ms = (start_loop_t - previous_command_t) * 1e3
+            previous_command_t = start_loop_t
 
-        act = teleop.get_action()
+            read_start_t = time.perf_counter()
-        act_processed_teleop = teleop_action_processor((act, obs))
+            act = teleop.get_action()
+            read_end_t = time.perf_counter()
+            robot.send_action(act)
+            send_end_t = time.perf_counter()
+            robot_logs = getattr(robot, "logs", {})
+            work_s = send_end_t - start_loop_t
+            precise_sleep(max(sleep_time_s - work_s, 0.0))
+            cycle_end_t = time.perf_counter()
+            latency_samples.append(
+                GuardLatencyTiming(
+                    iteration=len(latency_samples),
+                    elapsed_s=start_loop_t - experiment_start_t,
+                    period_ms=period_ms,
+                    gello_read_ms=(read_end_t - read_start_t) * 1e3,
+                    safety_guard_ms=float(robot_logs.get("safety_guard_dt_s", float("nan"))) * 1e3,
+                    guard_path=str(robot_logs.get("safety_guard_path", "unknown")),
+                    servo_j_ms=float(robot_logs.get("servo_j_dt_s", float("nan"))) * 1e3,
+                    send_action_ms=(send_end_t - read_end_t) * 1e3,
+                    work_ms=work_s * 1e3,
+                    cycle_ms=(cycle_end_t - start_loop_t) * 1e3,
+                )
+            )
+            if cycle_end_t - experiment_start_t >= cfg.experiment_duration_s:
+                events["exit"] = True
+        else:
+            # Get robot observation
+            obs = robot.get_observation()
 
-        robot_action_to_send = robot_action_processor((act_processed_teleop, obs))
+            act = teleop.get_action()
-        robot.send_action(robot_action_to_send)
+            act_processed_teleop = teleop_action_processor((act, obs))
 
-        dt_s = time.perf_counter() - start_loop_t
+            robot_action_to_send = robot_action_processor((act_processed_teleop, obs))
-        precise_sleep(sleep_time_s - dt_s)
+            robot.send_action(robot_action_to_send)
+
+            dt_s = time.perf_counter() - start_loop_t
+            precise_sleep(max(sleep_time_s - dt_s, 0.0))
     
     print("\n********** Teleop Control Loop Exit **********")
+    if latency_samples:
+        output_path = _write_guard_latency_timings(latency_samples, cfg.timing_log_dir, cfg.fps)
+        print(f"Guard latency timing log: {output_path}")
     cleanup_connections()
     atexit.unregister(cleanup_connections)
 

tests/test_guard_latency.py

@@ -0,0 +1,81 @@
+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

tests/test_manual_mode.py

@@ -119,6 +119,7 @@ def test_manual_mode_robot_enters_teaching_mode_without_sending_actions(monkeypa
     robot = uf_robot_module.UFRobot(config)
 
     robot.connect()
+    assert robot.is_connected
     assert arm.mode == 2
     assert ("set_teach_sensitivity", 4) in arm.calls
     assert robot._initial_point == arm.initial_point
@@ -147,9 +148,14 @@ def test_manual_mode_robot_enters_teaching_mode_without_sending_actions(monkeypa
     assert observation["J6.pos"] == 5.0
 
     robot.disconnect()
+    assert not robot.is_connected
     assert arm.mode == 0
     assert ("disconnect",) in arm.calls
 
+    call_count = len(arm.calls)
+    robot.disconnect()
+    assert len(arm.calls) == call_count
+
 
 def test_robot_reset_uses_sdk_initial_point_in_normal_mode(monkeypatch, tmp_path):
     from lerobot_robot_ufactory.robots.uf_robot import uf_robot as uf_robot_module

tests/test_tcp_z_safety.py

@@ -59,6 +59,7 @@ def test_joint_target_above_floor_is_unchanged():
     result = robot._guard_joint_target(requested)
 
     assert np.allclose(result, requested)
+    assert robot._last_guard_path == "fk_safe"
     assert arm.inverse_calls == []
     assert np.allclose(robot._last_safe_joint_target, requested)
 
@@ -74,6 +75,7 @@ def test_joint_guard_uses_rt_report_fast_path_far_above_floor():
     result = robot._guard_joint_target([0.1] * 7)
 
     assert np.allclose(result, [0.1] * 7)
+    assert robot._last_guard_path == "rt_fast_path"
     assert arm.inverse_calls == []
 
 
@@ -85,6 +87,7 @@ def test_joint_target_below_floor_clamps_only_tcp_z_before_inverse_kinematics():
     result = robot._guard_joint_target(requested)
 
     assert np.allclose(result, arm.inverse_result)
+    assert robot._last_guard_path == "fk_ik_clamp"
     inverse_pose, inverse_kwargs = arm.inverse_calls[0]
     assert inverse_pose == pytest.approx([300.0, 10.0, 100.0, 0.1, 0.2, 0.3])
     assert inverse_kwargs["limited"] is True
@@ -175,6 +178,9 @@ def test_send_action_sends_and_returns_clamped_joint_target():
     sent_action = robot.send_action(action)
 
     assert arm.sent_joint_targets == [pytest.approx(arm.inverse_result)]
+    assert robot.logs["safety_guard_dt_s"] >= 0
+    assert robot.logs["safety_guard_path"] == "fk_ik_clamp"
+    assert robot.logs["servo_j_dt_s"] >= 0
     assert [sent_action[f"J{i + 1}.pos"] for i in range(7)] == pytest.approx(
         arm.inverse_result
     )