add model inference evaluation

.gitignore

@@ -1,6 +1,7 @@
 # ============================
 # Python
 # ============================
+**/logs/**
 __pycache__/
 *.py[cod]
 *$py.class
@@ -90,3 +91,8 @@ models/
 ufactory_usage/
 .history/
 datasets/
+tmp/
+
+# Local checkout used for GELLO hardware development. Runtime fixes live in
+# the main package so the repository does not depend on a modified submodule.
+third_party/gello_software/

README.md

@@ -72,6 +72,7 @@ Predefined configs are provided under `config/`:
 - `teleop.joint_ids` / `teleop.joint_signs` — per-arm servo mapping and direction
 - `teleop.start_joints` — GELLO calibration reference, should match the xArm SDK initial point (degrees)
 - `teleop.gripper_id` — GELLO gripper servo ID (`8`; `-1` disables it)
+- `teleop.realtime_control_fps` — independent GELLO-to-xArm command loop rate; it is separate from `dataset.fps`
 - `dataset.root` / `dataset.repo_id` — where the dataset is stored
 - `dataset.single_task` — task description saved with each frame
 - `dataset.fps` / `episode_time_s` / `reset_time_s` — recording timing
@@ -84,6 +85,7 @@ Predefined configs are provided under `config/`:
 - `robot.teach_sensitivity` — teaching sensitivity, valid range 1–5
 - `robot.manual_gripper_speed` — gripper velocity in normalized position per second (default `0.5`)
 - `robot.observe_joint_vel` — record joint velocities in observations (`false` by default)
+- `robot.enable_logs` — enable optional per-cycle timing/diagnostic logs (`false` by default)
 - `robot.cameras.camera` — Intel RealSense camera (`serial_number_or_name`, resolution, fps)
 - `dataset.root` / `dataset.repo_id` / `single_task` / `fps` / `episode_time_s` / `reset_time_s` / `num_episodes` — dataset settings
 
@@ -125,21 +127,71 @@ 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 \
+  --robot.enable_logs=true \
+  --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. With `tcp_z_guard_backend:
+local_projection`, the CSV `guard_path` column marks `local_safe`,
+`local_projected`, `local_hold`, or `model_fault`. 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:
+
+```bash
+uv run uf-read-tcp-z \
+  --config-path config/gello/xarm7_gello_record_config.yaml \
+  --margin-mm 5
+```
+
+The command does not move the arm. Put the hard floor in `min_tcp_z_mm`; the
+configured `tcp_z_soft_margin_mm` is added above it for CPU-local projection.
+The xArm7 GELLO joint path preserves all seven joint targets and projects only
+the component that would cross the soft TCP-height plane. Controller Safety
+Boundary remains enabled at the hard floor as a final stop.
+
+> This protects the TCP against crossing a horizontal plane. It does not detect collisions involving links, the elbow, or the gripper body, and it does not replace the emergency stop. Measure again after changing the tool, TCP offset, robot base, or table position.
+
 ### 2. GELLO data collection
 
 ```bash
 # Record a new dataset
-uv run uf-lerobot-record --config_path config/gello/xarm7_gello_record_config.yaml
+uv run record --config_path config/gello/xarm7_gello_record_config.yaml
 
 # Continue recording on an existing dataset
-uv run uf-lerobot-record --config_path config/gello/xarm7_gello_record_config.yaml -r
+uv run record --config_path config/gello/xarm7_gello_record_config.yaml -r
 
 # Optional: save episodes in the background
-uv run uf-lerobot-record --config_path config/gello/xarm7_gello_record_config.yaml -a
+uv run record --config_path config/gello/xarm7_gello_record_config.yaml -a
 ```
 
 Controls: `Space` start the episode, `→` save it, `←` discard and re-record it, `Esc` stop recording. The arm resets to its initial point between episodes.
 
+The xArm7 GELLO example also enables a lightweight browser camera preview at
+`http://127.0.0.1:8765/` (or `http://<recorder-ip>:8765/` from another machine).
+It reuses frames already captured by the recorder: JPEG encoding and HTTP
+streaming run on background threads, the preview is capped at 8 FPS, and stale
+preview frames are dropped rather than delaying dataset collection. Disable it
+or tune it in the top-level `web_preview` section of the YAML configuration.
+When `robot.enable_logs` is enabled for diagnostics, one
+`logs/gello_record_sync_*.csv` file is written per episode. The
+`preview_clients`, `record_period_ms`, `frame_overrun_ms`, `action_age_ms`, and
+`preview_publish_ms` columns can be used to measure preview timing impact.
+
 > During collection the **relative position between the robot arm and the camera must not change**, and the camera setup at inference time must match the one used during collection. If the arm or camera moves, previously collected data becomes invalid.
 
 > If the dataset root already exists and `-r` is not given, the script asks whether to overwrite it, resume, or cancel.
@@ -151,7 +203,7 @@ Controls: `Space` start the episode, `→` save it, `←` discard and re-record
 ./start_manual_record.sh -r   # force resume; fails if the dataset directory does not exist
 ```
 
-The record script (`uf-lerobot-record`) reads `dataset.root` from the config and checks the path before recording, then:
+The `record` command reads `dataset.root` from the config and checks the path before recording, then:
 
 - Directory does not exist → records a new dataset.
 - Directory already exists (valid LeRobot dataset) and no `-r` was given → asks interactively:
@@ -160,7 +212,7 @@ The record script (`uf-lerobot-record`) reads `dataset.root` from the config and
   - `c` cancel
 - Directory exists but is incomplete (missing required metadata or data parquet files) → asks to overwrite it or cancel; non-interactive runs error out instead.
 
-Pass `-r` to resume directly without asking. Note that `./start_manual_record.sh` keeps its original launcher behavior — it automatically resumes an existing valid dataset (equivalent to `-r`), so run `uv run uf-lerobot-record --config_path config/manual_mode/xarm7_manual_record_config.yaml` directly if you want to see the overwrite/resume prompt.
+Pass `-r` to resume directly without asking. Note that `./start_manual_record.sh` keeps its original launcher behavior — it automatically resumes an existing valid dataset (equivalent to `-r`), so run `uv run record --config_path config/manual_mode/xarm7_manual_record_config.yaml` directly if you want to see the overwrite/resume prompt.
 
 During recording the arm is in teach mode: the actual joint state is written as both the observation and the action. Hold `C` to slowly close the gripper and `O` to slowly open it. Controls: `Space` start, `→` save, `←` discard & re-record, `Esc` stop. Reset the arm manually between episodes.
 
@@ -203,15 +255,15 @@ uv run uf-lerobot-eval \
 Replay the absolute joint states (`observation.state`) of a manual-drag episode on an xArm7. States are sent as absolute targets at the dataset FPS (default 30), so the motion matches the recording:
 
 ```bash
-uv run uf-lerobot-replay \
+uv run replay \
   --dataset-root /path/to/xarm7_manual_datas \
   --robot-ip 192.168.1.245
 
 # Skip the interactive confirmation (non-interactive use)
-uv run uf-lerobot-replay --dataset-root /path/to/xarm7_manual_datas --robot-ip 192.168.1.245 --yes
+uv run replay --dataset-root /path/to/xarm7_manual_datas --robot-ip 192.168.1.245 --yes
 
 # Replay another episode
-uv run uf-lerobot-replay --dataset-root /path/to/xarm7_manual_datas --robot-ip 192.168.1.245 --episode-index 3
+uv run replay --dataset-root /path/to/xarm7_manual_datas --robot-ip 192.168.1.245 --episode-index 3
 ```
 
 The robot first moves to the xArm SDK initial point, then replays the episode and stays at the last state. Make sure the workspace is clear and the recorded initial pose matches the current arm setup.

README_ZH.md

@@ -72,6 +72,7 @@ ls /dev/serial/by-id/
 - `teleop.joint_ids` / `teleop.joint_signs` — 各型号机械臂的舵机映射与方向
 - `teleop.start_joints` — GELLO 校准参考值（角度），应与 xArm SDK 初始点一致
 - `teleop.gripper_id` — GELLO 夹爪舵机 ID（`8`；`-1` 表示无夹爪）
+- `teleop.realtime_control_fps` — GELLO 到 xArm 的独立实时控制频率，与 `dataset.fps` 分开
 - `dataset.root` / `dataset.repo_id` — 数据集保存位置
 - `dataset.single_task` — 随每一帧保存的任务描述
 - `dataset.fps` / `episode_time_s` / `reset_time_s` — 录制时序参数
@@ -84,6 +85,7 @@ ls /dev/serial/by-id/
 - `robot.teach_sensitivity` — 示教灵敏度，有效范围 1–5
 - `robot.manual_gripper_speed` — 夹爪速度（每秒归一化位置变化，默认 `0.5`）
 - `robot.observe_joint_vel` — 是否在观测中记录关节速度（默认 `false`）
+- `robot.enable_logs` — 是否启用每帧耗时和诊断日志（默认 `false`）
 - `robot.cameras.camera` — Intel RealSense 相机配置（`serial_number_or_name`、分辨率、fps）
 - `dataset.root` / `dataset.repo_id` / `single_task` / `fps` / `episode_time_s` / `reset_time_s` / `num_episodes` — 数据集配置
 
@@ -113,28 +115,62 @@ uv run uf-camera-view -l -T realsense     # 列出每台相机的序列号
 
 ## 使用
 
-### 1. GELLO 遥操作测试
-
-不录制数据，仅测试 GELLO 与机械臂的联动：
-
-```bash
-uv run uf-robot-teleop --config_path config/gello/xarm7_gello_record_config.yaml
-uv run uf-robot-teleop --config_path config/gello/xarm7_gello_record_config.yaml --fps 60  # 可选，指定循环频率
-```
 
 `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 \
+  --robot.enable_logs=true \
+  --fps 60 \
+  --guard_latency_experiment=true \
+  --experiment_duration_s 60
+```
+
+按 `Space` 复位并开始。实验期间可在确保安全的前提下分别经过远离高度下限和接近
+高度下限的区域。使用 `tcp_z_guard_backend: local_projection` 时，CSV 的
+`guard_path` 会标记 `local_safe`、`local_projected`、`local_hold` 或
+`model_fault`，终端也会按路径输出分组统计。结果写入
+`logs/gello_guard_latency_<时间>.csv`。
+
+实验结果与分析见 [GELLO 安全高度 Guard 延迟实验记录](docs/gello_guard_latency_experiment_20260817.md)。
+
+完整的抖动修复、数据同步和夹爪 Error 19 排障过程见
+[xArm7 + GELLO 平滑安全录制实践](docs/gello_xarm7_smooth_safe_recording_zh.md)。
+
+#### 设置 GELLO TCP 最低高度
+
+先停止其他控制程序，将机械臂 TCP 移到最低安全位置，然后只读当前高度：
+
+```bash
+uv run uf-read-tcp-z \
+  --config-path config/gello/xarm7_gello_record_config.yaml \
+  --margin-mm 5
+```
+
+该命令不会移动机械臂。将硬下限填入 `min_tcp_z_mm`；CPU 本地投影会在其上
+额外叠加 `tcp_z_soft_margin_mm`。xArm7 GELLO 关节路径会保留全部七个关节目标，
+只投影会穿过 TCP 软高度面的运动分量；控制器 Safety Boundary 则在硬下限处
+作为最后一道停止保护。
+
+> 该限制只保护 TCP 不低于一个水平面，不能检测机械臂连杆、肘部或夹爪外形与桌子的碰撞，也不能替代急停。更换工具、TCP 偏置、底座或桌面位置后必须重新测量。
+
 ### 2. GELLO 数据采集
 
 ```bash
 # 录制新数据集
-uv run uf-lerobot-record --config_path config/gello/xarm7_gello_record_config.yaml
+uv run record --config_path config/gello/xarm7_gello_record_config.yaml
 
 # 在已有数据集上续录
-uv run uf-lerobot-record --config_path config/gello/xarm7_gello_record_config.yaml -r
+uv run record --config_path config/gello/xarm7_gello_record_config.yaml -r
 
 # 可选：后台异步保存 episode
-uv run uf-lerobot-record --config_path config/gello/xarm7_gello_record_config.yaml -a
+uv run record --config_path config/gello/xarm7_gello_record_config.yaml -a
 ```
 
 按键控制：`Space` 开始当前 episode，`→` 保存，`←` 放弃并重录，`Esc` 停止录制。每个 episode 之间机械臂会自动复位到初始点。
@@ -150,7 +186,7 @@ uv run uf-lerobot-record --config_path config/gello/xarm7_gello_record_config.ya
 ./start_manual_record.sh -r   # 强制续录；数据集目录不存在时会报错
 ```
 
-录制脚本（`uf-lerobot-record`）会从配置读取 `dataset.root` 并在录制前检查路径，然后：
+`record` 命令会从配置读取 `dataset.root` 并在录制前检查路径，然后：
 
 - 目录不存在：直接录制新数据集。
 - 目录已存在（有效 LeRobot 数据集）且未加 `-r`：交互询问：
@@ -159,7 +195,7 @@ uv run uf-lerobot-record --config_path config/gello/xarm7_gello_record_config.ya
   - `c` 取消
 - 目录存在但不完整（缺少必要元数据或数据 parquet 文件）：询问覆盖或取消；非交互运行时直接报错。
 
-加 `-r` 可跳过询问直接续录。注意 `./start_manual_record.sh` 保持原有启动脚本行为——检测到有效数据集会自动续录（相当于 `-r`），如果想看到覆盖/续录的询问，请直接用 `uv run uf-lerobot-record --config_path config/manual_mode/xarm7_manual_record_config.yaml` 运行。
+加 `-r` 可跳过询问直接续录。注意 `./start_manual_record.sh` 保持原有启动脚本行为——检测到有效数据集会自动续录（相当于 `-r`），如果想看到覆盖/续录的询问，请直接用 `uv run record --config_path config/manual_mode/xarm7_manual_record_config.yaml` 运行。
 
 录制时机械臂处于示教模式，实际关节状态会同时作为 observation 和 action 写入数据集。按住 `C` 缓慢闭合夹爪，按住 `O` 缓慢张开。按键控制：`Space` 开始，`→` 保存，`←` 放弃并重录，`Esc` 停止。episode 之间手动复位机械臂。
 
@@ -203,15 +239,15 @@ uv run uf-lerobot-eval \
 
 ```bash
 # 默认回放第一条
-uv run uf-lerobot-replay \
+uv run replay \
   --dataset-root /path/to/xarm7_manual_datas \
   --robot-ip 192.168.1.245
 
 # 跳过交互确认（无人值守）
-uv run uf-lerobot-replay --dataset-root /path/to/xarm7_manual_datas --robot-ip 192.168.1.245 --yes
+uv run replay --dataset-root /path/to/xarm7_manual_datas --robot-ip 192.168.1.245 --yes
 
 # 回放其他 episode
-uv run uf-lerobot-replay --dataset-root /path/to/xarm7_manual_datas --robot-ip 192.168.1.245 --episode-index 3
+uv run replay --dataset-root /path/to/xarm7_manual_datas --robot-ip 192.168.1.245 --episode-index 3
 ```
 
 回放开始前机械臂会先移动到 xArm SDK 初始点，播放结束后保持最后一帧姿态并断开连接。执行前请确认工作空间无障碍物，且数据中的初始姿态与当前设备一致。

config/gello/xarm5_gello_record_config.yaml

@@ -5,10 +5,12 @@ robot:
   control_space: "joint"
   robot_ip: "192.168.1.245"
   gripper_type: 1
+  enable_logs: false
 
 # make sure to edit with your correct configurations!
 teleop:
   type: uf::gello_teleop
+  realtime_control_fps: 60
   port: "/dev/serial/by-id/usb-FTDI_USB__-__Serial_Converter_FTAJZYC7-if00-port0"
   joint_ids: [1, 2, 4, 6, 7]
   joint_signs: [1, 1, -1, 1, 1]
@@ -16,8 +18,8 @@ teleop:
   gripper_id: 8
 
 dataset:
-  # root of local repo: /home/<user_name>/.cache/huggingface/lerobot (default)
-  root: "/home/uf/Data/lerobot_datas/record/ufactory/xarm5_gello_datas"
+  # Dataset path relative to the directory where the command is started.
+  root: "datasets/xarm5_gello_datas"
   repo_id: "ufactory/xarm5_gello_datas"
   single_task: "Pick up the purple grape and drop into the box on the left."
   fps: 30

config/gello/xarm6_gello_record_config.yaml

@@ -5,10 +5,12 @@ robot:
   control_space: "joint"
   robot_ip: "192.168.1.245"
   gripper_type: 1
+  enable_logs: false
 
 # make sure to edit with your correct configurations!
 teleop:
   type: uf::gello_teleop
+  realtime_control_fps: 60
   port: "/dev/serial/by-id/usb-FTDI_USB__-__Serial_Converter_FTAJZYC7-if00-port0"
   joint_ids: [1, 2, 4, 5, 6, 7]
   joint_signs: [1, 1, -1, 1, 1, 1]
@@ -16,8 +18,8 @@ teleop:
   gripper_id: 8
 
 dataset:
-  # root of local repo: /home/<user_name>/.cache/huggingface/lerobot (default)
-  root: "/home/uf/Data/lerobot_datas/record/ufactory/xarm6_gello_datas"
+  # Dataset path relative to the directory where the command is started.
+  root: "datasets/xarm6_gello_datas"
   repo_id: "ufactory/xarm6_gello_datas"
   single_task: "Pick up the purple grape and drop into the box on the left."
   fps: 30

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: -2.0
+  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

config/gello/xarm7_gello_record_config.yaml

@@ -5,22 +5,69 @@ robot:
   control_space: "joint"
   robot_ip: "192.168.1.245"
   gripper_type: 1
+  enable_logs: false
+  # Up to 60 Hz goal updates; the first command after idle is immediate.
+  # Keeping the gripper below its 5000 maximum speed avoids C19 in testing.
+  gripper_command_interval_s: 0.0166667
+  # Avoid the previous maximum-speed (5000) default on the tool RS485 device.
+  gripper_speed: 1500
+  # Use the high-frequency servo interface for lower-latency GELLO tracking.
+  joint_command_mode: 1
+  max_joint_velocity: 120
+  # TCP z floor in the xArm base coordinate system (mm).
+  min_tcp_z_mm: -2.0
+  # CPU-local FK/Jacobian projection keeps ServoJ free of synchronous SDK queries.
+  tcp_z_guard_backend: "local_projection"
+  tcp_z_soft_margin_mm: 0.5
+  local_kinematics_max_error_mm: 2.0
+  controller_safety_boundary: true
+  # Append gripper initialization/read/write failures here.
+  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
   # Redundant args, indicating the initial pose of xarm7. Set by 192.168.1.245:18333
   # start_joints: [0, -30, 0, 0, 0, 30, 0]
-
 # make sure to edit with your correct configurations!
 teleop:
   type: uf::gello_teleop
   id: "gello_teleop"
-  port: "/dev/serial/by-id/usb-FTDI_USB__-__Serial_Converter_FTAJZYC7-if00-port0"
-  # GELLO calibration reference; matches the xArm SDK initial point.
-  start_joints: [0, -30, 0, 0, 0, 30, 0]
+  # Independent GELLO -> xArm command loop; dataset.fps remains the recording rate.
+  realtime_control_fps: 60
+  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
+
+# Lossy browser preview of the frames already captured for the dataset.
+# JPEG encoding and network I/O run in the background; preview frames are
+# dropped instead of delaying the 30 Hz recorder or 60 Hz GELLO control loop.
+web_preview:
+  enabled: true
+  host: "0.0.0.0"
+  port: 8765
+  fps: 8
+  width: 480
+  jpeg_quality: 65
 
 dataset:
-  # root of local repo: /home/<user_name>/.cache/huggingface/lerobot (default)
-  root: "/home/uf/Data/lerobot_datas/record/ufactory/xarm6_gello_datas"
-  repo_id: "ufactory/xarm6_gello_datas"
+  # Dataset path relative to the directory where the command is started.
+  root: "datasets/xarm7_gello_joint_safe_datas"
+  repo_id: "ufactory/xarm7_gello_joint_safe_datas"
   single_task: "Pick up the purple grape and drop into the box on the left."
+  # Cameras deliver 30 Hz; arm ServoJ remains independently fixed at 60 Hz.
   fps: 30
   episode_time_s: 60  # max duration for one episode
   reset_time_s: 20    # time for resetting env between episodes

config/manual_mode/xarm7_manual_record_config.yaml

@@ -7,24 +7,26 @@ robot:
   robot_ip: "192.168.1.245"
   # Gripper type: 1 is the xArm gripper.
   gripper_type: 1
+  enable_logs: false
   manual_mode: true
   # Normalized gripper position change per second while holding C/O.
   manual_gripper_speed: 0.5
   # Teaching sensitivity, valid range is 1-5, affecting vel of the robot.
-  teach_sensitivity: 3
+  teach_sensitivity: 5
   # Whether to record joint velocities in observations.
   observe_joint_vel: false
 
   cameras:
     camera:
       type: intelrealsense
-      serial_number_or_name: "148522072685"
+      serial_number_or_name: "242622070583"
       width: 640
       height: 480
       fps: 30
 
 dataset:
-  root: "/home/wsx/code/lerobot_robot_ufactory/datasets/xarm7_manual_replay_pick_bottle"
+  # Dataset path relative to the directory where the command is started.
+  root: "datasets/xarm7_manual_replay_pick_bottle"
   repo_id: "zjuscl/xarm7_manual_replay_pick_bottle"
   # Task description stored with each recorded frame.
   single_task: "Pick up the black bottle and place it on the blue bag"
@@ -37,5 +39,3 @@ dataset:
   # Store camera observations as videos.
   video: true
   push_to_hub: false
-
-

config/pika/pika_record_config.yaml

@@ -24,8 +24,8 @@ teleop:
   robot_base_pose: [400, 0, 400, 180, 0, 0]
 
 dataset:
-  # root of local repo: /home/<user_name>/.cache/huggingface/lerobot (default)
-  root: "/home/uf/Data/lerobot_datas/record/ufactory/pika_datas"
+  # Dataset path relative to the directory where the command is started.
+  root: "datasets/pika_datas"
   repo_id: "ufactory/pika_datas"
   single_task: "Pick up the purple grape and drop into the box on the left."
   fps: 30

config/pika/xarm7_pika_record_config.yaml

@@ -5,6 +5,7 @@ robot:
   control_space: "cartesian"
   robot_ip: "192.168.1.85"
   gripper_type: 10
+  enable_logs: false
   max_linear_velocity: 200
   cameras:
     fisheye:
@@ -24,8 +25,8 @@ teleop:
   robot_base_pose: [400, 0, 400, 180, 0, 0]
 
 dataset:
-  # root of local repo: /home/<user_name>/.cache/huggingface/lerobot (default)
-  root: "/home/uf/Data/lerobot_datas/record/ufactory/xarm7_pika_datas"
+  # Dataset path relative to the directory where the command is started.
+  root: "datasets/xarm7_pika_datas"
   repo_id: "ufactory/xarm7_pika_datas"
   single_task: "Pick up the purple grape and drop into the box on the left."
   fps: 30

config/spacemouse/xarm7_spacemouse_record_config.yaml

@@ -5,6 +5,7 @@ robot:
   control_space: "cartesian"
   robot_ip: "192.168.1.127"
   gripper_type: 0
+  enable_logs: false
 
   cameras:
     overhead:
@@ -27,8 +28,8 @@ teleop:
   use_gripper: False
 
 dataset:
-  # root: /home/<user_name>/.cache/huggingface/lerobot (default)
-  root: "/home/uf/Data/lerobot_datas/record/ufactory/xarm7_pushT"
+  # Dataset path relative to the directory where the command is started.
+  root: "datasets/xarm7_pushT"
   repo_id: "ufactory/xarm7_pushT"
   single_task: "Push the T-block into the specified zone then go to reset position."
   fps: 10

config/umi/dual_umi_record_config.yaml

@@ -56,8 +56,8 @@ teleop:
       robot_base_pose: [400, 0, 150, 90, 0, 0]
 
 dataset:
-  # root of local repo: /home/<user_name>/.cache/huggingface/lerobot (default)
-  root: "/home/uf/Data/lerobot_datas/record/ufactory/dual_umi_datas"
+  # Dataset path relative to the directory where the command is started.
+  root: "datasets/dual_umi_datas"
   repo_id: "ufactory/dual_umi_datas"
   single_task: "Pick up the purple grape and drop into the box on the left."
   fps: 60
@@ -65,5 +65,3 @@ dataset:
   reset_time_s: 20    # time for resetting env between episodes
   num_episodes: 100
   push_to_hub: False
-
-

config/umi/dual_xarm6_umi_record_config.yaml

@@ -12,6 +12,7 @@ robot:
       control_space: "cartesian"
       robot_ip: "192.168.1.29"
       gripper_type: 2
+      enable_logs: false
       max_linear_velocity: 200
       cameras:
         fisheye:
@@ -28,6 +29,7 @@ robot:
       control_space: "cartesian"
       robot_ip: "192.168.1.83"
       gripper_type: 2
+      enable_logs: false
       max_linear_velocity: 200
       cameras:
         fisheye:
@@ -61,8 +63,8 @@ teleop:
       robot_base_pose: [400, 0, 150, 90, 0, 0]
 
 dataset:
-  # root of local repo: /home/<user_name>/.cache/huggingface/lerobot (default)
-  root: "/home/uf/Data/lerobot_datas/record/ufactory/dual_xarm6_umi_datas"
+  # Dataset path relative to the directory where the command is started.
+  root: "datasets/dual_xarm6_umi_datas"
   repo_id: "ufactory/dual_xarm6_umi_datas"
   single_task: "Pick up the purple grape and drop into the box on the left."
   fps: 60
@@ -70,4 +72,3 @@ dataset:
   reset_time_s: 20    # time for resetting env between episodes
   num_episodes: 100
   push_to_hub: False
-

config/umi/umi_record_config.yaml

@@ -24,8 +24,8 @@ teleop:
   robot_base_pose: [400, 0, 400, 180, 0, 0]
 
 dataset:
-  # root of local repo: /home/<user_name>/.cache/huggingface/lerobot (default)
-  root: "/home/uf/Data/lerobot_datas/record/ufactory/umi_datas"
+  # Dataset path relative to the directory where the command is started.
+  root: "datasets/umi_datas"
   repo_id: "ufactory/umi_datas"
   single_task: "Pick up the purple grape and drop into the box on the left."
   fps: 60

config/umi/xarm6_umi_record_config.yaml

@@ -5,6 +5,7 @@ robot:
   control_space: "cartesian"
   robot_ip: "192.168.1.83"
   gripper_type: 2
+  enable_logs: false
   max_linear_velocity: 250
   cameras:
     fisheye:
@@ -25,8 +26,8 @@ teleop:
   robot_base_pose: [400, 0, 400, 180, 0, 0]
 
 dataset:
-  # root of local repo: /home/<user_name>/.cache/huggingface/lerobot (default)
-  root: "/home/uf/Data/lerobot_datas/record/ufactory/xarm6_umi_datas"
+  # Dataset path relative to the directory where the command is started.
+  root: "datasets/xarm6_umi_datas"
   repo_id: "ufactory/xarm6_umi_datas"
   single_task: "Pick up the purple grape and drop into the box on the left."
   fps: 60

docs/gello_guard_latency_experiment_20260817.md

@@ -0,0 +1,219 @@
+# GELLO 安全高度 Guard 延迟实验记录
+
+## 1. 实验目的
+
+GELLO 以约 60 Hz 遥操作 xArm7 时，未启用安全高度保护的运动比较平滑；加入 TCP 最低高度保护后，机械臂出现卡顿、抖动和短暂停顿。
+
+本实验验证以下假设：
+
+> 应用侧 guard 在控制循环中同步调用 xArm 控制器的 FK、IK 或 joint-limit 接口，导致 ServoJ 命令到达控制器的时间不均匀。
+
+实验重点不是只看整个循环是否超过 16.667 ms，还要分别测量：
+
+- GELLO 读取耗时
+- 安全检查耗时
+- ServoJ SDK 调用耗时
+- 完整 `send_action` 耗时
+- 相邻 ServoJ 命令的近似下发间隔
+
+## 2. 实验环境
+
+- 日期：2026-08-17
+- 时区：Asia/Shanghai
+- 机械臂：xArm7
+- 控制频率：60 Hz
+- 目标周期：16.667 ms
+- 控制接口：`set_servo_angle_j`，即 `joint_command_mode: 1`
+- 安全高度：`min_tcp_z_mm: -2.0`
+- Guard 激活余量：`tcp_z_guard_activation_margin_mm: 100.0`
+- Guard 同步检查激活范围：实际 TCP z 不高于约 98 mm
+
+相关版本：
+
+- 原始 guard 基准提交：`14c3e798f01bee60d8a888cccc6ab5ad47437378`
+- 无 guard baseline 实验提交：`4231f1b`
+- Guard 延迟记录提交：`c0e950f`
+- Guard 延迟实验分支：`codex/gello-guard-latency`
+
+## 3. 实验方法
+
+### 3.1 实验 1：无 Guard Baseline
+
+活动控制循环只执行：
+
+```text
+GELLO get_action
+    -> robot.send_action
+    -> ServoJ
+    -> 等待下一周期
+```
+
+不执行每帧 observation、processor、FK、安全高度检查或新增 joint-limit 检查。
+
+运行命令：
+
+```bash
+uv run uf-robot-teleop \
+  --config_path config/gello/xarm7_gello_record_config.yaml \
+  --robot.enable_logs=true \
+  --fps 60 \
+  --experiment_1_baseline=true \
+  --experiment_duration_s 60
+```
+
+采用的有效数据文件：
+
+```text
+logs/gello_experiment_1_baseline_20260817_094957.csv
+```
+
+样本数为 3571，持续约 60 秒。
+
+### 3.2 实验 2：启用应用侧 Guard
+
+外层控制流程与 baseline 保持一致，但 `robot.send_action` 内启用 `min_tcp_z_mm` guard。记录代码进一步拆分了 safety guard 和 ServoJ 的耗时。
+
+运行命令：
+
+```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
+```
+
+数据文件：
+
+```text
+logs/gello_guard_latency_20260817_112752.csv
+```
+
+样本数为 3570，持续约 60 秒。
+
+每帧通过 `guard_path` 记录实际执行路径：
+
+- `rt_fast_path`：TCP 远离高度下限，只读取异步 RT-report 状态，不执行同步 FK。
+- `fk_safe`：TCP 进入 guard 激活范围，同步执行 FK 和 joint-limit 检查，目标仍安全。
+- `fk_ik_clamp`：目标低于安全高度，执行 FK、IK、joint-limit 和验证 FK。
+- `fallback`：安全检查失败，保持上一安全目标。
+
+## 4. 实验结果
+
+### 4.1 整体结果
+
+表中数值依次为 p50 / p95 / p99 / 最大值，单位均为 ms。
+
+| 指标 | 无 Guard Baseline | Guard 整体 |
+| --- | ---: | ---: |
+| 控制周期 | 16.744 / 16.929 / 17.465 / 20.594 | 16.741 / 17.013 / 17.937 / 20.165 |
+| GELLO 读取 | 0.052 / 0.090 / 0.125 / 0.362 | 0.053 / 0.102 / 0.131 / 0.639 |
+| Safety guard | 不适用 | 0.014 / 0.497 / 2.062 / 12.011 |
+| ServoJ | 未单独记录 | 0.333 / 1.153 / 2.254 / 4.726 |
+| `send_action` | 0.436 / 1.205 / 2.274 / 6.038 | 0.377 / 1.519 / 3.327 / 13.149 |
+| 循环工作耗时 | 0.497 / 1.243 / 2.337 / 6.124 | 0.440 / 1.581 / 3.370 / 13.180 |
+
+两次实验的循环工作耗时都没有超过 16.667 ms：
+
+- Baseline：0 / 3571 帧超期
+- Guard：0 / 3570 帧超期
+
+因此，只检查“循环工作是否超过 deadline”会得到不完整的结论。
+
+### 4.2 按 Guard 路径分组
+
+| 路径 | 帧数 | 占比 | Guard p50 / p95 / p99 / 最大 | `send_action` p50 / p95 / p99 / 最大 |
+| --- | ---: | ---: | ---: | ---: |
+| `rt_fast_path` | 3324 | 93.11% | 0.014 / 0.021 / 0.031 / 0.325 | 0.363 / 1.164 / 2.255 / 4.750 |
+| `fk_safe` | 246 | 6.89% | 0.689 / 3.043 / 5.283 / 12.011 | 1.002 / 4.202 / 6.715 / 13.149 |
+
+`rt_fast_path` 与无 guard baseline 基本一致。`fk_safe` 的 `send_action` 延迟明显增大：
+
+- p95 从 1.205 ms 增加到 4.202 ms，约为 baseline 的 3.49 倍。
+- p99 从 2.274 ms 增加到 6.715 ms，约为 baseline 的 2.95 倍。
+- 最大值从 6.038 ms 增加到 13.149 ms。
+
+本次实验没有出现 `fk_ik_clamp` 或 `fallback`。因此实验期间 guard 没有使用 IK 改写目标，也没有拒绝目标；观测到的额外延迟可以单独归因于同步 FK 和 joint-limit 检查。
+
+### 4.3 ServoJ 近似下发间隔
+
+循环起始周期稳定并不代表 ServoJ 实际到达控制器的时间稳定。同步 FK 位于每帧 ServoJ 之前，因此 FK 耗时变化会改变 ServoJ 在该帧中的下发相位。
+
+本实验使用以下公式推导 ServoJ 调用开始时间：
+
+```text
+近似 ServoJ 下发时刻
+  = elapsed_s * 1000
+  + gello_read_ms
+  + safety_guard_ms
+```
+
+该值没有包含 guard 返回后到 SDK 调用前的少量 Python 开销，所以是近似值；这些开销远小于观测到的 FK 长尾，不影响结论。
+
+| 路径 | p1 | p5 | p50 | p95 | p99 | 最小 | 最大 |
+| --- | ---: | ---: | ---: | ---: | ---: | ---: | ---: |
+| Baseline | 16.685 | 16.712 | 16.749 | 16.929 | 17.461 | 16.545 | 20.594 |
+| `rt_fast_path` | 16.677 | 16.706 | 16.742 | 16.996 | 17.834 | 16.254 | 20.171 |
+| `fk_safe` | 12.751 | 14.916 | 16.760 | 19.093 | 20.825 | 6.756 | 28.817 |
+
+`fk_safe` 中出现了典型的长短周期交替：
+
+```text
+28.817 ms -> 6.756 ms
+25.510 ms -> 7.243 ms
+20.877 ms -> 12.723 ms
+20.756 ms -> 12.787 ms
+```
+
+其表现是控制器先较长时间收不到新目标，随后在很短间隔内收到下一目标，符合实际体感中的“停一下，然后突然追赶”。
+
+### 4.4 Guard 激活时间段
+
+本次实验中同步 FK 路径集中在以下时间段：
+
+| 路径 | 帧范围 | 时间范围 | 帧数 |
+| --- | ---: | ---: | ---: |
+| `rt_fast_path` | 0-2352 | 0.000-39.516 s | 2353 |
+| `fk_safe` | 2353-2398 | 39.533-40.288 s | 46 |
+| `rt_fast_path` | 2399-3369 | 40.304-56.610 s | 971 |
+| `fk_safe` | 3370-3569 | 56.627-59.984 s | 200 |
+
+若机械臂的卡顿体感集中在约 39.5-40.3 秒和 56.6-60.0 秒，则与同步 FK 路径在时间上直接吻合。
+
+### 4.5 其他错误排查
+
+`logs/xarm7_gripper_errors.log` 中本次实验之前最近的 controller error 时间为 11:20。本次 guard 实验约在 11:26-11:27 运行，期间没有新增 gripper/controller error，因此这些错误不是本次延迟长尾的原因。
+
+## 5. 结论
+
+实验结果支持最初假设：
+
+1. GELLO 读取非常快，不是卡顿来源。
+2. ServoJ SDK 调用存在少量长尾，但 baseline 和 `rt_fast_path` 表现相近，不是加入 guard 后才出现的主要变化。
+3. TCP 远离安全高度时，RT-report 快速路径几乎没有额外成本。
+4. TCP 接近安全高度后，同步 FK 和 joint-limit 检查产生 3-12 ms 的不稳定延迟。
+5. 即使整个循环工作耗时仍小于 16.667 ms，同步检查也会改变 ServoJ 在帧内的下发相位，造成约 6.8-28.8 ms 的不均匀命令间隔。
+6. 本轮没有 IK clamp 或 fallback，因此不需要用“关节目标被修改”来解释抖动；仅同步控制器通信已经足以解释现象。
+
+综合判断：应用侧同步安全检查是加入 guard 后抖动的主要原因。
+
+## 6. 后续方案
+
+建议按以下优先级处理：
+
+1. 优先使用 xArm 控制器侧 TCP 安全边界，让控制器在内部阻止越界，不在 60 Hz Python 发送线程中同步查询 FK/IK。
+2. 如果必须在应用侧检查，使用本地运动学库计算 FK/IK，避免每帧和控制器进行同步请求。
+3. 如果本地计算不可用，将安全计算与 ServoJ 发送线程解耦，并采用保守的最后安全目标策略；需要另外验证异步结果的时效性和安全性。
+4. 保留 controller error、guard path 和各阶段延迟记录，后续方案必须用相同实验复测。
+
+不建议为了平滑性直接移除实际运行中的安全保护。无 guard 模式仅用于受控环境下建立 baseline。
+
+## 7. 下一轮验收指标
+
+控制器侧安全边界方案应至少满足：
+
+- `send_action` p95/p99 接近 baseline。
+- ServoJ 下发间隔不再因接近安全高度出现明显长短周期交替。
+- 靠近安全面和触发安全边界时均不执行 Python 侧同步 FK/IK。
+- 保持 TCP 最低高度保护有效。
+- 无新增 controller、gripper 或通信错误。

docs/gello_xarm7_smooth_safe_recording_zh.md

@@ -0,0 +1,176 @@
+# xArm7 + GELLO：消除遥操作抖动并可靠录制数据
+
+这篇文档记录一次真实排障。目标看起来很简单：GELLO 控制 xArm7 时保留第七关节，TCP 又不能撞桌，同时录下同步的图像、关节状态和动作。实际遇到了三个互相关联的问题：机械臂抖动、录制数据时间对不上、夹爪触发控制器 Error 19。
+
+下面不堆术语，先讲最终答案，再解释为什么。
+
+## 最终方案
+
+- GELLO 始终发送七个关节角，机械臂使用 `set_servo_angle_j`，控制频率 60 Hz。这样 J7 不会因为改发 EEF 位姿而丢失。
+- 60 Hz 控制放在独立实时线程里。相机读取、图片编码和数据写盘再慢，也不能拖慢关节命令。
+- TCP 高度保护使用 CPU 本地运动学计算，不在控制循环里向 xArm 控制器同步请求 FK/IK。
+- 控制器 Safety Boundary 仍然开启，作为本地保护之外的最后一道硬保护。
+- 相机和数据集按真实的 30 Hz 记录，机械臂控制仍保持独立的 60 Hz。
+- 每条 action 带发送时刻；每次 RT joint state 采样也带时刻。写入数据集时，为 state 选择当时已经发送的最近 action，绝不拿“未来的 action”配较早的 state。
+- xArm Gripper 保持 60 Hz 的目标检查能力，但只有变化超过阈值才真正发送；夹爪速度从最大值 5000 降到 1500，解决 Error 19。
+
+当前使用的配置是：
+
+```yaml
+robot:
+  control_space: "joint"
+  joint_command_mode: 1
+
+  min_tcp_z_mm: 70.0
+  tcp_z_guard_backend: "local_projection"
+  tcp_z_soft_margin_mm: 5.0
+  controller_safety_boundary: true
+
+  gripper_command_interval_s: 0.0166667
+  gripper_command_threshold: 0.01
+  gripper_speed: 1500
+
+teleop:
+  realtime_control_fps: 60
+
+dataset:
+  fps: 30
+```
+
+完整配置见 `config/gello/xarm7_gello_record_config.yaml`。
+
+## 一、遥操作为什么会抖
+
+最初为了保护 TCP 高度，每一轮控制都做了下面这些事：
+
+```text
+读取 GELLO
+  -> 请求控制器做 FK / IK / joint-limit 检查
+  -> 发送 ServoJ
+```
+
+问题不在 FK 数学本身，而在“每一帧都要等控制器回复”。一次请求可能很快，下一次却慢几毫秒。60 Hz 控制周期只有 16.67 ms，这种不稳定延迟会让 ServoJ 命令变成：等得久一下，紧接着又很快补一条。机械臂体感就是停一下、追一下，也就是抖动。
+
+延迟实验已经观测到接近安全高度时，ServoJ 间隔会在约 6.8–28.8 ms 之间长短交替。即使平均频率看起来仍是 60 Hz，命令到达时间不均匀也足以造成抖动。详细原始实验见 `docs/gello_guard_latency_experiment_20260817.md`。
+
+录制时还有第二个抖动来源：原来的单线程循环要依次读取机器人、读取两路相机、处理图像、写数据，然后才发送下一条控制命令。相机一次等待约 33 ms，直接把 ServoJ 卡住。实验模式不读取相机所以很平滑，正式录制却抖，差别就在这里。
+
+## 二、怎么消除抖动
+
+### 1. 控制和录制彻底分开
+
+新增固定频率控制器 `RealtimeTeleopController`。它只负责：
+
+```text
+每 16.67 ms：读取 GELLO -> 本地安全检查 -> 发送 ServoJ
+```
+
+主录制线程负责：
+
+```text
+读取 RT state -> 读取相机 -> 组装数据 -> 写入 dataset
+```
+
+两者互不等待。相机偶尔慢一帧，只会影响这一帧什么时候写完，不会改变 ServoJ 的节拍。主线程如果卡死超过 1 秒，控制线程会自动停止，避免程序异常后继续下发命令。
+
+普通 `uf-robot-teleop` 也使用同一个实时控制器，所以录制结束后单独遥操作不会重新掉回容易抖动的旧循环。
+
+### 2. 安全检查留在本机 CPU
+
+xArm7 的本地模型从控制器读取一次标定参数，启动时与控制器 FK 做交叉验证。验证通过后，每个控制周期的 TCP 高度和雅可比投影都在 CPU 计算，不再产生控制器网络往返。
+
+这里选择 CPU 而不是 GPU，是因为一次 7 自由度 FK/Jacobian 计算非常小。GPU 的数据搬运和调度开销反而更大；5090 对这种单样本、低维计算没有速度优势。
+
+正常目标原样发送。目标准备穿过软高度面时，只投影会继续降低 TCP 的关节运动分量，而不是改发六维 EEF 命令，因此七个关节自由度仍然保留。控制器侧的 Safety Boundary 放在硬下限处兜底。
+
+## 三、怎么确认录制的数据同步
+
+把控制拆到另一个线程以后，不能简单地在相机读完后拿“最新 action”。那条 action 可能是在 state 采样之后才发送的，相当于用未来动作解释过去状态。
+
+现在的配对规则是：
+
+1. 从 xArm RT report 复制关节 state 时，立即记录单调时钟 `state_sample_s`。
+2. 每次 ServoJ 实际发送完成后，记录 action 和 `action_sent_s`。
+3. 写 dataset 时，查找 `action_sent_s <= state_sample_s` 的最近一条 action。
+4. 图像、这个 state 和查到的 action 一起组成数据帧。
+
+每次录制还会生成：
+
+```text
+logs/gello_record_sync_<时间>.csv
+```
+
+重点看这些列：
+
+- `action_age_ms`：state 采样时，这条 action 已经发送多久。它应该非负，通常小于一个 60 Hz 周期。
+- `state_to_observation_end_ms`：state 采样后，两路相机读取和整理用了多久。
+- `camera_timings`：每路相机调用的起止时间。
+- `frame_loop_ms`：这一帧写入前的主循环工作时间。
+
+实际日志验证过，正常帧的 `action_age_ms` 通常约 1–10 ms，没有选择未来 action。
+
+相机硬件配置是 30 Hz，所以 dataset 也必须写成 30 Hz。之前 dataset 标成 60 Hz、实际只能得到约 30 帧，会让数据时间轴看起来快一倍。现在控制频率和录制频率已经分开：
+
+```text
+机械臂控制：60 Hz
+图像/state/action 采样：30 Hz
+```
+
+30 Hz 数据集每帧记录的是该采样时刻有效的 60 Hz 控制命令，这是正常的降采样，不是不同步。
+
+## 四、夹爪 Error 19 是怎么消除的
+
+现象是只要连续控制夹爪，就出现：
+
+```text
+set_rs485_data -> code=1
+controller_error=19
+```
+
+排查过程里先试过降低夹爪命令频率：2 Hz、5 Hz、6.7 Hz 都不报错，但低频带来明显跟手延迟。继续对照实验后发现，真正与故障一致的变量不是频率，而是夹爪速度：
+
+- 报错时使用默认最大速度 5000。
+- 速度降到 1500 后，从 2 Hz 一直提高到 20 Hz 都稳定。
+- 最后恢复到最高 60 Hz，仍然稳定。
+
+运行期发送也改成 SDK 专用的非阻塞 `set_gripper_position`，不再手工拼通用 RS485 数据包；持续 ServoJ 时关闭 `wait_motion`，否则 SDK 会等待机械臂停止。
+
+最终参数为：
+
+```yaml
+gripper_speed: 1500
+gripper_command_interval_s: 0.0166667
+gripper_command_threshold: 0.01
+```
+
+这里的 60 Hz 是“最多检查和发送 60 次”。当夹爪目标变化不足 `0.01` 时会去重，不会发送没有意义的重复 RS485 命令。
+
+每次成功或失败的夹爪命令都会写入：
+
+```text
+logs/xarm7_gripper_errors.log
+```
+
+成功记录包含目标值、脉冲位置、调用耗时和返回码。最终实验中单次调用通常只需要约 1.3–2.2 ms，60 Hz、速度 1500 时没有再次出现 C19。
+
+如果以后更换夹爪、线缆或固件后 C19 再次出现，应先把速度降下来验证。如果低速也报错，就应该检查腕部线缆、接头、末端供电和末端 IO 板固件，而不是无限降低控制频率。
+
+## 五、运行和验收
+
+正式录制：
+
+```bash
+uv run record \
+  --config_path config/gello/xarm7_gello_record_config.yaml
+```
+
+每次修改控制或相机配置后，至少检查：
+
+1. 遥操作全过程没有肉眼可见的停顿或追赶。
+2. J7 可以独立旋转。
+3. 接近 TCP 高度下限时本地投影生效，控制器 Safety Boundary 保持开启。
+4. 同步日志中 `action_age_ms` 不为负，绝大多数小于 16.67 ms。
+5. 实际 dataset 是约 30 FPS，元数据也为 30 FPS。
+6. 连续开合夹爪时没有新增 Error 19。
+
+不要提交运行生成的 CSV 或设备错误日志；它们用于本机诊断，不属于训练数据和源代码。

pyproject.toml

@@ -35,18 +35,21 @@ dependencies = [
 [project.scripts]
 uf-robot-teleop = "lerobot_robot_ufactory.scripts.uf_robot_teleop:main"
 uf-lerobot-record = "lerobot_robot_ufactory.scripts.uf_lerobot_record:main"
+record = "lerobot_robot_ufactory.scripts.uf_lerobot_record:main"
 uf-lerobot-eval = "lerobot_robot_ufactory.scripts.uf_lerobot_eval:main"
 uf-lerobot-replay = "lerobot_robot_ufactory.scripts.uf_lerobot_replay:main"
+replay = "lerobot_robot_ufactory.scripts.uf_lerobot_replay:main"
 uf-vive-calibrate = "lerobot_robot_ufactory.scripts.vive_calibrate:main"
 uf-camera-view = "lerobot_robot_ufactory.scripts.uf_camera_view:main"
 uf-realsense-view = "lerobot_robot_ufactory.scripts.uf_realsense_view:main"
 uf-camera-test = "lerobot_robot_ufactory.scripts.uf_camera_test:main"
+uf-read-tcp-z = "lerobot_robot_ufactory.scripts.uf_read_tcp_z:main"
 
 [project.optional-dependencies]
 # GELLO 遥操作
 gello = [
     "gello @ git+https://github.com/xArm-Developer/gello_software.git",
-    "dynamixel-sdk @ git+https://github.com/ROBOTIS-GIT/DynamixelSDK.git#subdirectory=python",
+    "dynamixel-sdk>=4.0.5",
 ]
 # SpaceMouse 遥操作
 spacemouse = [

rules/99-ftdi-serial.rules

@@ -0,0 +1,2 @@
+# FTDI FT232H (GELLO 示教臂串口) — 自动授予读写权限
+KERNEL=="ttyUSB*", ATTRS{idVendor}=="0403", ATTRS{idProduct}=="6014", MODE:="0666", SYMLINK+="ttyFTDI"

scripts/compare_act.py

@@ -0,0 +1,433 @@
+#!/usr/bin/env python3
+"""
+Compare ACT model inference at two checkpoints against ground truth.
+
+Usage:
+    cd /home/lizhuoyuan/project/lerobot_xarm7
+    python scripts/compare_act.py --num-samples 8 --episode 0
+"""
+
+import argparse
+import csv
+import os
+from pathlib import Path
+
+import matplotlib
+matplotlib.use("Agg")
+import matplotlib.pyplot as plt
+import numpy as np
+import torch
+from matplotlib.gridspec import GridSpec
+
+# ── lerobot imports ──────────────────────────────────────────────
+from lerobot.policies.act.modeling_act import ACTPolicy
+from lerobot.processor import PolicyProcessorPipeline
+from lerobot.processor.converters import (
+    batch_to_transition,
+    transition_to_batch,
+    policy_action_to_transition,
+    transition_to_policy_action,
+)
+from lerobot.datasets.lerobot_dataset import LeRobotDataset
+
+JOINT_NAMES = ["J1", "J2", "J3", "J4", "J5", "J6", "J7", "Gripper"]
+
+
+def parse_args():
+    p = argparse.ArgumentParser(description="Compare ACT 40K vs 60K inference")
+    p.add_argument("--dataset-root", default="./datasets/xarm7-pick-bottle",
+                   help="Path to dataset root")
+    p.add_argument("--checkpoint-40k",
+                   default="./outputs/train/2026-08-08/09-57-03_act/checkpoints/040000/pretrained_model",
+                   help="Path to 40K checkpoint pretrained_model dir")
+    p.add_argument("--checkpoint-60k",
+                   default="./outputs/train/2026-08-08/09-57-03_act/checkpoints/060000/pretrained_model",
+                   help="Path to 60K checkpoint pretrained_model dir")
+    p.add_argument("--episodes", type=str, default="0",
+                   help="Comma-separated episode indices, e.g. '0,10,20,30,40,50'")
+    p.add_argument("--num-samples", type=int, default=8, help="Number of frames to sample per episode")
+    p.add_argument("--output-dir", default="./outputs/compare_act",
+                   help="Output directory for results")
+    return p.parse_args()
+
+
+def load_policy_and_processors(checkpoint_path: str):
+    """Load ACT policy, preprocessor, and postprocessor from a checkpoint."""
+    print(f"  Loading policy from {checkpoint_path} ...")
+    policy = ACTPolicy.from_pretrained(checkpoint_path)
+    policy.to("cpu")
+    policy.reset()
+
+    preprocessor = PolicyProcessorPipeline.from_pretrained(
+        pretrained_model_name_or_path=checkpoint_path,
+        config_filename="policy_preprocessor.json",
+        overrides={"device_processor": {"device": "cpu"}},
+        to_transition=batch_to_transition,
+        to_output=transition_to_batch,
+    )
+    postprocessor = PolicyProcessorPipeline.from_pretrained(
+        pretrained_model_name_or_path=checkpoint_path,
+        config_filename="policy_postprocessor.json",
+        overrides={"device_processor": {"device": "cpu"}},
+        to_transition=policy_action_to_transition,
+        to_output=transition_to_policy_action,
+    )
+    return policy, preprocessor, postprocessor
+
+
+def get_episode_frame_range(dataset: LeRobotDataset, episode_idx: int):
+    """Get (from_idx, to_idx) for a specific episode.
+
+    dataset.meta.episodes is a HuggingFace Dataset with columns:
+    episode_index, dataset_from_index, dataset_to_index, length, ...
+    """
+    eps = dataset.meta.episodes
+    for i in range(len(eps)):
+        if int(eps[i]["episode_index"]) == episode_idx:
+            from_idx = int(eps[i]["dataset_from_index"])
+            to_idx = int(eps[i]["dataset_to_index"])
+            return from_idx, to_idx
+    raise ValueError(f"Episode {episode_idx} not found in dataset")
+
+
+def run_inference(policy, preprocessor, postprocessor, obs_dict: dict) -> np.ndarray:
+    """Run inference on a single observation dict, return predicted action as (8,) numpy."""
+    batch = preprocessor(obs_dict)
+    with torch.inference_mode():
+        action_chunk = policy.predict_action_chunk(batch)  # (1, chunk_size, 8)
+    action = action_chunk[:, 0, :]  # (1, 8)
+    action = postprocessor(action)
+    return action.cpu().numpy().squeeze(0)  # (8,)
+
+
+def run_one_episode(args, episode_idx, dataset, policy_40k, preproc_40k, postproc_40k,
+                     policy_60k, preproc_60k, postproc_60k):
+    """Run inference on one episode. Returns (results, images, mae_40k, mae_60k, l2_40k, l2_60k)."""
+    from_idx, to_idx = get_episode_frame_range(dataset, episode_idx)
+    total_frames = to_idx - from_idx
+    print(f"  Episode {episode_idx}: frames [{from_idx}, {to_idx}), total={total_frames}")
+
+    sample_indices = np.linspace(from_idx, to_idx - 1, args.num_samples, dtype=int)
+    print(f"  Sampling {args.num_samples} frames at indices: {list(sample_indices)}")
+
+    results = []
+    images = []
+
+    for i, global_idx in enumerate(sample_indices):
+        print(f"    Frame {i+1}/{args.num_samples} (global idx={global_idx}) ...")
+        frame = dataset[global_idx]
+
+        gt_action = frame["action"].numpy().squeeze()
+
+        img = frame["observation.images.camera"].numpy()
+        img = np.transpose(img, (1, 2, 0))
+        img_uint8 = (img * 255).clip(0, 255).astype(np.uint8)
+        images.append(img_uint8)
+
+        obs_dict = {
+            "observation.state": frame["observation.state"],
+            "observation.images.camera": frame["observation.images.camera"],
+        }
+
+        policy_40k.reset()
+        policy_60k.reset()
+
+        pred_40k = run_inference(policy_40k, preproc_40k, postproc_40k, obs_dict)
+        pred_60k = run_inference(policy_60k, preproc_60k, postproc_60k, obs_dict)
+
+        results.append({
+            "frame": i,
+            "global_idx": int(global_idx),
+            "gt": gt_action,
+            "pred_40k": pred_40k,
+            "pred_60k": pred_60k,
+        })
+
+    # Compute metrics
+    n_joints = 8
+    gt_all = np.stack([r["gt"] for r in results])
+    pred_40k_all = np.stack([r["pred_40k"] for r in results])
+    pred_60k_all = np.stack([r["pred_60k"] for r in results])
+
+    err_40k = np.abs(pred_40k_all - gt_all)
+    err_60k = np.abs(pred_60k_all - gt_all)
+    mae_40k = err_40k.mean(axis=0)
+    mae_60k = err_60k.mean(axis=0)
+    l2_40k = np.sqrt(((pred_40k_all - gt_all) ** 2).sum(axis=1))
+    l2_60k = np.sqrt(((pred_60k_all - gt_all) ** 2).sum(axis=1))
+
+    return results, images, mae_40k, mae_60k, l2_40k.mean(), l2_60k.mean()
+
+
+def save_episode_plots(args, episode_idx, results, images, mae_40k, mae_60k,
+                        gt_all, pred_40k_all, pred_60k_all, out_dir):
+    """Generate per-episode plots and CSV."""
+    n_joints = 8
+
+    # CSV
+    csv_path = os.path.join(out_dir, "comparison.csv")
+    err_40k = np.abs(pred_40k_all - gt_all)
+    err_60k = np.abs(pred_60k_all - gt_all)
+    l2_40k = np.sqrt(((pred_40k_all - gt_all) ** 2).sum(axis=1))
+    l2_60k = np.sqrt(((pred_60k_all - gt_all) ** 2).sum(axis=1))
+
+    with open(csv_path, "w", newline="") as f:
+        writer = csv.writer(f)
+        header = ["frame", "global_idx"]
+        for jn in JOINT_NAMES:
+            header += [f"GT_{jn}", f"40K_{jn}", f"60K_{jn}", f"err40K_{jn}", f"err60K_{jn}"]
+        header += ["L2_40K", "L2_60K"]
+        writer.writerow(header)
+        for i, r in enumerate(results):
+            row = [r["frame"], r["global_idx"]]
+            for j in range(n_joints):
+                row += [f"{r['gt'][j]:.6f}", f"{r['pred_40k'][j]:.6f}",
+                        f"{r['pred_60k'][j]:.6f}",
+                        f"{err_40k[i][j]:.6f}", f"{err_60k[i][j]:.6f}"]
+            row += [f"{l2_40k[i]:.6f}", f"{l2_60k[i]:.6f}"]
+            writer.writerow(row)
+
+    # 图1: Trajectory curves
+    fig1, axes = plt.subplots(2, 4, figsize=(18, 9))
+    axes = axes.flatten()
+    colors = {"GT": "black", "40K": "#2196F3", "60K": "#FF9800"}
+    x = np.arange(args.num_samples)
+    for j in range(n_joints):
+        ax = axes[j]
+        ax.plot(x, gt_all[:, j], "o-", color=colors["GT"], label="GT", linewidth=2, markersize=5)
+        ax.plot(x, pred_40k_all[:, j], "s--", color=colors["40K"], label="40K", linewidth=1.5, markersize=5)
+        ax.plot(x, pred_60k_all[:, j], "d-.", color=colors["60K"], label="60K", linewidth=1.5, markersize=5)
+        ax.set_title(JOINT_NAMES[j], fontsize=12, fontweight="bold")
+        ax.set_xlabel("Frame index")
+        ax.set_ylabel("Joint value (rad)")
+        ax.legend(fontsize=8)
+        ax.grid(True, alpha=0.3)
+    fig1.suptitle(f"ACT Inference — Episode {episode_idx} ({args.num_samples} frames)",
+                  fontsize=14, fontweight="bold")
+    fig1.tight_layout()
+    fig1.savefig(os.path.join(out_dir, "trajectory_curves.png"), dpi=150)
+    plt.close(fig1)
+
+    # 图2: MAE bar chart
+    fig2, ax = plt.subplots(figsize=(10, 5))
+    x_pos = np.arange(n_joints)
+    width = 0.35
+    bars1 = ax.bar(x_pos - width/2, mae_40k, width, label="40K", color="#2196F3", edgecolor="white")
+    bars2 = ax.bar(x_pos + width/2, mae_60k, width, label="60K", color="#FF9800", edgecolor="white")
+    ax.set_xticks(x_pos)
+    ax.set_xticklabels(JOINT_NAMES)
+    ax.set_ylabel("MAE (rad)")
+    ax.set_title(f"Per-Joint MAE: 40K vs 60K — Episode {episode_idx}")
+    ax.legend()
+    ax.grid(axis="y", alpha=0.3)
+    for bar in bars1:
+        h = bar.get_height()
+        ax.text(bar.get_x() + bar.get_width()/2., h + 0.001, f"{h:.4f}",
+                ha="center", va="bottom", fontsize=7)
+    for bar in bars2:
+        h = bar.get_height()
+        ax.text(bar.get_x() + bar.get_width()/2., h + 0.001, f"{h:.4f}",
+                ha="center", va="bottom", fontsize=7)
+    fig2.tight_layout()
+    fig2.savefig(os.path.join(out_dir, "error_comparison.png"), dpi=150)
+    plt.close(fig2)
+
+    # 图3: Image collage with action tables
+    n_cols = min(4, args.num_samples)
+    n_rows = (args.num_samples + n_cols - 1) // n_cols
+    fig3 = plt.figure(figsize=(4 * n_cols, 4.5 * n_rows))
+    gs = GridSpec(n_rows * 2, n_cols, figure=fig3, height_ratios=[3, 1] * n_rows)
+
+    for i in range(args.num_samples):
+        row = (i // n_cols) * 2
+        col = i % n_cols
+
+        ax_img = fig3.add_subplot(gs[row, col])
+        ax_img.imshow(images[i])
+        ax_img.set_title(f"Frame {i}", fontsize=10)
+        ax_img.axis("off")
+
+        ax_tbl = fig3.add_subplot(gs[row + 1, col])
+        ax_tbl.axis("off")
+
+        table_data = [["Joint", "GT", "40K", "60K"]]
+        for j in range(n_joints):
+            gt_val = results[i]["gt"][j]
+            p40_val = results[i]["pred_40k"][j]
+            p60_val = results[i]["pred_60k"][j]
+
+            def fmt(v, err, thresh=0.05):
+                s = f"{v:.3f}"
+                return f"!{s}" if err > thresh else s
+
+            table_data.append([
+                JOINT_NAMES[j],
+                fmt(gt_val, 0),
+                fmt(p40_val, abs(p40_val - gt_val)),
+                fmt(p60_val, abs(p60_val - gt_val)),
+            ])
+
+        tbl = ax_tbl.table(cellText=table_data, loc="center", cellLoc="center")
+        tbl.auto_set_font_size(False)
+        tbl.set_fontsize(7)
+        tbl.scale(1.0, 1.1)
+
+        for j in range(n_joints):
+            err40 = abs(results[i]["pred_40k"][j] - results[i]["gt"][j])
+            err60 = abs(results[i]["pred_60k"][j] - results[i]["gt"][j])
+            if err40 > 0.05:
+                tbl[(j + 1, 2)].set_facecolor("#FFCDD2")
+            if err60 > 0.05:
+                tbl[(j + 1, 3)].set_facecolor("#FFCDD2")
+
+    fig3.suptitle(f"Frame-by-Frame Comparison — Episode {episode_idx}",
+                  fontsize=14, fontweight="bold")
+    fig3.tight_layout()
+    fig3.savefig(os.path.join(out_dir, "frame_comparison.png"), dpi=150)
+    plt.close(fig3)
+
+
+def main():
+    args = parse_args()
+    os.makedirs(args.output_dir, exist_ok=True)
+
+    # Parse episodes
+    episode_list = [int(x.strip()) for x in args.episodes.split(",")]
+    print(f"Episodes to evaluate: {episode_list}")
+
+    # ── 1. Load dataset (full, no episode filter) ────────────────
+    print("\n" + "=" * 60)
+    print("Loading dataset (full) ...")
+    dataset = LeRobotDataset(
+        repo_id="xarm7-pick-bottle",
+        root=args.dataset_root,
+    )
+    print(f"  Total episodes: {len(dataset.meta.episodes)}")
+
+    # ── 2. Load models (once) ────────────────────────────────────
+    print("\nLoading models (shared across episodes) ...")
+    print("  Loading 40K model ...")
+    policy_40k, preproc_40k, postproc_40k = load_policy_and_processors(args.checkpoint_40k)
+    print("  Loading 60K model ...")
+    policy_60k, preproc_60k, postproc_60k = load_policy_and_processors(args.checkpoint_60k)
+
+    # ── 3. Evaluate each episode ─────────────────────────────────
+    n_joints = 8
+    all_mae_40k = []
+    all_mae_60k = []
+    all_l2_40k = []
+    all_l2_60k = []
+
+    for ep in episode_list:
+        print(f"\n{'─' * 50}")
+        print(f"Evaluating Episode {ep}")
+        print(f"{'─' * 50}")
+
+        ep_out_dir = os.path.join(args.output_dir, f"ep{ep}")
+        os.makedirs(ep_out_dir, exist_ok=True)
+
+        results, images, mae_40k, mae_60k, l2_40k, l2_60k = run_one_episode(
+            args, ep, dataset, policy_40k, preproc_40k, postproc_40k,
+            policy_60k, preproc_60k, postproc_60k)
+
+        all_mae_40k.append(mae_40k)
+        all_mae_60k.append(mae_60k)
+        all_l2_40k.append(l2_40k)
+        all_l2_60k.append(l2_60k)
+
+        # Print per-episode summary
+        print(f"\n  Episode {ep} Summary:")
+        print(f"  {'Joint':>10} | {'40K MAE':>10} | {'60K MAE':>10} | {'Δ':>10}")
+        print(f"  {'─' * 48}")
+        for j in range(n_joints):
+            diff = mae_40k[j] - mae_60k[j]
+            sign = "▼" if diff > 0 else "▲"
+            print(f"  {JOINT_NAMES[j]:>10} | {mae_40k[j]:10.4f} | {mae_60k[j]:10.4f} | {sign}{abs(diff):9.4f}")
+        print(f"  {'L2 mean':>10} | {l2_40k:10.4f} | {l2_60k:10.4f} |")
+
+        # Extract arrays for plotting
+        gt_all = np.stack([r["gt"] for r in results])
+        pred_40k_all = np.stack([r["pred_40k"] for r in results])
+        pred_60k_all = np.stack([r["pred_60k"] for r in results])
+
+        save_episode_plots(args, ep, results, images, mae_40k, mae_60k,
+                          gt_all, pred_40k_all, pred_60k_all, ep_out_dir)
+        print(f"  Plots saved to {ep_out_dir}")
+
+    # ── 4. Aggregate results across episodes ─────────────────────
+    mean_mae_40k = np.stack(all_mae_40k).mean(axis=0)
+    mean_mae_60k = np.stack(all_mae_60k).mean(axis=0)
+    mean_l2_40k = np.mean(all_l2_40k)
+    mean_l2_60k = np.mean(all_l2_60k)
+
+    print(f"\n{'=' * 60}")
+    print(f"=== AGGREGATE RESULTS ({len(episode_list)} episodes) ===")
+    print(f"{'Joint':>10} | {'40K MAE':>10} | {'60K MAE':>10} | {'Δ':>10}")
+    print("-" * 48)
+    for j in range(n_joints):
+        diff = mean_mae_40k[j] - mean_mae_60k[j]
+        sign = "▼" if diff > 0 else "▲"
+        pct = (diff / mean_mae_40k[j] * 100) if mean_mae_40k[j] > 0 else 0
+        print(f"{JOINT_NAMES[j]:>10} | {mean_mae_40k[j]:10.4f} | {mean_mae_60k[j]:10.4f} | {sign}{abs(diff):9.4f} ({pct:+.0f}%)")
+    print(f"{'L2 mean':>10} | {mean_l2_40k:10.4f} | {mean_l2_60k:10.4f} |")
+    l2_diff_pct = (mean_l2_40k - mean_l2_60k) / mean_l2_40k * 100
+    print(f"\n  Overall L2 improvement: {l2_diff_pct:.1f}%")
+
+    # Save aggregate CSV
+    agg_csv = os.path.join(args.output_dir, "aggregate_summary.csv")
+    with open(agg_csv, "w", newline="") as f:
+        writer = csv.writer(f)
+        writer.writerow(["Joint", "40K_MAE", "60K_MAE", "Diff", "Change%"])
+        for j in range(n_joints):
+            diff = mean_mae_40k[j] - mean_mae_60k[j]
+            pct = (diff / mean_mae_40k[j] * 100) if mean_mae_40k[j] > 0 else 0
+            writer.writerow([JOINT_NAMES[j], f"{mean_mae_40k[j]:.6f}", f"{mean_mae_60k[j]:.6f}",
+                           f"{diff:.6f}", f"{pct:.1f}%"])
+        writer.writerow(["L2_mean", f"{mean_l2_40k:.6f}", f"{mean_l2_60k:.6f}", "", f"{l2_diff_pct:.1f}%"])
+    print(f"\nAggregate CSV saved to {agg_csv}")
+
+    # Save per-episode summary CSV
+    eps_csv = os.path.join(args.output_dir, "per_episode_summary.csv")
+    with open(eps_csv, "w", newline="") as f:
+        writer = csv.writer(f)
+        writer.writerow(["Episode", "L2_40K", "L2_60K"])
+        for i, ep in enumerate(episode_list):
+            writer.writerow([ep, f"{all_l2_40k[i]:.6f}", f"{all_l2_60k[i]:.6f}"])
+    print(f"Per-episode summary saved to {eps_csv}")
+
+    # Aggregate bar chart
+    fig_agg, ax = plt.subplots(figsize=(12, 6))
+    x_pos = np.arange(n_joints + 1)
+    labels = JOINT_NAMES + ["L2"]
+    vals_40k = list(mean_mae_40k) + [mean_l2_40k]
+    vals_60k = list(mean_mae_60k) + [mean_l2_60k]
+    width = 0.35
+    bars1 = ax.bar(x_pos - width/2, vals_40k, width, label="40K", color="#2196F3", edgecolor="white")
+    bars2 = ax.bar(x_pos + width/2, vals_60k, width, label="60K", color="#FF9800", edgecolor="white")
+    ax.set_xticks(x_pos)
+    ax.set_xticklabels(labels)
+    ax.set_ylabel("MAE / L2 (rad)")
+    ax.set_title(f"Aggregate Error: 40K vs 60K (avg over {len(episode_list)} episodes)")
+    ax.legend()
+    ax.grid(axis="y", alpha=0.3)
+    for bar in bars1:
+        h = bar.get_height()
+        ax.text(bar.get_x() + bar.get_width()/2., h + 0.0005, f"{h:.4f}",
+                ha="center", va="bottom", fontsize=7)
+    for bar in bars2:
+        h = bar.get_height()
+        ax.text(bar.get_x() + bar.get_width()/2., h + 0.0005, f"{h:.4f}",
+                ha="center", va="bottom", fontsize=7)
+    fig_agg.tight_layout()
+    fig_agg.savefig(os.path.join(args.output_dir, "aggregate_error.png"), dpi=150)
+    plt.close(fig_agg)
+    print("Aggregate bar chart saved: aggregate_error.png")
+
+    print(f"\n{'=' * 60}")
+    print(f"All outputs saved to {args.output_dir}/")
+    print("Done!")
+    print("Done!")
+
+
+if __name__ == "__main__":
+    main()

src/lerobot_robot_ufactory/robots/uf_robot/local_kinematics.py

@@ -0,0 +1,139 @@
+"""Small, allocation-light local kinematics helpers for xArm7 safety checks."""
+
+from __future__ import annotations
+
+import socket
+import struct
+
+import numpy as np
+
+
+_KINEMATICS_REQUEST = bytes((0x00, 0x01, 0x00, 0x02, 0x00, 0x01, 0x08))
+_KINEMATICS_RESPONSE_SIZE = 179
+
+
+def read_xarm7_kinematics(robot_ip: str, timeout_s: float = 2.0) -> np.ndarray:
+    """Read the controller-calibrated joint origins used by UFACTORY's ROS model.
+
+    Returns seven rows of ``x, y, z, roll, pitch, yaw``. Translation is in
+    metres and rotation is in radians.
+    """
+    with socket.create_connection((robot_ip, 502), timeout=timeout_s) as sock:
+        sock.settimeout(timeout_s)
+        sock.sendall(_KINEMATICS_REQUEST)
+        response = bytearray()
+        while len(response) < _KINEMATICS_RESPONSE_SIZE:
+            chunk = sock.recv(_KINEMATICS_RESPONSE_SIZE - len(response))
+            if not chunk:
+                break
+            response.extend(chunk)
+
+    if len(response) != _KINEMATICS_RESPONSE_SIZE or response[8] == 0:
+        raise RuntimeError(
+            f"Unable to read xArm kinematics calibration: bytes={len(response)}, "
+            f"valid={bool(response[8]) if len(response) > 8 else False}"
+        )
+    if response[9] != 7:
+        raise RuntimeError(f"Expected xArm7 kinematics, controller reported {response[9]} axes")
+    origins = np.asarray(struct.unpack("<42f", response[11:179]), dtype=np.float64).reshape(7, 6)
+    if not np.all(np.isfinite(origins)):
+        raise RuntimeError("Controller returned non-finite kinematics calibration")
+    return origins
+
+
+def _rpy_rotation(roll: float, pitch: float, yaw: float) -> np.ndarray:
+    cr, sr = np.cos(roll), np.sin(roll)
+    cp, sp = np.cos(pitch), np.sin(pitch)
+    cy, sy = np.cos(yaw), np.sin(yaw)
+    return np.asarray(
+        [
+            [cy * cp, cy * sp * sr - sy * cr, cy * sp * cr + sy * sr],
+            [sy * cp, sy * sp * sr + cy * cr, sy * sp * cr - cy * sr],
+            [-sp, cp * sr, cp * cr],
+        ],
+        dtype=np.float64,
+    )
+
+
+def _pose_transform(pose: np.ndarray, translation_scale: float) -> np.ndarray:
+    transform = np.eye(4, dtype=np.float64)
+    transform[:3, :3] = _rpy_rotation(*pose[3:6])
+    transform[:3, 3] = pose[:3] * translation_scale
+    return transform
+
+
+def xarm_rpy_transform(pose: list[float] | np.ndarray) -> np.ndarray:
+    """Convert xArm FK ``x, y, z, roll, pitch, yaw`` output to a matrix."""
+    value = np.asarray(pose, dtype=np.float64)
+    if value.shape != (6,) or not np.all(np.isfinite(value)):
+        raise ValueError("RPY pose must contain six finite values")
+    return _pose_transform(value, 1.0)
+
+
+class XArm7Kinematics:
+    """Forward kinematics and TCP-height Jacobian for one calibrated xArm7."""
+
+    def __init__(
+        self,
+        joint_origins: np.ndarray,
+        tcp_offset: list[float] | np.ndarray | None = None,
+        world_offset: list[float] | np.ndarray | None = None,
+        end_transform: np.ndarray | None = None,
+    ) -> None:
+        origins = np.asarray(joint_origins, dtype=np.float64)
+        if origins.shape != (7, 6) or not np.all(np.isfinite(origins)):
+            raise ValueError("joint_origins must be a finite 7x6 array")
+        tcp = np.zeros(6) if tcp_offset is None else np.asarray(tcp_offset, dtype=np.float64)
+        world = np.zeros(6) if world_offset is None else np.asarray(world_offset, dtype=np.float64)
+        if tcp.shape != (6,) or world.shape != (6,) or not np.all(np.isfinite([*tcp, *world])):
+            raise ValueError("TCP and world offsets must be finite six-element poses")
+
+        self._origins = tuple(_pose_transform(row, 1000.0) for row in origins)
+        if end_transform is None:
+            self._tcp = _pose_transform(tcp, 1.0)
+        else:
+            endpoint = np.asarray(end_transform, dtype=np.float64)
+            if endpoint.shape != (4, 4) or not np.all(np.isfinite(endpoint)):
+                raise ValueError("end_transform must be a finite 4x4 matrix")
+            self._tcp = endpoint.copy()
+        self._world = _pose_transform(world, 1.0)
+
+    def forward_matrix(self, joints: list[float] | np.ndarray) -> np.ndarray:
+        q = np.asarray(joints, dtype=np.float64)
+        if q.shape != (7,) or not np.all(np.isfinite(q)):
+            raise ValueError("joints must be a finite seven-element vector")
+        transform = self._world.copy()
+        for origin, angle in zip(self._origins, q, strict=True):
+            transform = transform @ origin
+            c, s = np.cos(angle), np.sin(angle)
+            rotation_z = np.asarray(
+                [[c, -s, 0.0, 0.0], [s, c, 0.0, 0.0], [0.0, 0.0, 1.0, 0.0], [0.0, 0.0, 0.0, 1.0]],
+                dtype=np.float64,
+            )
+            transform = transform @ rotation_z
+        return transform @ self._tcp
+
+    def tcp_position(self, joints: list[float] | np.ndarray) -> np.ndarray:
+        return self.forward_matrix(joints)[:3, 3]
+
+    def tcp_z_and_jacobian(self, joints: list[float] | np.ndarray) -> tuple[float, np.ndarray]:
+        q = np.asarray(joints, dtype=np.float64)
+        if q.shape != (7,) or not np.all(np.isfinite(q)):
+            raise ValueError("joints must be a finite seven-element vector")
+
+        transform = self._world.copy()
+        axes = np.empty((7, 3), dtype=np.float64)
+        points = np.empty((7, 3), dtype=np.float64)
+        for index, (origin, angle) in enumerate(zip(self._origins, q, strict=True)):
+            transform = transform @ origin
+            points[index] = transform[:3, 3]
+            axes[index] = transform[:3, 2]
+            c, s = np.cos(angle), np.sin(angle)
+            rotation_z = np.asarray(
+                [[c, -s, 0.0, 0.0], [s, c, 0.0, 0.0], [0.0, 0.0, 1.0, 0.0], [0.0, 0.0, 0.0, 1.0]],
+                dtype=np.float64,
+            )
+            transform = transform @ rotation_z
+        tcp_position = (transform @ self._tcp)[:3, 3]
+        jacobian_z = np.cross(axes, tcp_position - points)[:, 2]
+        return float(tcp_position[2]), jacobian_z

src/lerobot_robot_ufactory/robots/uf_robot/uf_robot.py

@@ -5,8 +5,10 @@ import math
 import logging
 import struct
 import numpy as np
+from datetime import datetime
 from enum import IntEnum
 from dataclasses import dataclass
+from pathlib import Path
 from threading import Thread, Event, Lock
 from lerobot.robots import Robot
 from lerobot.cameras.utils import make_cameras_from_configs
@@ -15,9 +17,29 @@ from .uf_robot_config import UFRobotConfig
 from xarm.wrapper import XArmAPI
 from xarm.core.utils import convert
 
+from .local_kinematics import (
+    XArm7Kinematics,
+    read_xarm7_kinematics,
+    xarm_rpy_transform,
+)
+
+
+logger = logging.getLogger(__name__)
+
 ## Configurations:
 INIT_SYNC_JOINT_VELOCITY_RAD = 0.2
-ROBOT_RESET_SPEED_DEG = 60
+ROBOT_RESET_SPEED_DEG = 20
+TCP_Z_CLAMP_TOLERANCE_MM = 1e-3
+TCP_Z_LOG_INTERVAL_S = 1.0
+TCP_Z_MAX_IK_JOINT_STEP_RAD = math.radians(10.0)
+LOCAL_GUARD_MAX_ITERATIONS = 8
+LOCAL_GUARD_JACOBIAN_DAMPING = 1e-6
+XARM7_JOINT_LOWER_RAD = np.asarray(
+    [-2 * math.pi, -2.059, -2 * math.pi, -0.19198, -2 * math.pi, -1.69297, -2 * math.pi]
+)
+XARM7_JOINT_UPPER_RAD = np.asarray(
+    [2 * math.pi, 2.0944, 2 * math.pi, 3.927, 2 * math.pi, math.pi, 2 * math.pi]
+)
 
 CARTESIAN_OBS_KEYS = [
     "pose.x", "pose.y", "pose.z", "pose.rx", "pose.ry", "pose.rz",
@@ -85,17 +107,42 @@ class UFRobot(Robot, Thread):
         self._is_connected = False
         self._is_calibrated =True
 
+        self.enable_logs = bool(getattr(config, "enable_logs", False))
         self.logs = {}
 
         self._cmd_cnt = 0
+        self._last_gripper_command = None
+        self._last_gripper_command_attempt_s = float("-inf")
+        self._last_logged_controller_error = 0
 
         self._max_joint_velocity = math.radians(self.config.max_joint_velocity)
         self._max_linear_velocity = self.config.max_linear_velocity
 
+        self._min_tcp_z_mm = self.config.min_tcp_z_mm
+        self._tcp_z_guard_activation_margin_mm = self.config.tcp_z_guard_activation_margin_mm
+        self._tcp_z_guard_backend = self.config.tcp_z_guard_backend
+        self._tcp_z_soft_floor_mm = (
+            None
+            if self._min_tcp_z_mm is None
+            else self._min_tcp_z_mm + self.config.tcp_z_soft_margin_mm
+        )
+        self._local_kinematics = None
+        self._local_joint_origins = None
+        self._local_model_fault_count = 0
+        self._last_safe_joint_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
+
         self.report_stop_event = Event()
         self._rt_report_normal = False
         self._update_lock = Lock()
-        self._use_rt_report = (self._control_space == "cartesian") # Cartesian observations must utilize rt_report
+        # Cartesian observations and the joint-mode TCP z guard use the
+        # asynchronous RT report.
+        self._use_rt_report = (
+            self._control_space == "cartesian" or self._min_tcp_z_mm is not None
+        )
         self._cart_obs_has_vel = any('velo.' in key for key in CARTESIAN_OBS_KEYS)
         self._jnt_obs_has_vel = self.config.observe_joint_vel
 
@@ -174,6 +221,8 @@ class UFRobot(Robot, Thread):
         return action_ft
 
     def connect(self, calibrate: bool = True) -> None:
+        if self._tcp_z_guard_backend == "local_projection":
+            self._local_joint_origins = read_xarm7_kinematics(self.config.robot_ip)
         self.real_arm = XArmAPI(self.config.robot_ip)
         time.sleep(0.2)
         self._is_connected = self.real_arm.connected
@@ -193,6 +242,9 @@ class UFRobot(Robot, Thread):
             raise RuntimeError(f"Invalid initial point returned by xArm: {initial_point}")
         self._initial_point = list(initial_point[:self._dof])
 
+        if self._tcp_z_guard_backend == "local_projection":
+            self._initialize_local_kinematics()
+
         for cam in self.cameras.values():
             cam.connect()
             self._is_connected = self._is_connected and cam.is_connected
@@ -210,6 +262,15 @@ class UFRobot(Robot, Thread):
             self.configure()
         else:
             self.reset_to_initial()
+        # reset_to_initial clears any pre-existing controller error before
+        # configuring APIs that may otherwise be rejected with code 1.
+        if (
+            self._tcp_z_guard_backend == "local_projection"
+            and self.config.controller_safety_boundary
+        ):
+            self._configure_controller_safety_boundary()
+        if self._min_tcp_z_mm is not None and self.config.manual_mode:
+            self._initialize_tcp_z_guard()
         if calibrate:  
             self.calibrate()
 
@@ -243,6 +304,363 @@ class UFRobot(Robot, Thread):
             raise RuntimeError(f"Failed to move to xArm initial point, code={code}")
 
         self.configure()
+        if self._min_tcp_z_mm is not None:
+            self._initialize_tcp_z_guard()
+
+    def _initialize_tcp_z_guard(self) -> None:
+        """Initialize guard state from the robot's current physical target."""
+        if self._control_space != "joint" or self._min_tcp_z_mm is None or self.real_arm is None:
+            return
+
+        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"Unable to initialize TCP z guard from joint state, code={code}")
+        target = np.asarray(states[0][:self._dof], dtype=np.float64)
+        if not np.all(np.isfinite(target)):
+            raise RuntimeError("Unable to initialize TCP z guard from non-finite joint state")
+        self._last_safe_joint_target = target
+        if self._tcp_z_guard_backend == "local_projection":
+            if self._local_kinematics is None:
+                raise RuntimeError("Local xArm7 kinematics has not been initialized")
+            current_z = float(self._local_kinematics.tcp_position(target)[2])
+            if current_z < self._min_tcp_z_mm:
+                raise RuntimeError(
+                    f"Current TCP z {current_z:.2f} mm is below the hard floor "
+                    f"{self._min_tcp_z_mm:.2f} mm"
+                )
+        self._tcp_z_is_clamped = False
+        self._tcp_z_last_log_time = 0.0
+        self._tcp_z_last_error_log_time = 0.0
+
+    def _controller_pose_offset(self, name: str) -> np.ndarray:
+        pose = np.asarray(getattr(self.real_arm, name), dtype=np.float64)
+        if pose.shape != (6,) or not np.all(np.isfinite(pose)):
+            raise RuntimeError(f"Controller returned an invalid {name}: {pose}")
+        if not self.real_arm.default_is_radian:
+            pose[3:6] = np.radians(pose[3:6])
+        return pose
+
+    def _initialize_local_kinematics(self) -> None:
+        if self._local_joint_origins is None:
+            raise RuntimeError("xArm7 calibration parameters were not loaded")
+        code, states = self.real_arm.get_joint_states(is_radian=True, num=1)
+        if code != 0 or not states or len(states[0]) < 7:
+            raise RuntimeError(f"Unable to validate local kinematics from joint state, code={code}")
+        current = np.asarray(states[0][:7], dtype=np.float64)
+        world_offset = self._controller_pose_offset("world_offset")
+        chain_kinematics = XArm7Kinematics(
+            self._local_joint_origins,
+            world_offset=world_offset,
+        )
+        code, current_pose = self.real_arm.get_forward_kinematics(
+            current.tolist(), input_is_radian=True, return_is_radian=True
+        )
+        if code != 0 or current_pose is None or len(current_pose) < 6:
+            raise RuntimeError(f"Controller FK failed while identifying its TCP endpoint, code={code}")
+        controller_transform = xarm_rpy_transform(current_pose[:6])
+        endpoint_transform = (
+            np.linalg.inv(chain_kinematics.forward_matrix(current)) @ controller_transform
+        )
+        self._local_kinematics = XArm7Kinematics(
+            self._local_joint_origins,
+            world_offset=world_offset,
+            end_transform=endpoint_transform,
+        )
+
+        current_z = float(self._local_kinematics.tcp_position(current)[2])
+        if current_z < self._min_tcp_z_mm:
+            raise RuntimeError(
+                f"Current TCP z {current_z:.2f} mm is below the configured hard floor "
+                f"{self._min_tcp_z_mm:.2f} mm"
+            )
+        validation_points = [current.copy(), current.copy()]
+        validation_points[0][1] = np.clip(
+            validation_points[0][1] + 0.05,
+            XARM7_JOINT_LOWER_RAD[1],
+            XARM7_JOINT_UPPER_RAD[1],
+        )
+        validation_points[1][3] = np.clip(
+            validation_points[1][3] + 0.05,
+            XARM7_JOINT_LOWER_RAD[3],
+            XARM7_JOINT_UPPER_RAD[3],
+        )
+        for joints in validation_points:
+            code, pose = self.real_arm.get_forward_kinematics(
+                joints.tolist(), input_is_radian=True, return_is_radian=True
+            )
+            if code != 0 or pose is None or len(pose) < 3:
+                raise RuntimeError(f"Controller FK failed during local model validation, code={code}")
+            local_position = self._local_kinematics.tcp_position(joints)
+            error_mm = float(np.linalg.norm(local_position - np.asarray(pose[:3], dtype=np.float64)))
+            if not math.isfinite(error_mm) or error_mm > self.config.local_kinematics_max_error_mm:
+                raise RuntimeError(
+                    f"Local kinematics differs from controller FK by {error_mm:.3f} mm "
+                    f"(limit {self.config.local_kinematics_max_error_mm:.3f} mm)"
+                )
+
+    @staticmethod
+    def _motion_code(code):
+        return code[0] if isinstance(code, (tuple, list)) else code
+
+    def _configure_controller_safety_boundary(self) -> None:
+        floor = int(math.ceil(self._min_tcp_z_mm))
+        boundary = [9999, -9999, 9999, -9999, 9999, floor]
+        code = self._motion_code(self.real_arm.set_reduced_tcp_boundary(boundary))
+        self._check_motion_code("set_reduced_tcp_boundary", code)
+        code = self._motion_code(self.real_arm.set_fence_mode(True))
+        self._check_motion_code("set_fence_mode(True)", code)
+
+        code, states = self.real_arm.get_reduced_states(is_radian=True)
+        self._check_motion_code("get_reduced_states", code)
+        if len(states) < 2 or list(map(int, states[1][:6])) != boundary:
+            raise RuntimeError(f"Controller safety boundary readback mismatch: {states}")
+        if len(states) >= 6 and not bool(states[5]):
+            raise RuntimeError("Controller fence mode did not remain enabled")
+
+    def _log_tcp_z_clamp(self, clamped: bool, requested_z: float | None = None) -> None:
+        """Log clamp state changes while avoiding per-cycle console spam."""
+        now = time.monotonic()
+        if clamped:
+            should_log = not self._tcp_z_is_clamped or now - self._tcp_z_last_log_time >= TCP_Z_LOG_INTERVAL_S
+            if should_log:
+                logger.warning(
+                    "TCP z safety clamp active: requested %.2f mm, limiting to %.2f mm",
+                    requested_z if requested_z is not None else float("nan"),
+                    self._min_tcp_z_mm,
+                )
+                self._tcp_z_last_log_time = now
+        elif self._tcp_z_is_clamped:
+            logger.info("TCP z safety clamp released")
+        self._tcp_z_is_clamped = clamped
+
+    def _log_tcp_z_guard_error(self, message: str) -> None:
+        now = time.monotonic()
+        if now - self._tcp_z_last_error_log_time >= TCP_Z_LOG_INTERVAL_S:
+            logger.error("TCP z safety guard rejected target: %s", message)
+            self._tcp_z_last_error_log_time = now
+
+    def _guard_joint_target(self, command: list[float]) -> np.ndarray | None:
+        if getattr(self, "_tcp_z_guard_backend", "controller_rpc") == "local_projection":
+            return self._guard_joint_target_local(command)
+        return self._guard_joint_target_controller_rpc(command)
+
+    def _guard_joint_target_local(self, command: list[float]) -> np.ndarray | None:
+        """Project a joint update onto the local TCP-height constraint."""
+        desired = np.asarray(command, dtype=np.float64)
+        fallback = self._last_safe_joint_target
+        try:
+            if self._local_kinematics is None:
+                raise RuntimeError("local kinematics is unavailable")
+            if not self._local_model_matches_rt():
+                self._last_guard_path = "model_fault"
+                return None if fallback is None else np.asarray(fallback, dtype=np.float64).copy()
+            if desired.shape != (7,) or not np.all(np.isfinite(desired)):
+                raise ValueError("joint target has invalid shape or contains NaN/Inf")
+            if fallback is None:
+                raise RuntimeError("last safe joint target is unavailable")
+
+            previous = np.asarray(fallback, dtype=np.float64)
+            if previous.shape != (7,) or not np.all(np.isfinite(previous)):
+                raise RuntimeError("last safe joint target is invalid")
+            delta = (desired - previous + math.pi) % (2 * math.pi) - math.pi
+            delta = np.clip(delta, -TCP_Z_MAX_IK_JOINT_STEP_RAD, TCP_Z_MAX_IK_JOINT_STEP_RAD)
+            candidate = np.clip(previous + delta, XARM7_JOINT_LOWER_RAD, XARM7_JOINT_UPPER_RAD)
+            soft_floor = float(self._tcp_z_soft_floor_mm)
+
+            requested_z = float(self._local_kinematics.tcp_position(candidate)[2])
+            if requested_z >= soft_floor:
+                self._last_guard_path = "local_safe"
+                self._last_safe_joint_target = candidate.copy()
+                self._log_tcp_z_clamp(False)
+                return candidate
+
+            projected = candidate
+            for _ in range(LOCAL_GUARD_MAX_ITERATIONS):
+                z_value, jacobian_z = self._local_kinematics.tcp_z_and_jacobian(projected)
+                error = soft_floor - z_value
+                if error <= TCP_Z_CLAMP_TOLERANCE_MM:
+                    break
+                norm_sq = float(jacobian_z @ jacobian_z)
+                if not math.isfinite(norm_sq) or norm_sq < 1e-10:
+                    raise RuntimeError("TCP-height Jacobian is singular")
+                projected = projected + jacobian_z * error / (
+                    norm_sq + LOCAL_GUARD_JACOBIAN_DAMPING
+                )
+                projected = np.clip(projected, XARM7_JOINT_LOWER_RAD, XARM7_JOINT_UPPER_RAD)
+
+            projected_delta = (projected - previous + math.pi) % (2 * math.pi) - math.pi
+            max_delta = float(np.max(np.abs(projected_delta)))
+            if max_delta > TCP_Z_MAX_IK_JOINT_STEP_RAD:
+                projected = previous + projected_delta * (TCP_Z_MAX_IK_JOINT_STEP_RAD / max_delta)
+
+            projected_z = float(self._local_kinematics.tcp_position(projected)[2])
+            if projected_z < soft_floor - TCP_Z_CLAMP_TOLERANCE_MM:
+                # The linearized correction can overshoot near high curvature.
+                # Backtrack toward the known-safe previous command without an RPC.
+                accepted = None
+                for alpha in np.linspace(0.875, 0.0, 8):
+                    trial = previous + alpha * (projected - previous)
+                    if float(self._local_kinematics.tcp_position(trial)[2]) >= soft_floor:
+                        accepted = trial
+                        break
+                if accepted is None:
+                    self._last_guard_path = "local_hold"
+                    self._log_tcp_z_clamp(True, requested_z)
+                    return previous.copy()
+                projected = accepted
+
+            self._last_guard_path = "local_projected"
+            self._last_safe_joint_target = projected.copy()
+            self._log_tcp_z_clamp(True, requested_z)
+            return projected
+        except Exception as exc:
+            self._last_guard_path = "local_hold"
+            self._log_tcp_z_guard_error(str(exc))
+            return None if fallback is None else np.asarray(fallback, dtype=np.float64).copy()
+
+    def _local_model_matches_rt(self) -> bool:
+        """Compare co-timed RT joint/TCP feedback without making an SDK request."""
+        if not getattr(self, "_rt_report_normal", False):
+            return True
+        with self._update_lock:
+            joints = np.asarray(self.rt_actual_joint_pos, dtype=np.float64).copy()
+            reported = np.asarray(self.rt_actual_tcp_pose[:3], dtype=np.float64).copy()
+        try:
+            local = self._local_kinematics.tcp_position(joints)
+            error_mm = float(np.linalg.norm(local - reported))
+            limit = float(self.config.local_kinematics_max_error_mm)
+            if getattr(self, "enable_logs", True):
+                self.logs["local_kinematics_error_mm"] = error_mm
+            if math.isfinite(error_mm) and error_mm <= limit:
+                self._local_model_fault_count = 0
+                return True
+        except Exception as exc:
+            self._log_tcp_z_guard_error(f"RT model validation failed: {exc}")
+        self._local_model_fault_count += 1
+        if self._local_model_fault_count >= 3:
+            self._log_tcp_z_guard_error(
+                f"local/RT TCP mismatch persisted for {self._local_model_fault_count} frames"
+            )
+            return False
+        return True
+
+    def _guard_joint_target_controller_rpc(self, command: list[float]) -> np.ndarray | None:
+        """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
+        try:
+            if desired.shape != (self._dof,) or not np.all(np.isfinite(desired)):
+                raise ValueError("joint target has invalid shape or contains NaN/Inf")
+
+            # Far above the floor, the current TCP height is available from
+            # the RT report. Bypass the synchronous controller FK call so the
+            # 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
+
+            code, pose = self.real_arm.get_forward_kinematics(
+                desired.tolist(), 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)):
+                raise RuntimeError(f"forward kinematics failed, code={code}")
+
+            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)
+                return desired
+
+            clamped_pose = pose[:6].copy()
+            clamped_pose[2] = self._min_tcp_z_mm
+            ik_reference = fallback if fallback is not None else desired
+            code, inverse = self.real_arm.get_inverse_kinematics(
+                clamped_pose.tolist(),
+                input_is_radian=True,
+                return_is_radian=True,
+                limited=True,
+                ref_angles=np.asarray(ik_reference, dtype=np.float64).tolist(),
+            )
+            inverse = np.asarray(inverse, dtype=np.float64)
+            if code != 0 or inverse.shape[0] < self._dof or not np.all(np.isfinite(inverse)):
+                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
+            )
+            verified_pose = np.asarray(verified_pose, dtype=np.float64)
+            if (
+                code != 0
+                or verified_pose.shape[0] < 3
+                or not np.all(np.isfinite(verified_pose))
+                or verified_pose[2] < self._min_tcp_z_mm - TCP_Z_CLAMP_TOLERANCE_MM
+            ):
+                raise RuntimeError(f"inverse-kinematics result is below the TCP z floor, code={code}")
+
+            self._last_safe_joint_target = safe_target
+            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
+            return np.asarray(fallback, dtype=np.float64).copy()
+
+    def _validate_guard_joint_target(
+        self,
+        target: np.ndarray,
+        previous_safe_target: np.ndarray | None,
+        label: str,
+    ) -> None:
+        code, is_limited = self.real_arm.is_joint_limit(target.tolist(), is_radian=True)
+        if code != 0 or is_limited is not False:
+            raise RuntimeError(
+                f"{label} violates a joint limit, code={code}, limited={is_limited}, "
+                f"target={target.tolist()}"
+            )
+
+        if previous_safe_target is None:
+            return
+        previous = np.asarray(previous_safe_target, dtype=np.float64)
+        if previous.shape != target.shape or not np.all(np.isfinite(previous)):
+            raise RuntimeError("previous safe joint target is invalid")
+        delta = (target - previous + math.pi) % (2 * math.pi) - math.pi
+        max_delta = float(np.max(np.abs(delta)))
+        if max_delta > TCP_Z_MAX_IK_JOINT_STEP_RAD:
+            raise RuntimeError(
+                f"{label} jumps {math.degrees(max_delta):.1f} deg from the previous safe target"
+            )
+
+    def _rt_actual_tcp_is_far_above_floor(self) -> bool:
+        if self._min_tcp_z_mm is None or not getattr(self, "_rt_report_normal", False):
+            return False
+        update_lock = getattr(self, "_update_lock", None)
+        if update_lock is None:
+            return False
+        with update_lock:
+            pose = getattr(self, "rt_actual_tcp_pose", None)
+            if pose is None or len(pose) < 3:
+                return False
+            actual_z = float(pose[2])
+        activation_margin = getattr(self, "_tcp_z_guard_activation_margin_mm", 100.0)
+        return (
+            math.isfinite(actual_z)
+            and actual_z > self._min_tcp_z_mm + activation_margin
+        )
 
     def configure(self) -> None:
         self.real_arm.motion_enable()
@@ -302,11 +720,17 @@ class UFRobot(Robot, Thread):
         self.real_arm._arm._baud_checkset = True
         try:
             if self._gripper_type == GripperType.xArmGripper:
-                self.real_arm.set_gripper_enable(True)
-                self.real_arm.set_gripper_mode(0)
-                self.real_arm.set_gripper_speed(self._gripper_param.speed)
+                self._check_gripper_code("set_gripper_enable", self.real_arm.set_gripper_enable(True))
+                self._check_gripper_code("set_gripper_mode", self.real_arm.set_gripper_mode(0))
+                self._check_gripper_code("set_gripper_speed", self.real_arm.set_gripper_speed(self._gripper_param.speed))
                 if move_to_open:
-                    self.real_arm.set_gripper_position(self._gripper_param.open_pos)
+                    self._check_gripper_code(
+                        "set_gripper_position",
+                        self.real_arm.set_gripper_position(
+                            self._gripper_param.open_pos,
+                            wait=True,
+                        ),
+                    )
             elif self._gripper_type == GripperType.xArmGripperG2:
                 self.real_arm.set_gripper_enable(True)
                 self.real_arm.set_gripper_mode(0)
@@ -339,6 +763,7 @@ class UFRobot(Robot, Thread):
         if move_to_open:
             self._gripper_param.grippos = self._gripper_param.open_pos
             self._gripper_param.gripper_norm = 0.0
+            self._last_gripper_command = 0.0
 
     def calibrate(self) -> None:
         self._is_calibrated = True
@@ -346,9 +771,11 @@ class UFRobot(Robot, Thread):
 
     def get_observation(self) -> dict[str, np.ndarray]:
         obs_dict = {}
+        self._log_controller_error_if_changed("get_observation")
+        logs_enabled = bool(getattr(self, "enable_logs", True))
 
-        # Read Stretch state
-        before_read_t = time.perf_counter()
+        # Read robot state
+        before_read_t = time.perf_counter() if logs_enabled else None
         if self._control_space == "joint":
             code, states = self.real_arm.get_joint_states(is_radian=True, num=3)
             pos_list = states[0].copy()
@@ -377,6 +804,8 @@ class UFRobot(Robot, Thread):
         if self._gripper_type > GripperType.NoGripper:
             if self._gripper_type == GripperType.xArmGripper:
                 code, grippos = self.real_arm.get_gripper_position()
+                if code != 0 or grippos is None:
+                    self._log_gripper_error("get_gripper_position", code, f"position={grippos}")
                 grippos_norm = self._gripper_param.get_gripper_norm(grippos)
             elif self._gripper_type == GripperType.xArmGripperG2:
                 code, grippos = self.real_arm.get_gripper_g2_position()
@@ -391,12 +820,13 @@ class UFRobot(Robot, Thread):
                 self.real_arm.robotiq_get_status(number_of_registers=3)
                 grippos = self.real_arm.robotiq_status['gPO']  # 0..255
                 grippos_norm = self._gripper_param.get_gripper_norm(grippos) # 0=open, 1=closed
-            self.logs["read_pos_dt_s"] = time.perf_counter() - before_read_t
+            if logs_enabled:
+                self.logs["read_pos_dt_s"] = time.perf_counter() - before_read_t
             obs_dict[f"{self.prefix}gripper.pos"] = grippos_norm
 
         # Capture images from cameras
         for cam_key, cam in self.cameras.items():
-            before_camread_t = time.perf_counter()
+            before_camread_t = time.perf_counter() if logs_enabled else None
             frame = cam.async_read()
             shape = frame.shape
             if (self.camera_height > 0 and self.camera_height != shape[0]) or (self.camera_width > 0 and self.camera_width != shape[1]):
@@ -405,17 +835,111 @@ class UFRobot(Robot, Thread):
                 import cv2
                 frame = cv2.resize(frame, (camera_height, camera_width), interpolation=cv2.INTER_AREA)
             obs_dict[f"{self.prefix}{cam_key}"] = frame
-            self.logs[f"async_read_camera_{cam_key}_dt_s"] = time.perf_counter() - before_camread_t
+            if logs_enabled:
+                self.logs[f"async_read_camera_{cam_key}_dt_s"] = (
+                    time.perf_counter() - before_camread_t
+                )
 
         return obs_dict
 
+    def get_realtime_observation(self) -> dict[str, np.ndarray]:
+        """Build a recording observation without controller command-channel reads.
+
+        Joint feedback comes from the asynchronous RT report, while gripper
+        feedback uses the latest commanded/cached value. Camera reads remain
+        outside the ServoJ control thread.
+        """
+        if self._control_space != "joint":
+            return self.get_observation()
+        if not self._rt_report_normal:
+            raise ConnectionError("RT Report for target robot NOT READY!")
+        logs_enabled = bool(getattr(self, "enable_logs", True))
+        with self._update_lock:
+            positions = self.rt_actual_joint_pos.copy()
+            velocities = self.rt_actual_joint_speed.copy()
+            # Timestamp the state snapshot before the slower camera reads.
+            self._last_realtime_observation_monotonic_s = time.perf_counter()
+        obs_dict = {
+            f"{self.prefix}J{index + 1}.pos": positions[index]
+            for index in range(self._dof)
+        }
+        if self._jnt_obs_has_vel:
+            obs_dict.update(
+                {
+                    f"{self.prefix}J{index + 1}.vel": velocities[index]
+                    for index in range(self._dof)
+                }
+            )
+        if self._gripper_type > GripperType.NoGripper:
+            gripper = self._last_gripper_command
+            if gripper is None:
+                gripper = self._gripper_param.gripper_norm
+            obs_dict[f"{self.prefix}gripper.pos"] = float(gripper)
+
+        for camera_key, camera in self.cameras.items():
+            before_camera_t = time.perf_counter() if logs_enabled else None
+            frame = camera.async_read()
+            after_camera_t = time.perf_counter() if logs_enabled else None
+            shape = frame.shape
+            if (
+                self.camera_height > 0
+                and self.camera_height != shape[0]
+                or self.camera_width > 0
+                and self.camera_width != shape[1]
+            ):
+                import cv2
+
+                width = self.camera_width if self.camera_width != 0 else shape[1]
+                height = self.camera_height if self.camera_height != 0 else shape[0]
+                frame = cv2.resize(frame, (width, height), interpolation=cv2.INTER_AREA)
+            obs_dict[f"{self.prefix}{camera_key}"] = frame
+            if logs_enabled:
+                self.logs[f"async_read_camera_{camera_key}_dt_s"] = (
+                    after_camera_t - before_camera_t
+                )
+                if not hasattr(self, "_last_realtime_camera_timings"):
+                    self._last_realtime_camera_timings = {}
+                self._last_realtime_camera_timings[camera_key] = (
+                    before_camera_t,
+                    after_camera_t,
+                )
+        if logs_enabled:
+            self._last_realtime_observation_end_monotonic_s = time.perf_counter()
+        return obs_dict
+
     def _send_gripper_action(self, gripper_norm: float) -> None:
         gripper_norm = min(max(float(gripper_norm), 0.0), 1.0)
+        logs_enabled = bool(getattr(self, "enable_logs", True))
+        if (
+            self._last_gripper_command is not None
+            and abs(gripper_norm - self._last_gripper_command)
+            < self.config.gripper_command_threshold
+        ):
+            return
+
+        # The gripper goes through the controller RS485 bridge. Driving that
+        # bridge at the 60 Hz ServoJ rate can trigger controller error 19.
+        # Intermediate targets are coalesced and failed attempts are also
+        # rate-limited so an error cannot cause a retry storm.
+        now = time.perf_counter()
+        if now - self._last_gripper_command_attempt_s < self.config.gripper_command_interval_s:
+            return
+        self._last_gripper_command_attempt_s = now
+        command_start_s = now if logs_enabled else None
+
         if self._gripper_type == GripperType.xArmGripper:
             grippos = self._gripper_param.get_grippos(gripper_norm)
-            modbus_datas = [0x08, 0x10, 0x07, 0x00, 0x00, 0x02, 0x04]
-            modbus_datas.extend(list(struct.pack('>i', grippos)))
-            self.real_arm.getset_tgpio_modbus_data(modbus_datas)
+            # Use the SDK's dedicated gripper command instead of injecting a
+            # generic RS485 packet through set_rs485_data. During continuous
+            # ServoJ motion the default wait_motion check cannot complete, so
+            # explicitly bypass it while retaining a non-blocking write.
+            result = self.real_arm.set_gripper_position(
+                grippos,
+                wait=False,
+                wait_motion=False,
+                check_baud=False,
+                check_err=False,
+            )
         elif self._gripper_type == GripperType.xArmGripperG2:
             grippos = self._gripper_param.get_grippos(gripper_norm)
             grippos = int((math.degrees(math.asin((grippos - 16) / 110)) + 8.33) * 18.28)
@@ -423,7 +947,7 @@ class UFRobot(Robot, Thread):
             modbus_datas.extend(list(struct.pack('>h', self._gripper_param.speed)))
             modbus_datas.extend(list(struct.pack('>h', self._gripper_param.force)))
             modbus_datas.extend(list(struct.pack('>i', grippos)))
-            self.real_arm.getset_tgpio_modbus_data(modbus_datas)
+            result = self.real_arm.getset_tgpio_modbus_data(modbus_datas)
         elif self._gripper_type == GripperType.BioGripperG2:
             grippos = self._gripper_param.get_grippos(gripper_norm)
             grippos = int(grippos * 3.7342 - 265.13)
@@ -431,14 +955,83 @@ class UFRobot(Robot, Thread):
             modbus_datas.extend(list(struct.pack('>h', self._gripper_param.speed)))
             modbus_datas.extend(list(struct.pack('>h', self._gripper_param.force)))
             modbus_datas.extend(list(struct.pack('>i', grippos)))
-            self.real_arm.getset_tgpio_modbus_data(modbus_datas)
+            result = self.real_arm.getset_tgpio_modbus_data(modbus_datas)
         elif self._gripper_type == GripperType.PikaGripper:
             grippos = self._gripper_param.get_grippos(gripper_norm)
             self.pika_gripper.set_gripper_distance(grippos)
+            result = 0
         elif self._gripper_type == GripperType.RobotiqGripper:
             grippos = self._gripper_param.get_grippos(gripper_norm)
             modbus_datas = [0x09, 0x10, 0x03, 0xE8, 0x00, 0x03, 0x06, 0x09, 0x00, 0x00, grippos, self._gripper_param.speed, self._gripper_param.force]
-            self.real_arm.getset_tgpio_modbus_data(modbus_datas)
+            result = self.real_arm.getset_tgpio_modbus_data(modbus_datas)
+
+        code = result[0] if isinstance(result, (tuple, list)) else result
+        command_dt_ms = (
+            (time.perf_counter() - command_start_s) * 1000
+            if logs_enabled
+            else None
+        )
+        if code not in (None, 0):
+            detail = f"target={gripper_norm:.6f}, pulse={grippos}"
+            if command_dt_ms is not None:
+                detail += f", dt_ms={command_dt_ms:.3f}"
+            self._log_gripper_error(
+                "send_gripper_action",
+                code,
+                detail,
+            )
+            return
+        if logs_enabled:
+            self._log_gripper_command(gripper_norm, grippos, command_dt_ms)
+        self._last_gripper_command = gripper_norm
+
+    def _log_gripper_command(self, target: float, pulse: int, dt_ms: float) -> None:
+        log_path = self.config.gripper_error_log_path
+        if not log_path:
+            return
+        try:
+            path = Path(log_path).expanduser()
+            path.parent.mkdir(parents=True, exist_ok=True)
+            timestamp = datetime.now().astimezone().isoformat(timespec="milliseconds")
+            with path.open("a", encoding="utf-8") as stream:
+                stream.write(
+                    f"{timestamp} gripper command: target={target:.6f}, "
+                    f"pulse={pulse}, dt_ms={dt_ms:.3f}, code=0\n"
+                )
+        except OSError:
+            logging.exception("Failed to write gripper command log to %s", log_path)
+
+    def _log_gripper_error(self, operation: str, code, detail: str = "") -> None:
+        controller_error = getattr(self.real_arm, "error_code", None)
+        message = (
+            f"gripper communication error: operation={operation}, code={code}, "
+            f"controller_error={controller_error}, {detail}"
+        ).rstrip(", ")
+        logging.error(message)
+
+        log_path = self.config.gripper_error_log_path
+        if not log_path:
+            return
+        try:
+            path = Path(log_path).expanduser()
+            path.parent.mkdir(parents=True, exist_ok=True)
+            timestamp = datetime.now().astimezone().isoformat(timespec="milliseconds")
+            with path.open("a", encoding="utf-8") as stream:
+                stream.write(f"{timestamp} {message}\n")
+        except OSError:
+            logging.exception("Failed to write gripper error log to %s", log_path)
+
+    def _check_gripper_code(self, operation: str, code) -> None:
+        if code in (None, 0):
+            return
+        self._log_gripper_error(operation, code)
+        raise RuntimeError(f"{operation} failed, code={code}, {self._motion_status()}")
+
+    def _log_controller_error_if_changed(self, operation: str) -> None:
+        controller_error = getattr(self.real_arm, "error_code", 0)
+        if controller_error and controller_error != self._last_logged_controller_error:
+            self._log_gripper_error(operation, "controller", "controller error became active")
+        self._last_logged_controller_error = controller_error
 
     def _motion_status(self) -> str:
         """Return controller state details for a failed motion command."""
@@ -457,6 +1050,7 @@ class UFRobot(Robot, Thread):
     def send_action(self, action: dict) -> np.ndarray:
         if not self._is_connected:
             raise ConnectionError()
+        self._log_controller_error_if_changed("send_action")
         if self.config.manual_mode:
             gripper_key = f"{self.prefix}gripper.pos"
             if (
@@ -472,7 +1066,9 @@ class UFRobot(Robot, Thread):
         if self.config.no_action:
             return action
 
-        before_write_t = time.perf_counter()
+        logs_enabled = bool(getattr(self, "enable_logs", True))
+        before_write_t = time.perf_counter() if logs_enabled else None
+        safe_action = dict(action)
         if self._control_space == "joint":
             # first sync with gello or other control device SLOWLY!
             jnt_spd = INIT_SYNC_JOINT_VELOCITY_RAD if self._cmd_cnt < 20 else self._max_joint_velocity
@@ -481,8 +1077,20 @@ 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() if logs_enabled else None
+            safe_cmd = self._guard_joint_target(cmd_list)
+            if logs_enabled:
+                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.
+                safe_cmd = None
+            else:
+                for i in range(self._dof):
+                    safe_action[f"{self.prefix}J{i+1}.pos"] = float(safe_cmd[i])
 
-            if self.config.joint_command_mode == 1:
+            if safe_cmd is not None and self.config.joint_command_mode == 1:
                 # set_servo_angle_j is an absolute target command. It is the
                 # SDK's high-frequency interface and executes only the latest
                 # target, so it must be used with servo motion mode (1).
@@ -492,11 +1100,14 @@ 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() if logs_enabled else None
                 code = self.real_arm.set_servo_angle_j(
-                    cmd_list[:self._dof], speed=jnt_spd, is_radian=True
+                    safe_cmd[:self._dof].tolist(), speed=jnt_spd, is_radian=True
                 )
+                if logs_enabled:
+                    self.logs["servo_j_dt_s"] = time.perf_counter() - servo_j_start_t
                 self._check_motion_code("set_servo_angle_j", code)
-            else:
+            elif safe_cmd is not None:
                 # The legacy mode-6 path uses the absolute move_joint API.
                 # The first blocking command must be sent in position mode.
                 if wait_ == False and self.real_arm.mode != 6:
@@ -513,7 +1124,7 @@ class UFRobot(Robot, Thread):
                     time.sleep(0.1)
 
                 code = self.real_arm.set_servo_angle(
-                    angle=cmd_list[:self._dof],
+                    angle=safe_cmd[:self._dof].tolist(),
                     speed=jnt_spd,
                     is_radian=True,
                     wait=wait_,
@@ -531,32 +1142,40 @@ class UFRobot(Robot, Thread):
         if self._cmd_cnt < 99999:
             self._cmd_cnt += 1 # CHECK!! possibility of overflow?
         if self._gripper_type > GripperType.NoGripper:
-            self._send_gripper_action(action[f"{self.prefix}gripper.pos"])
+            self._send_gripper_action(safe_action[f"{self.prefix}gripper.pos"])
 
-        self.logs["write_pos_dt_s"] = time.perf_counter() - before_write_t
-        return action
+        if logs_enabled:
+            self.logs["write_pos_dt_s"] = time.perf_counter() - before_write_t
+        return safe_action
 
     def print_logs(self) -> None:
         pass
 
     def disconnect(self) -> None:
-        self.real_arm.set_state(4) # stop
-        self.real_arm.set_mode(0)
+        if not self._is_connected and self.real_arm is None:
+            return
         if self._use_rt_report:
             self.report_stop_event.set()
-            self.join()
-        self.real_arm.disconnect()
+            if self.is_alive():
+                self.join()
+        if self.real_arm is not None and getattr(self.real_arm, "connected", False):
+            self.real_arm.set_state(4) # stop
+            self.real_arm.set_mode(0)
+            self.real_arm.disconnect()
         # CHECK!! how about gripper? 
 
         for cam in self.cameras.values():
-            cam.disconnect()
+            if getattr(cam, "is_connected", False):
+                cam.disconnect()
 
         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/robots/uf_robot/uf_robot_config.py

@@ -1,3 +1,4 @@
+import math
 from dataclasses import dataclass, field
 from lerobot.cameras import CameraConfig
 from lerobot.robots import RobotConfig
@@ -16,6 +17,10 @@ class UFRobotConfig(RobotConfig):
     gripper_port: str = None    # only used by pika gripper (gripper_type=10)
     gripper_speed: int = -1     # auto
     gripper_force: int = -1     # auto
+    gripper_command_threshold: float = 0.01  # normalized change required before sending a new command
+    gripper_command_interval_s: float = 0.1  # minimum interval between tool RS485 goals
+    gripper_error_log_path: str | None = "logs/xarm_gripper_errors.log"
+    enable_logs: bool = False  # optional per-cycle timing and diagnostic logs
     observe_joint_vel: bool = False # only effective in joint control mode
     manual_mode: bool = False  # xArm joint teaching mode; records state and optional gripper actions
     manual_gripper_speed: float = 0.5  # normalized gripper position per second in manual mode
@@ -26,6 +31,19 @@ class UFRobotConfig(RobotConfig):
     max_joint_velocity: int = 90   # °/s, only effective in joint control mode
     max_linear_velocity: int = 200 # mm/s, only effective in cartesian control mode
     no_action: bool = False # only for debug
+    # Optional TCP height floor in the xArm base coordinate system (mm).
+    # The value should include any desired safety margin above the table.
+    min_tcp_z_mm: float | None = None
+    # Skip synchronous FK while the actual TCP is this far above the floor.
+    # The RT report keeps this fast path asynchronous and avoids jitter during
+    # normal teleoperation; FK/IK remains active near the configured floor.
+    tcp_z_guard_activation_margin_mm: float = 100.0
+    # ``local_projection`` performs all per-cycle FK/Jacobian work on the CPU.
+    # ``controller_rpc`` retains the legacy controller FK/IK implementation.
+    tcp_z_guard_backend: str = "controller_rpc"
+    tcp_z_soft_margin_mm: float = 5.0
+    local_kinematics_max_error_mm: float = 2.0
+    controller_safety_boundary: bool = False
 
     def __post_init__(self):
         super().__post_init__()
@@ -37,5 +55,30 @@ class UFRobotConfig(RobotConfig):
                 raise ValueError("teach_sensitivity must be between 1 and 5")
         if self.manual_gripper_speed < 0:
             raise ValueError("manual_gripper_speed must be non-negative")
+        if not 0 <= self.gripper_command_threshold <= 1:
+            raise ValueError("gripper_command_threshold must be between 0 and 1")
+        if not math.isfinite(self.gripper_command_interval_s) or self.gripper_command_interval_s < 0:
+            raise ValueError("gripper_command_interval_s must be finite and non-negative")
         if self.control_space == "joint" and self.joint_command_mode not in (1, 6):
             raise ValueError("joint_command_mode must be 1 or 6 for joint control")
+        if self.min_tcp_z_mm is not None and not math.isfinite(self.min_tcp_z_mm):
+            raise ValueError("min_tcp_z_mm must be finite when provided")
+        if (
+            not math.isfinite(self.tcp_z_guard_activation_margin_mm)
+            or self.tcp_z_guard_activation_margin_mm < 0
+        ):
+            raise ValueError("tcp_z_guard_activation_margin_mm must be finite and non-negative")
+        if self.tcp_z_guard_backend not in ("controller_rpc", "local_projection"):
+            raise ValueError("tcp_z_guard_backend must be 'controller_rpc' or 'local_projection'")
+        if self.tcp_z_guard_backend == "local_projection":
+            if self.control_space != "joint" or self.robot_dof != 7:
+                raise ValueError("local_projection requires joint control on an xArm7")
+            if self.min_tcp_z_mm is None:
+                raise ValueError("local_projection requires min_tcp_z_mm")
+        if not math.isfinite(self.tcp_z_soft_margin_mm) or self.tcp_z_soft_margin_mm < 0:
+            raise ValueError("tcp_z_soft_margin_mm must be finite and non-negative")
+        if (
+            not math.isfinite(self.local_kinematics_max_error_mm)
+            or self.local_kinematics_max_error_mm <= 0
+        ):
+            raise ValueError("local_kinematics_max_error_mm must be finite and positive")

src/lerobot_robot_ufactory/scripts/uf_lerobot_eval.py

@@ -8,7 +8,6 @@ from dataclasses import asdict, dataclass
 from pprint import pformat
 import numpy as np
 import lerobot_robot_ufactory # patch
-from lerobot.scripts.lerobot_record import register_third_party_plugins
 from lerobot.datasets.pipeline_features import aggregate_pipeline_dataset_features, create_initial_features
 from lerobot.datasets.utils import build_dataset_frame, combine_feature_dicts
 from lerobot.policies.utils import make_robot_action
@@ -56,15 +55,6 @@ def continuous_rotvec(new_rv, prev_rv):
             new_rv = -(2 * np.pi - angle) * axis
     return new_rv
 
-def blend_poses(pose_a, pose_b, alpha):
-    """位姿混合: (1-alpha)*A + alpha*B, 旋转用 SO(3) 插值。
-    先将 pose_b 的 rotvec 归一化到与 pose_a 同符号半球，避免 ±π 跳变破坏线性混合。"""
-    blended_pos = (1 - alpha) * np.array(pose_a[:3]) + alpha * np.array(pose_b[:3])
-    # 旋转用线性混合 rotvec (delta 很小时近似 SLERP)
-    rot_b = continuous_rotvec(np.array(pose_b[3:6]), np.array(pose_a[3:6]))
-    blended_rot = (1 - alpha) * np.array(pose_a[3:6]) + alpha * rot_b
-    return np.concatenate([blended_pos, blended_rot]).tolist()
-
 def compute_relative_axis_angle(rot_prev, rot_curr):
     """
     计算两个轴角之间的相对旋转。
@@ -212,24 +202,6 @@ def eval_loop(cfg: EvalConfig, relative=False, rx_continuous=False, safety_guard
     device = get_safe_torch_device(policy.config.device, log=True)
     sleep_time_s = 1 / dataset_metadata.fps
 
-    # Gripper look-ahead: denormalization stats for peeking into the action queue
-    _gripper_mean = dataset_metadata.stats['action']['mean'][-1].item()
-    _gripper_std = dataset_metadata.stats['action']['std'][-1].item()
-    _gripper_min = dataset_metadata.stats['action']['min'][-1].item()
-    _gripper_max = dataset_metadata.stats['action']['max'][-1].item()
-    is_act_policy = hasattr(policy.config, 'chunk_size')
-    # ACT: lookahead 30 (~1s) 补偿 chunk 慢启动; DP: 队列仅 8 步，lookahead 4
-    GRIPPER_LOOKAHEAD = 0 if is_act_policy else 4
-
-    # =====================================================
-    # Chunk boundary smoothing: only damp large discontinuities at action chunk
-    # boundaries (mean ~6mm jump) while preserving smooth within-chunk motion (~1mm).
-    # When step-to-step cmd change exceeds SMOOTH_THRESHOLD, clamp it to that limit.
-    # =====================================================
-    SMOOTH_THRESHOLD = 0  # mm: smooth chunk boundary jumps while preserving trajectory
-    SMOOTH_ROT_THRESHOLD = 0.05  # rad: max allowed rotation jump per step
-    prev_smoothed_pose = None
-
     print("\n********** Policy Eval Episode Loop Start **********")
     print(f'relative: {relative}')
 
@@ -248,10 +220,8 @@ def eval_loop(cfg: EvalConfig, relative=False, rx_continuous=False, safety_guard
         prev_robot_dict = {}
         prev_action_dict = {}
 
-        is_multiple_robot = False
         if hasattr(cfg.robot, 'robots'):
             keys = cfg.robot.robots.keys()
-            is_multiple_robot = True
         else:
             keys = ['']
         for key in keys:
@@ -284,7 +254,6 @@ def eval_loop(cfg: EvalConfig, relative=False, rx_continuous=False, safety_guard
             obs = robot.get_observation()
 
             curr_robot_dict = {}
-            curr_action_dict = {}
             for key in keys:
                 prefix = f'.{key}' if key else ''
                 is_tcp = f'{prefix}pose.x' in obs and f'{prefix}pose.y' in obs and f'{prefix}pose.z' in obs and f'{prefix}pose.rx' in obs and f'{prefix}pose.ry' in obs and f'{prefix}pose.rz' in obs
@@ -348,67 +317,13 @@ def eval_loop(cfg: EvalConfig, relative=False, rx_continuous=False, safety_guard
 
                 # robot_action_to_send[f'{prefix}pose.z'] = max(robot_action_to_send[f'{prefix}pose.z'], 199)
 
-                # Rate-limited smoothing: cap position velocity to reduce chunk boundary jerks
-                # Uses vector-norm clamping to preserve motion direction
-                if SMOOTH_THRESHOLD > 0:
-                    pos_keys = [f'{prefix}pose.x', f'{prefix}pose.y', f'{prefix}pose.z']
-                    rot_keys = [f'{prefix}pose.rx', f'{prefix}pose.ry', f'{prefix}pose.rz']
-                    if prev_smoothed_pose is None:
-                        prev_smoothed_pose = {k: robot_action_to_send[k] for k in pos_keys + rot_keys}
-                    else:
-                        # Vector-norm clamp on position (preserves direction)
-                        delta_pos = np.array([robot_action_to_send[k] - prev_smoothed_pose[k] for k in pos_keys])
-                        norm = np.linalg.norm(delta_pos)
-                        if norm > SMOOTH_THRESHOLD:
-                            delta_pos = delta_pos * (SMOOTH_THRESHOLD / norm)
-                        for i, k in enumerate(pos_keys):
-                            prev_smoothed_pose[k] = prev_smoothed_pose[k] + delta_pos[i]
-                            robot_action_to_send[k] = prev_smoothed_pose[k]
-                        # Per-axis clamp on rotation
-                        for k in rot_keys:
-                            delta = robot_action_to_send[k] - prev_smoothed_pose[k]
-                            if abs(delta) > SMOOTH_ROT_THRESHOLD:
-                                delta = SMOOTH_ROT_THRESHOLD * (1 if delta > 0 else -1)
-                            prev_smoothed_pose[k] = prev_smoothed_pose[k] + delta
-                            robot_action_to_send[k] = prev_smoothed_pose[k]
-
                 if relative:
                     curr_action_pose = np.array([
                         robot_action_to_send[f'{prefix}pose.x'], robot_action_to_send[f'{prefix}pose.y'], robot_action_to_send[f'{prefix}pose.z'],
                         robot_action_to_send[f'{prefix}pose.rx'], robot_action_to_send[f'{prefix}pose.ry'], robot_action_to_send[f'{prefix}pose.rz']
                     ])
-                    # 相对增量模式: 漂移修正，将指令位姿温和拉回实际位姿
-                    # prev_action_pose = blend_poses(curr_action_pose, curr_robot_pose, 0.05)
                     prev_action_dict[key]['pose'] = curr_action_pose
 
-            # # Gripper look-ahead: peek ahead in the action queue to compensate
-            # # for the slow ramp in the ACT chunk (eliminates 1-2s gripper delay)
-            # gripper_raw = robot_action_to_send.get('left.gripper.pos', 0)
-            # if hasattr(policy, '_action_queue') and len(policy._action_queue) > 0:
-            #     # ACT: 队列为 deque of tensors, 归一化方式 MEAN_STD
-            #     lookahead_idx = min(GRIPPER_LOOKAHEAD, len(policy._action_queue) - 1)
-            #     future_gripper_norm = policy._action_queue[lookahead_idx][0, -1].item()
-            #     gripper_raw = future_gripper_norm * _gripper_std + _gripper_mean
-            # elif hasattr(policy, '_queues') and 'action' in policy._queues and len(policy._queues['action']) > 0:
-            #     # DP: 队列结构不同, 归一化方式 MIN_MAX → [-1,1] → [min,max]
-            #     lookahead_idx = min(GRIPPER_LOOKAHEAD, len(policy._queues['action']) - 1)
-            #     future_gripper_norm = policy._queues['action'][lookahead_idx][0, -1].item()
-            #     gripper_raw = (future_gripper_norm + 1) / 2 * (_gripper_max - _gripper_min) + _gripper_min
-            # robot_action_to_send['left.gripper.pos'] = 1.0 if gripper_raw > 0.4 else 0.0
-
-            # gripper_raw = robot_action_to_send.get('right.gripper.pos', 0)
-            # if hasattr(policy, '_action_queue') and len(policy._action_queue) > 0:
-            #     # ACT: 队列为 deque of tensors, 归一化方式 MEAN_STD
-            #     lookahead_idx = min(GRIPPER_LOOKAHEAD, len(policy._action_queue) - 1)
-            #     future_gripper_norm = policy._action_queue[lookahead_idx][0, -1].item()
-            #     gripper_raw = future_gripper_norm * _gripper_std + _gripper_mean
-            # elif hasattr(policy, '_queues') and 'action' in policy._queues and len(policy._queues['action']) > 0:
-            #     # DP: 队列结构不同, 归一化方式 MIN_MAX → [-1,1] → [min,max]
-            #     lookahead_idx = min(GRIPPER_LOOKAHEAD, len(policy._queues['action']) - 1)
-            #     future_gripper_norm = policy._queues['action'][lookahead_idx][0, -1].item()
-            #     gripper_raw = (future_gripper_norm + 1) / 2 * (_gripper_max - _gripper_min) + _gripper_min
-            # robot_action_to_send['right.gripper.pos'] = 1.0 if gripper_raw > 0.4 else 0.0
-
             # Safety check: any violation triggers an e-stop; the action is
             # NOT sent and the loop waits for the operator to press right arrow
             if safety_guard is not None:

src/lerobot_robot_ufactory/scripts/uf_lerobot_record.py

@@ -1,4 +1,5 @@
 import sys
+import csv
 import copy
 import time
 import queue
@@ -6,21 +7,26 @@ import argparse
 import logging
 import shutil
 import threading
-from dataclasses import dataclass
+from dataclasses import dataclass, field
 from pathlib import Path
 import lerobot_robot_ufactory # patch
 from lerobot.scripts.lerobot_record import *
 from lerobot.scripts.lerobot_record import RecordConfig as LeRobotRecordConfig
 from lerobot_robot_ufactory.teleoperators.uf_mock_teleop import UFMockTeleop
 from lerobot_robot_ufactory.teleoperators.base_teleop import UFBaseTeleop
+from lerobot_robot_ufactory.utils.realtime_teleop import RealtimeTeleopController
 from lerobot_robot_ufactory.utils.utils import init_keyboard_listener
+from lerobot_robot_ufactory.utils.web_preview import RecordingWebPreview, WebPreviewConfig
 
 
 @dataclass
 class UFRecordConfig(LeRobotRecordConfig):
     """RecordConfig variant that permits UFACTORY manual-mode recording."""
 
+    web_preview: WebPreviewConfig = field(default_factory=WebPreviewConfig)
+
     def __post_init__(self):
+        self.web_preview.validate()
         manual_mode = getattr(self.robot, "manual_mode", False)
         if manual_mode:
             if self.teleop is not None or self.policy is not None:
@@ -100,6 +106,18 @@ def _manual_gripper_action_key(action_features):
     return next((key for key in action_features if key.endswith("gripper.pos")), None)
 
 
+def _diagnostic_logs_enabled(robot) -> bool:
+    """Return whether optional per-cycle diagnostics are enabled for a robot."""
+    config = getattr(robot, "config", None)
+    if config is not None and hasattr(config, "enable_logs"):
+        return bool(config.enable_logs)
+
+    child_robots = getattr(robot, "robots", None)
+    if child_robots:
+        return any(_diagnostic_logs_enabled(child) for child in child_robots.values())
+    return False
+
+
 def _manual_action_from_observation(observation, action_features, gripper_target=None):
     """Keep only robot action fields when mirroring manual-mode state."""
     action = {key: value for key, value in observation.items() if key in action_features}
@@ -262,19 +280,24 @@ def _disconnect_recording_resources(robot, teleop, listener):
 
 
 class _RecordingCleanup:
-    def __init__(self, robot, teleop, listener, async_episode_saver):
+    def __init__(self, robot, teleop, listener, async_episode_saver, web_preview=None):
         self.robot = robot
         self.teleop = teleop
         self.listener = listener
         self.async_episode_saver = async_episode_saver
+        self.web_preview = web_preview
 
     def __enter__(self):
         return self
 
     def __exit__(self, exc_type, exc_value, traceback):
         try:
-            if self.async_episode_saver is not None:
-                self.async_episode_saver.close()
+            try:
+                if self.async_episode_saver is not None:
+                    self.async_episode_saver.close()
+            finally:
+                if self.web_preview is not None:
+                    self.web_preview.stop()
         finally:
             _disconnect_recording_resources(self.robot, self.teleop, self.listener)
         return False
@@ -307,6 +330,7 @@ def record_loop(
     manual_mode: bool = False,
     manual_gripper_keys: dict[str, bool] | None = None,
     manual_gripper_speed: float = 0.5,
+    web_preview: RecordingWebPreview | None = None,
 ):
     if dataset is not None and dataset.fps != fps:
         raise ValueError(f"The dataset fps should be equal to requested fps ({dataset.fps} != {fps}).")
@@ -350,20 +374,100 @@ def record_loop(
     manual_gripper_target = None
     manual_gripper_action_key = _manual_gripper_action_key(robot.action_features)
 
+    realtime_controller = None
+    diagnostic_logs_enabled = _diagnostic_logs_enabled(robot)
+    sync_log_file = None
+    sync_log_writer = None
+    sync_frame_index = 0
+    if (
+        policy is None
+        and isinstance(teleop, UFBaseTeleop)
+        and getattr(robot, "_control_space", None) == "joint"
+        and hasattr(robot, "get_realtime_observation")
+    ):
+        realtime_controller = RealtimeTeleopController(
+            robot=robot,
+            teleop=teleop,
+            teleop_action_processor=teleop_action_processor,
+            robot_action_processor=robot_action_processor,
+            fps=int(teleop.config.realtime_control_fps),
+            initial_observation=last_robot_cmd,
+        )
+        realtime_controller.start()
+        if diagnostic_logs_enabled:
+            sync_log_dir = Path("logs")
+            sync_log_dir.mkdir(parents=True, exist_ok=True)
+            sync_log_path = sync_log_dir / (
+                f"gello_record_sync_{time.strftime('%Y%m%d_%H%M%S')}_"
+                f"{time.time_ns() % 1_000_000:06d}.csv"
+            )
+            sync_log_file = sync_log_path.open("w", newline="", buffering=1)
+            sync_log_writer = csv.DictWriter(
+                sync_log_file,
+                fieldnames=[
+                    "frame",
+                    "state_sample_s",
+                    "action_sent_s",
+                    "action_age_ms",
+                    "observation_end_s",
+                    "state_to_observation_end_ms",
+                    "camera_timings",
+                    "preview_publish_ms",
+                    "preview_clients",
+                    "preview_source_generation",
+                    "preview_encoded_frames",
+                    "preview_last_encode_ms",
+                    "preview_max_encode_ms",
+                    "record_period_ms",
+                    "frame_loop_ms",
+                    "frame_budget_ms",
+                    "frame_overrun_ms",
+                ],
+            )
+            sync_log_writer.writeheader()
+            logging.info("Realtime dataset synchronization log: %s", sync_log_path)
+
     timestamp = 0
     start_episode_t = time.perf_counter()
+    previous_loop_start_t = None
     while timestamp < control_time_s:
         start_loop_t = time.perf_counter()
+        record_period_ms = (
+            0.0
+            if previous_loop_start_t is None
+            else (start_loop_t - previous_loop_start_t) * 1000
+        )
+        previous_loop_start_t = start_loop_t
 
         if events["exit_early"]:
             events["exit_early"] = False
             break
 
         # Get robot observation
-        obs = robot.get_observation()
+        if realtime_controller is not None:
+            obs = robot.get_realtime_observation()
+            observation_monotonic_s = getattr(robot, "_last_realtime_observation_monotonic_s", None)
+            if observation_monotonic_s is None:
+                observation_monotonic_s = time.perf_counter()
+            realtime_controller.update_observation(obs)
+            if diagnostic_logs_enabled:
+                matched_action, matched_action_sent_s = realtime_controller.action_sample_at(
+                    observation_monotonic_s
+                )
+            else:
+                matched_action = realtime_controller.action_at(observation_monotonic_s)
+        else:
+            obs = robot.get_observation()
 
         # Applies a pipeline to the raw robot observation, default is IdentityProcessor
         obs_processed = robot_observation_processor(obs)
+        preview_publish_ms = 0.0
+        if web_preview is not None:
+            # This only replaces references in a latest-frame slot. All image
+            # processing and network I/O remain on preview background threads.
+            before_preview_publish_t = time.perf_counter()
+            web_preview.publish(obs_processed)
+            preview_publish_ms = (time.perf_counter() - before_preview_publish_t) * 1000
 
         if policy is not None or dataset is not None:
             observation_frame = build_dataset_frame(dataset.features, obs_processed, prefix=OBS_STR)
@@ -407,15 +511,20 @@ def record_loop(
             act_processed_teleop = teleop_action_processor((act, obs))
 
         elif policy is None and isinstance(teleop, Teleoperator):
-            act = teleop.get_action()
+            if realtime_controller is not None:
+                act_processed_teleop = matched_action
+                act = None
+            else:
+                act = teleop.get_action()
 
             # (space mouse) from delta Cartesian cmd to absolute command
-            if "pose.dx" in act:
+            if act is not None and "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"]})
                 act = last_robot_cmd.copy() # watch out this is shallow copy, not for nested dict
 
             # Applies a pipeline to the raw teleop action, default is IdentityProcessor
-            act_processed_teleop = teleop_action_processor((act, obs))
+            if realtime_controller is None:
+                act_processed_teleop = teleop_action_processor((act, obs))
 
         elif policy is None and isinstance(teleop, list):
             arm_action = teleop_arm.get_action()
@@ -444,7 +553,14 @@ def record_loop(
         # Action can eventually be clipped using `max_relative_target`,
         # so action actually sent is saved in the dataset. action = postprocessor.process(action)
         # TODO(steven, pepijn, adil): we should use a pipeline step to clip the action, so the sent action is the action that we input to the robot.
-        _sent_action = robot.send_action(robot_action_to_send)
+        if realtime_controller is None:
+            _sent_action = robot.send_action(robot_action_to_send)
+        else:
+            _sent_action = matched_action
+        # Robots may clamp or otherwise sanitize a command before sending it.
+        # Store that effective command so demonstrations match the motion.
+        if isinstance(_sent_action, dict):
+            action_values = _sent_action
 
         # Write to dataset
         if dataset is not None:
@@ -454,6 +570,39 @@ def record_loop(
                 frame = frame_callback(frame)
             dataset.add_frame(frame)
 
+        if sync_log_writer is not None:
+            observation_end_s = getattr(
+                robot, "_last_realtime_observation_end_monotonic_s", observation_monotonic_s
+            )
+            camera_timings = getattr(robot, "_last_realtime_camera_timings", {})
+            preview_stats = web_preview.timing_stats() if web_preview is not None else {}
+            frame_loop_ms = (time.perf_counter() - start_loop_t) * 1000
+            frame_budget_ms = 1000 / fps
+            sync_log_writer.writerow(
+                {
+                    "frame": sync_frame_index,
+                    "state_sample_s": f"{observation_monotonic_s:.9f}",
+                    "action_sent_s": f"{matched_action_sent_s:.9f}",
+                    "action_age_ms": f"{(observation_monotonic_s - matched_action_sent_s) * 1000:.3f}",
+                    "observation_end_s": f"{observation_end_s:.9f}",
+                    "state_to_observation_end_ms": f"{(observation_end_s - observation_monotonic_s) * 1000:.3f}",
+                    "camera_timings": repr(camera_timings),
+                    "preview_publish_ms": f"{preview_publish_ms:.6f}",
+                    "preview_clients": preview_stats.get("preview_clients", 0),
+                    "preview_source_generation": preview_stats.get(
+                        "preview_source_generation", 0
+                    ),
+                    "preview_encoded_frames": preview_stats.get("preview_encoded_frames", 0),
+                    "preview_last_encode_ms": f'{preview_stats.get("preview_last_encode_ms", 0.0):.3f}',
+                    "preview_max_encode_ms": f'{preview_stats.get("preview_max_encode_ms", 0.0):.3f}',
+                    "record_period_ms": f"{record_period_ms:.3f}",
+                    "frame_loop_ms": f"{frame_loop_ms:.3f}",
+                    "frame_budget_ms": f"{frame_budget_ms:.3f}",
+                    "frame_overrun_ms": f"{max(0.0, frame_loop_ms - frame_budget_ms):.3f}",
+                }
+            )
+            sync_frame_index += 1
+
         if display_data:
             log_rerun_data(
                 observation=obs_processed, action=action_values, compress_images=display_compressed_images
@@ -464,6 +613,11 @@ def record_loop(
 
         timestamp = time.perf_counter() - start_episode_t
 
+    if realtime_controller is not None:
+        realtime_controller.stop()
+    if sync_log_file is not None:
+        sync_log_file.close()
+
 
 def _prepare_recording_episode(robot, teleop, is_uf_teleop, manual_mode):
     if is_uf_teleop:
@@ -478,7 +632,6 @@ def _prepare_recording_episode(robot, teleop, is_uf_teleop, manual_mode):
 
     if is_uf_teleop:
         obs = robot.get_observation()
-        teleop.reset_to_robot_observation(obs)
         teleop.set_teleop_enabled(True, obs)
 
 
@@ -653,12 +806,23 @@ def record(cfg: UFRecordConfig, async_save: bool = False) -> LeRobotDataset:
             },
         )
 
+    web_preview = None
     try:
         robot.connect()
         if teleop is not None:
             teleop.connect()
+        if cfg.web_preview.enabled:
+            web_preview = RecordingWebPreview(cfg.web_preview)
+            web_preview.start()
+            print(f"Camera web preview: {web_preview.url}")
+            if cfg.web_preview.host == "0.0.0.0":
+                print(
+                    f"From another machine: http://<recorder-ip>:{cfg.web_preview.port}/"
+                )
     except BaseException:
         try:
+            if web_preview is not None:
+                web_preview.stop()
             _disconnect_recording_resources(robot, teleop, None)
         except BaseException:
             logging.exception("Failed to clean up after recording device connection failure")
@@ -723,7 +887,9 @@ def record(cfg: UFRecordConfig, async_save: bool = False) -> LeRobotDataset:
     if async_episode_saver is not None:
         print('Async episode saving is enabled.')
 
-    with _RecordingCleanup(robot, teleop, listener, async_episode_saver), VideoEncodingManager(dataset):
+    with _RecordingCleanup(
+        robot, teleop, listener, async_episode_saver, web_preview
+    ), VideoEncodingManager(dataset):
         recorded_episodes = 0
         while recorded_episodes < cfg.dataset.num_episodes and not events["stop_recording"]:
             time.sleep(0.01)
@@ -769,6 +935,7 @@ def record(cfg: UFRecordConfig, async_save: bool = False) -> LeRobotDataset:
                     manual_mode=manual_mode,
                     manual_gripper_keys=manual_gripper_keys,
                     manual_gripper_speed=getattr(cfg.robot, "manual_gripper_speed", 0.5),
+                    web_preview=web_preview,
                 )
             else:
                 continue

src/lerobot_robot_ufactory/scripts/uf_read_tcp_z.py

@@ -0,0 +1,92 @@
+import argparse
+import math
+from pathlib import Path
+from typing import Callable
+
+import numpy as np
+import yaml
+from xarm.wrapper import XArmAPI
+
+
+def _load_robot_config(config_path: Path) -> tuple[str, int]:
+    with config_path.open(encoding="utf-8") as stream:
+        config = yaml.safe_load(stream)
+
+    robot_config = config.get("robot") if isinstance(config, dict) else None
+    if not isinstance(robot_config, dict):
+        raise ValueError(f"{config_path} does not contain a robot configuration")
+
+    robot_ip = robot_config.get("robot_ip")
+    robot_dof = robot_config.get("robot_dof")
+    if not isinstance(robot_ip, str) or not robot_ip:
+        raise ValueError(f"{config_path} does not define robot.robot_ip")
+    if robot_dof not in (5, 6, 7):
+        raise ValueError(f"{config_path} has invalid robot.robot_dof: {robot_dof}")
+    return robot_ip, int(robot_dof)
+
+
+def read_tcp_z(
+    config_path: Path,
+    margin_mm: float = 5.0,
+    arm_factory: Callable[[str], object] = XArmAPI,
+) -> tuple[float, float]:
+    """Read the current TCP z using the same FK API as the runtime guard."""
+    if not math.isfinite(margin_mm) or margin_mm < 0:
+        raise ValueError("margin-mm must be a finite, non-negative number")
+
+    robot_ip, robot_dof = _load_robot_config(config_path)
+    arm = arm_factory(robot_ip)
+    try:
+        if not arm.connected:
+            raise ConnectionError(f"Unable to connect to xArm at {robot_ip}")
+        if arm.axis != robot_dof:
+            raise RuntimeError(
+                f"Connected xArm has {arm.axis} axes, but config specifies {robot_dof}"
+            )
+
+        code, states = arm.get_joint_states(is_radian=True, num=1)
+        if code != 0 or not states or len(states[0]) < robot_dof:
+            raise RuntimeError(f"get_joint_states failed, code={code}")
+        joints = np.asarray(states[0][:robot_dof], dtype=np.float64)
+        if not np.all(np.isfinite(joints)):
+            raise RuntimeError("get_joint_states returned NaN/Inf")
+
+        code, pose = arm.get_forward_kinematics(
+            joints.tolist(), input_is_radian=True, return_is_radian=True
+        )
+        pose = np.asarray(pose, dtype=np.float64)
+        if code != 0 or pose.shape[0] < 3 or not np.all(np.isfinite(pose)):
+            raise RuntimeError(f"get_forward_kinematics failed, code={code}")
+
+        tcp_z_mm = float(pose[2])
+        return tcp_z_mm, tcp_z_mm + margin_mm
+    finally:
+        arm.disconnect()
+
+
+def main() -> None:
+    parser = argparse.ArgumentParser(
+        description="Read the current xArm TCP z without moving the robot."
+    )
+    parser.add_argument(
+        "--config-path",
+        type=Path,
+        required=True,
+        help="GELLO YAML configuration containing robot.robot_ip and robot.robot_dof",
+    )
+    parser.add_argument(
+        "--margin-mm",
+        type=float,
+        default=5.0,
+        help="safety margin added to the measured z (default: 5 mm)",
+    )
+    args = parser.parse_args()
+
+    tcp_z_mm, recommended_mm = read_tcp_z(args.config_path, args.margin_mm)
+    print(f"Current TCP z: {tcp_z_mm:.3f} mm")
+    print(f"Safety margin: {args.margin_mm:.3f} mm")
+    print(f"Recommended YAML value: min_tcp_z_mm: {recommended_mm:.3f}")
+
+
+if __name__ == "__main__":
+    main()

src/lerobot_robot_ufactory/scripts/uf_robot_teleop.py

@@ -1,12 +1,15 @@
 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
 from lerobot.processor import (
     make_default_processors,
 )
@@ -26,6 +29,7 @@ from lerobot.utils.utils import (
 from lerobot_robot_ufactory.configs import parser
 from lerobot_robot_ufactory.utils.utils import is_headless, init_keyboard_listener
 from lerobot_robot_ufactory.teleoperators.base_teleop import UFBaseTeleop
+from lerobot_robot_ufactory.utils.realtime_teleop import RealtimeTeleopController
 
 
 @dataclass
@@ -33,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 = {}
@@ -42,10 +53,124 @@ 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)
+        if hasattr(cfg.robot, "enable_logs") and not cfg.robot.enable_logs:
+            raise ValueError(
+                "Guard latency experiment requires robot.enable_logs=true"
+            )
+        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
@@ -53,8 +178,37 @@ def teleop_loop(cfg: TeleopConfig):
 
     teleop_action_processor, robot_action_processor, robot_observation_processor = make_default_processors()
 
+    robot_connected = False
+    teleop_connected = False
+    listener = None
+    cleanup_done = False
+
+    def cleanup_connections():
+        nonlocal cleanup_done
+        if cleanup_done:
+            return
+        cleanup_done = True
+        if teleop_connected:
+            try:
+                teleop.disconnect()
+            except Exception:
+                logging.exception("Failed to disconnect teleoperator cleanly")
+        if robot_connected:
+            try:
+                robot.disconnect()
+            except Exception:
+                logging.exception("Failed to disconnect robot cleanly")
+        if listener is not None:
+            try:
+                listener.stop()
+            except Exception:
+                logging.exception("Failed to stop keyboard listener cleanly")
+
+    atexit.register(cleanup_connections)
     robot.connect()
+    robot_connected = True
     teleop.connect()
+    teleop_connected = True
 
     sleep_time_s = 1 / cfg.fps
 
@@ -71,13 +225,11 @@ def teleop_loop(cfg: TeleopConfig):
         reset()
         if is_uf_teleop:
             obs = robot.get_observation()
-            teleop.reset_to_robot_observation(obs)
             teleop.set_teleop_enabled(True, obs)
 
     is_reset = is_uf_teleop
     is_paused = True
     events = {"exit": False}
-    listener = None
     key_dict = {}
 
     if is_evt:
@@ -132,6 +284,39 @@ 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
+    realtime_controller = None
+    realtime_control_fps = int(teleop.config.realtime_control_fps)
+
+    def start_realtime_controller():
+        nonlocal realtime_controller
+        if (
+            cfg.guard_latency_experiment
+            or not is_uf_teleop
+            or getattr(robot, "_control_space", None) != "joint"
+        ):
+            return
+        obs = robot.get_realtime_observation()
+        realtime_controller = RealtimeTeleopController(
+            robot,
+            teleop,
+            teleop_action_processor,
+            robot_action_processor,
+            realtime_control_fps,
+            obs,
+        )
+        realtime_controller.start()
+
+    def stop_realtime_controller():
+        nonlocal realtime_controller
+        if realtime_controller is not None:
+            realtime_controller.stop()
+            realtime_controller = None
+
+    if not is_evt and not is_paused:
+        start_realtime_controller()
 
     while not events["exit"]:
         start_loop_t = time.perf_counter()
@@ -142,6 +327,7 @@ def teleop_loop(cfg: TeleopConfig):
                 is_reset = True
                 if not is_paused:
                     is_paused = True
+                    stop_realtime_controller()
                     if is_uf_teleop:
                         teleop.set_teleop_enabled(False)
                 print('⌨   [ESC] Exit  [Space] Reset / Start  [←] Reset')
@@ -152,6 +338,7 @@ def teleop_loop(cfg: TeleopConfig):
                 key_space_pressed = True
                 is_paused = not is_paused
                 if is_paused:
+                    stop_realtime_controller()
                     if is_uf_teleop:
                         teleop.set_teleop_enabled(False)
                     # print('========== Teleop is paused ==========')
@@ -164,6 +351,7 @@ def teleop_loop(cfg: TeleopConfig):
                     elif is_uf_teleop:
                         obs = robot.get_observation()
                         teleop.set_teleop_enabled(True, obs)
+                    start_realtime_controller()
                     print('⌨   [ESC] Exit  [Space] Pause  [←] Reset')
                 continue
             elif not key_dict[keyboard.Key.space] and key_space_pressed:
@@ -172,23 +360,63 @@ def teleop_loop(cfg: TeleopConfig):
             if is_reset or is_paused:
                 continue
 
-        # Get robot observation
-        obs = robot.get_observation()
+        if cfg.guard_latency_experiment:
+            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()
-        act_processed_teleop = teleop_action_processor((act, obs))
-
-        robot_action_to_send = robot_action_processor((act_processed_teleop, obs))
-        robot.send_action(robot_action_to_send)
-
-        dt_s = time.perf_counter() - start_loop_t
-        precise_sleep(sleep_time_s - dt_s)
+            read_start_t = time.perf_counter()
+            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:
+            if realtime_controller is not None:
+                realtime_controller.heartbeat()
+                realtime_controller.raise_if_failed()
+                precise_sleep(sleep_time_s)
+            else:
+                # Generic non-UFACTORY teleoperators retain the standard loop.
+                obs = robot.get_observation()
+                act = teleop.get_action()
+                act_processed_teleop = teleop_action_processor((act, obs))
+                robot_action_to_send = robot_action_processor((act_processed_teleop, obs))
+                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 **********")
-    robot.disconnect()
-    teleop.disconnect()
-    if is_evt and listener is not None:
-        listener.stop()
+    stop_realtime_controller()
+    if latency_samples:
+        output_path = _write_guard_latency_timings(
+            latency_samples, cfg.timing_log_dir, realtime_control_fps
+        )
+        print(f"Guard latency timing log: {output_path}")
+    cleanup_connections()
+    atexit.unregister(cleanup_connections)
 
 @parser.wrap()
 def get_cfg(cfg: TeleopConfig) -> TeleopConfig:

src/lerobot_robot_ufactory/teleoperators/gello_teleop/gello_adapter.py

@@ -0,0 +1,154 @@
+from __future__ import annotations
+
+from dataclasses import dataclass
+from typing import Optional, Sequence, Tuple
+
+import numpy as np
+from dynamixel_sdk import COMM_SUCCESS
+from dynamixel_sdk.robotis_def import (
+    DXL_HIBYTE,
+    DXL_HIWORD,
+    DXL_LOBYTE,
+    DXL_LOWORD,
+)
+from gello.dynamixel import driver as driver_module
+from gello.dynamixel.driver import DynamixelDriver
+from gello.robots.dynamixel import DynamixelRobot
+
+
+class SafeDynamixelDriver(DynamixelDriver):
+    """GELLO driver with serialized writes and complete torque cleanup."""
+
+    def set_joints(self, joint_angles: Sequence[float]) -> None:
+        if len(joint_angles) != len(self._ids):
+            raise ValueError("joint_angles must match the configured Dynamixel IDs")
+        if not self._torque_enabled:
+            raise RuntimeError("Torque must be enabled to set joint angles")
+        if self._is_fake:
+            self._fake_joint_angles = np.asarray(joint_angles, dtype=float)
+            return
+
+        with self._lock:
+            try:
+                for dxl_id, angle in zip(self._ids, joint_angles, strict=True):
+                    position_value = int(angle * 2048 / np.pi)
+                    parameter = [
+                        DXL_LOBYTE(DXL_LOWORD(position_value)),
+                        DXL_HIBYTE(DXL_LOWORD(position_value)),
+                        DXL_LOBYTE(DXL_HIWORD(position_value)),
+                        DXL_HIBYTE(DXL_HIWORD(position_value)),
+                    ]
+                    if not self._groupSyncWrite.addParam(dxl_id, parameter):
+                        raise RuntimeError(
+                            f"Failed to set joint angle for Dynamixel ID {dxl_id}"
+                        )
+
+                result = self._groupSyncWrite.txPacket()
+                if result != COMM_SUCCESS:
+                    detail = self._packetHandler.getTxRxResult(result)
+                    raise RuntimeError(
+                        f"Failed to syncwrite goal position: {detail} ({result})"
+                    )
+            finally:
+                self._groupSyncWrite.clearParam()
+
+    def set_torque_mode(self, enable: bool) -> None:
+        if self._is_fake:
+            self._torque_enabled = enable
+            return
+
+        torque_value = driver_module.TORQUE_ENABLE if enable else driver_module.TORQUE_DISABLE
+        failures = []
+        with self._lock:
+            for dxl_id in self._ids:
+                result, error = self._packetHandler.write1ByteTxRx(
+                    self._portHandler,
+                    dxl_id,
+                    driver_module.ADDR_TORQUE_ENABLE,
+                    torque_value,
+                )
+                if result != COMM_SUCCESS:
+                    detail = self._packetHandler.getTxRxResult(result)
+                    failures.append(f"ID {dxl_id}: {detail} ({result})")
+                    continue
+                if error == 0:
+                    continue
+
+                if not enable:
+                    state, read_result, _ = self._packetHandler.read1ByteTxRx(
+                        self._portHandler,
+                        dxl_id,
+                        driver_module.ADDR_TORQUE_ENABLE,
+                    )
+                    if read_result == COMM_SUCCESS and state == driver_module.TORQUE_DISABLE:
+                        continue
+
+                detail = self._packetHandler.getRxPacketError(error)
+                failures.append(f"ID {dxl_id}: {detail} ({error})")
+
+        if failures:
+            raise RuntimeError("Failed to set torque mode: " + "; ".join(failures))
+        self._torque_enabled = enable
+
+
+class ContinuousDynamixelRobot(DynamixelRobot):
+    """Dynamixel GELLO whose arm joints remain continuous across encoder wrap."""
+
+    def get_joint_state(self) -> np.ndarray:
+        pos = (self._driver.get_joints() - self._joint_offsets) * self._joint_signs
+        if len(pos) != self.num_dofs():
+            raise RuntimeError("Unexpected Dynamixel joint count")
+
+        arm_dofs = len(pos) - 1 if self.gripper_open_close is not None else len(pos)
+        if self._last_pos is not None:
+            pos[:arm_dofs] += 2 * np.pi * np.round(
+                (self._last_pos[:arm_dofs] - pos[:arm_dofs]) / (2 * np.pi)
+            )
+
+        if self.gripper_open_close is not None:
+            gripper_open, gripper_close = self.gripper_open_close
+            gripper_pos = (pos[-1] - gripper_open) / (gripper_close - gripper_open)
+            pos[-1] = min(max(0.0, gripper_pos), 1.0)
+
+        if self._last_pos is None:
+            self._last_pos = pos
+        else:
+            pos = self._last_pos * (1 - self._alpha) + pos * self._alpha
+            self._last_pos = pos
+        return pos
+
+
+@dataclass
+class PatchedDynamixelRobotConfig:
+    joint_ids: Sequence[int]
+    joint_offsets: Sequence[float]
+    joint_signs: Sequence[int]
+    gripper_config: Optional[Tuple[int, float, float]]
+
+    def __post_init__(self) -> None:
+        if len(self.joint_ids) != len(self.joint_offsets):
+            raise ValueError("joint_ids and joint_offsets must have the same length")
+        if len(self.joint_ids) != len(self.joint_signs):
+            raise ValueError("joint_ids and joint_signs must have the same length")
+
+    def make_robot(
+        self,
+        port: str = "/dev/ttyUSB0",
+        start_joints: Optional[np.ndarray] = None,
+    ) -> ContinuousDynamixelRobot:
+        # Upstream DynamixelRobot imports its driver inside __init__. Replace
+        # that symbol only while constructing this instance.
+        original_driver = driver_module.DynamixelDriver
+        driver_module.DynamixelDriver = SafeDynamixelDriver
+        try:
+            return ContinuousDynamixelRobot(
+                joint_ids=self.joint_ids,
+                joint_offsets=self.joint_offsets,
+                joint_signs=self.joint_signs,
+                real=True,
+                port=port,
+                gripper_config=self.gripper_config,
+                start_joints=start_joints,
+            )
+        finally:
+            driver_module.DynamixelDriver = original_driver

src/lerobot_robot_ufactory/teleoperators/gello_teleop/gello_teleop.py

@@ -1,20 +1,14 @@
 #!/usr/bin/env python
 import logging
-import time
-import math
 import numpy as np
 from lerobot.utils.errors import DeviceAlreadyConnectedError, DeviceNotConnectedError
 from ..base_teleop import UFBaseTeleop
+from .gello_adapter import PatchedDynamixelRobotConfig
 from .gello_teleop_config import GelloTeleopConfig
 
 
 logger = logging.getLogger(__name__)
 
-GELLO_RESET_SPEED_DEG = 30.0
-GELLO_RESET_TOLERANCE_DEG = 2.0
-GELLO_RESET_CONTROL_HZ = 50.0
-GELLO_RESET_TIMEOUT_MARGIN_S = 5.0
-
 class GelloTeleop(UFBaseTeleop):
     """
     GELLO for xArm tele-op, ref: https://wuphilipp.github.io/gello_site/
@@ -31,26 +25,22 @@ class GelloTeleop(UFBaseTeleop):
         self._needs_alignment = True
         self._is_calibrated = True # CHECK!!
 
-        from gello.dynamixel.driver import DynamixelDriver
-        from gello.agents.gello_agent import DynamixelRobotConfig
-
-        # auto get joint offset from gello
-        joint_ids = []
-        joint_ids.extend(self.config.joint_ids)
+        joint_offsets = [0.0] * len(self.config.joint_ids)
+        self._align_gripper_to_current = self.config.gripper_open_deg is None
         if self.config.gripper_id >= 0:
-            joint_ids.append(self.config.gripper_id)
-        driver = DynamixelDriver(joint_ids, port=self.config.port, baudrate=57600)
-        for _ in range(10):
-            driver.get_joints()  # warmup
-        curr_joints = driver.get_joints()
-        driver.close()
-        joint_offsets = []
-        start_joints = list(map(math.radians, self.config.start_joints))
-        for i in range(len(start_joints)):
-            offset = curr_joints[i] - start_joints[i] / self.config.joint_signs[i]
-            joint_offsets.append(offset)
-        if self.config.gripper_id >= 0:
-            gripper_config = [self.config.gripper_id, np.rad2deg(curr_joints[-1]) - 0.2, np.rad2deg(curr_joints[-1]) - 42]
+            if self.config.gripper_open_deg is not None:
+                gripper_open_deg = self.config.gripper_open_deg
+                gripper_close_deg = self.config.gripper_close_deg
+            else:
+                # Only the range matters. It is shifted to the current GELLO
+                # gripper position whenever teleoperation is enabled.
+                gripper_open_deg = 0.0
+                gripper_close_deg = -42.0
+            gripper_config = [
+                self.config.gripper_id,
+                gripper_open_deg,
+                gripper_close_deg,
+            ]
         else:
             gripper_config = None
 
@@ -60,9 +50,9 @@ class GelloTeleop(UFBaseTeleop):
                 "joint_offsets": joint_offsets,
                 "gripper_config": gripper_config
         }
-        self._dynamixel_robo_config = DynamixelRobotConfig(**param_dict)
+        self._dynamixel_robo_config = PatchedDynamixelRobotConfig(**param_dict)
         print(self._dynamixel_robo_config)
-        self.dof = len(start_joints)
+        self.dof = len(self.config.joint_ids)
 
     @property
     def action_features(self) -> dict:
@@ -126,67 +116,49 @@ class GelloTeleop(UFBaseTeleop):
         pass
 
     def reset_to_robot_observation(self, obs):
-        """Move the physical Gello to the robot's post-reset joint state."""
+        """Map the current passive GELLO pose to the robot's current pose."""
         if not self._is_connected:
             raise DeviceNotConnectedError("Gello teleop is not connected")
 
         self._teleop_enabled = False
         gello_robot = self.gello_agent._robot
         driver = gello_robot._driver
+        gello_robot.set_torque_mode(False)
         current_raw = np.asarray(driver.get_joints(), dtype=float)
-        target_raw = current_raw.copy()
         signs = np.asarray(gello_robot._joint_signs, dtype=float)
-        offsets = np.asarray(gello_robot._joint_offsets, dtype=float)
 
-        target_robot_joints = np.asarray(
+        robot_joints = np.asarray(
             [obs[f"J{i + 1}.pos"] for i in range(self.dof)], dtype=float
         )
-        target_raw[: self.dof] = target_robot_joints * signs[: self.dof] + offsets[: self.dof]
+        gello_robot._joint_offsets[: self.dof] = (
+            current_raw[: self.dof] - robot_joints * signs[: self.dof]
+        )
 
-        if gello_robot.gripper_open_close is not None and len(target_raw) > self.dof:
+        if (
+            self._align_gripper_to_current
+            and gello_robot.gripper_open_close is not None
+            and len(current_raw) > self.dof
+        ):
             gripper_pos = float(obs.get("gripper.pos", 0.0))
             gripper_open, gripper_close = gello_robot.gripper_open_close
             gripper_pos = min(max(gripper_pos, 0.0), 1.0)
-            target_raw[self.dof] = gripper_open + gripper_pos * (gripper_close - gripper_open)
-
-        arm_delta = np.max(np.abs(target_raw[: self.dof] - current_raw[: self.dof]))
-        reset_speed_rad_s = math.radians(GELLO_RESET_SPEED_DEG)
-        duration_s = max(0.5, float(arm_delta / reset_speed_rad_s))
-        deadline = time.perf_counter() + duration_s + GELLO_RESET_TIMEOUT_MARGIN_S
-        success = False
-
-        try:
-            gello_robot.set_torque_mode(True)
-            start_t = time.perf_counter()
-            while True:
-                elapsed_s = time.perf_counter() - start_t
-                progress = min(elapsed_s / duration_s, 1.0)
-                command = current_raw + (target_raw - current_raw) * progress
-                driver.set_joints(command.tolist())
-                if progress >= 1.0:
-                    break
-                time.sleep(1.0 / GELLO_RESET_CONTROL_HZ)
-
-            while time.perf_counter() < deadline:
-                measured_raw = np.asarray(driver.get_joints(), dtype=float)
-                if np.max(np.abs(measured_raw - target_raw)) <= math.radians(GELLO_RESET_TOLERANCE_DEG):
-                    success = True
-                    break
-                driver.set_joints(target_raw.tolist())
-                time.sleep(1.0 / GELLO_RESET_CONTROL_HZ)
-
-            if not success:
-                raise RuntimeError("Gello did not reach the robot initial point before timeout")
-        finally:
-            gello_robot.set_torque_mode(False)
-            gello_robot._last_pos = None
+            gripper_span = gripper_close - gripper_open
+            gripper_open = current_raw[self.dof] - gripper_pos * gripper_span
+            gello_robot.gripper_open_close = (
+                gripper_open,
+                gripper_open + gripper_span,
+            )
 
+        gello_robot._last_pos = None
         self._needs_alignment = False
+        logger.info("Current GELLO pose aligned to current robot observation")
 
     def set_teleop_enabled(self, enabled: bool, obs=None):
         if enabled and not self._is_connected:
             raise DeviceNotConnectedError("Gello teleop is not connected")
-        if enabled and self._needs_alignment and obs is not None:
+        if enabled and self._needs_alignment:
+            if obs is None:
+                raise ValueError("Robot observation is required to enable GELLO teleoperation")
             self.reset_to_robot_observation(obs)
         if not enabled and self._is_connected and hasattr(self, "gello_agent"):
             self.gello_agent._robot.set_torque_mode(False)
@@ -197,11 +169,8 @@ class GelloTeleop(UFBaseTeleop):
     def get_action(self) -> dict[str, np.ndarray]:
         if not self._teleop_enabled:
             raise RuntimeError("Gello teleop is disabled")
-        start = time.perf_counter()
         fake_obs = dict({"joint_state": np.array([0.0]*(self.dof+1))}) # for agent.act() argument, actually no use
         action_array = self.gello_agent.act(fake_obs) # current gello joint pos as np.ndarray
-        dt_ms = (time.perf_counter() - start) * 1e3
-        logger.debug(f"{self} read action: {dt_ms:.1f}ms")
 
         action = {}
         for i in range(self.dof):

src/lerobot_robot_ufactory/teleoperators/gello_teleop/gello_teleop_config.py

@@ -1,23 +1,41 @@
 #!/usr/bin/env python
 
 from dataclasses import dataclass
-from typing import Tuple
+from typing import Optional, Tuple
 from lerobot.teleoperators import TeleoperatorConfig
 
 
 @TeleoperatorConfig.register_subclass("uf::gello_teleop")
 @dataclass
 class GelloTeleopConfig(TeleoperatorConfig):
+    # Frequency of the independent GELLO -> xArm realtime control loop.
+    realtime_control_fps: int = 60
     # Port to connect to the gello dummy arm
     port: str = "/dev/serial/by-id/usb-FTDI_USB__-__Serial_Converter_FTAJZYC7-if00-port0"
 
     # Others: Calibration angles, joint directions etc
     joint_ids: Tuple[int, ...] = (1, 2, 3, 4, 5, 6, 7)
     joint_signs: Tuple[int, ...] = (1, 1, 1, 1, 1, 1, 1) # if follow the original open-sourced gello xarm7 setup
-    # GELLO encoder calibration reference; this is not the xArm reset target.
+    # Accepted for compatibility but ignored: arm zero offsets are captured
+    # from the current GELLO and xArm poses whenever teleoperation is enabled.
+    joint_offsets: Optional[Tuple[float, ...]] = None
+    # Retained for compatibility with existing xArm5/xArm6 YAML files. GELLO
+    # alignment now always uses its current pose when teleoperation is enabled.
     start_joints: Tuple[float, ...] = (0, 0, 0, 90, 0, 90, 0)  # °
     gripper_id: int = 8  # -1: no gripper
-    torque_joint_ids: Tuple[int, ...] = None  # deprecated; reset controls all GELLO joints.
+    gripper_open_deg: Optional[float] = None
+    gripper_close_deg: Optional[float] = None
+    torque_joint_ids: Tuple[int, ...] = None  # deprecated
 
     def __post_init__(self):
         self.id = 'gello_teleop' if self.id is None else self.id
+        if self.realtime_control_fps <= 0:
+            raise ValueError("realtime_control_fps must be positive")
+        if len(self.joint_ids) != len(self.joint_signs):
+            raise ValueError("joint_ids and joint_signs must have the same length")
+        if len(self.joint_ids) != len(self.start_joints):
+            raise ValueError("joint_ids and start_joints must have the same length")
+        if self.joint_offsets is not None and len(self.joint_ids) != len(self.joint_offsets):
+            raise ValueError("joint_ids and joint_offsets must have the same length")
+        if (self.gripper_open_deg is None) != (self.gripper_close_deg is None):
+            raise ValueError("gripper_open_deg and gripper_close_deg must be set together")

src/lerobot_robot_ufactory/utils/realtime_teleop.py

@@ -0,0 +1,118 @@
+"""Fixed-rate UFACTORY teleoperation isolated from observation and recording work."""
+
+from __future__ import annotations
+
+import threading
+import time
+from collections import deque
+
+from lerobot.utils.robot_utils import precise_sleep
+
+
+class RealtimeTeleopController:
+    def __init__(
+        self,
+        robot,
+        teleop,
+        teleop_action_processor,
+        robot_action_processor,
+        fps: int,
+        initial_observation: dict,
+    ) -> None:
+        self.robot = robot
+        self.teleop = teleop
+        self.teleop_action_processor = teleop_action_processor
+        self.robot_action_processor = robot_action_processor
+        self.period_s = 1.0 / fps
+        self._observation = initial_observation
+        self._latest_action = None
+        self._action_history = deque(maxlen=max(16, fps * 2))
+        self._exception = None
+        self._heartbeat = time.perf_counter()
+        self._lock = threading.Lock()
+        self._stop = threading.Event()
+        self._first_action = threading.Event()
+        self._thread = threading.Thread(target=self._run, name="uf-servoj-control", daemon=True)
+
+    def start(self) -> None:
+        self._thread.start()
+        if not self._first_action.wait(timeout=2.0):
+            self.raise_if_failed()
+            raise RuntimeError("Timed out waiting for the first realtime ServoJ action")
+        self.raise_if_failed()
+
+    def stop(self) -> None:
+        self._stop.set()
+        if self._thread.is_alive():
+            self._thread.join(timeout=2.0)
+        self.raise_if_failed()
+
+    def update_observation(self, observation: dict) -> None:
+        with self._lock:
+            self._observation = observation
+            self._heartbeat = time.perf_counter()
+
+    def heartbeat(self) -> None:
+        with self._lock:
+            self._heartbeat = time.perf_counter()
+
+    def latest_action(self) -> dict:
+        self.raise_if_failed()
+        with self._lock:
+            if self._latest_action is None:
+                raise RuntimeError("Realtime controller has not sent an action")
+            return dict(self._latest_action)
+
+    def action_at(self, monotonic_s: float) -> dict:
+        """Return the command active at a sampled observation time."""
+        action, _ = self.action_sample_at(monotonic_s)
+        return action
+
+    def action_sample_at(self, monotonic_s: float) -> tuple[dict, float]:
+        """Return the command and send timestamp active at observation time."""
+        self.raise_if_failed()
+        with self._lock:
+            if not self._action_history:
+                raise RuntimeError("Realtime controller has not sent an action")
+            selected_time, selected = self._action_history[0]
+            for sent_at_s, action in reversed(self._action_history):
+                if sent_at_s <= monotonic_s:
+                    selected_time = sent_at_s
+                    selected = action
+                    break
+            return dict(selected), selected_time
+
+    def raise_if_failed(self) -> None:
+        if self._exception is not None:
+            raise RuntimeError("Realtime ServoJ control thread failed") from self._exception
+
+    def _run(self) -> None:
+        next_tick = time.perf_counter()
+        try:
+            while not self._stop.is_set():
+                with self._lock:
+                    observation = self._observation
+                    heartbeat = self._heartbeat
+                if time.perf_counter() - heartbeat > 1.0:
+                    raise RuntimeError("Recording/teleop owner heartbeat timed out")
+                action = self.teleop.get_action()
+                processed = self.teleop_action_processor((action, observation))
+                command = self.robot_action_processor((processed, observation))
+                sent = self.robot.send_action(command)
+                effective = sent if isinstance(sent, dict) else command
+                sent_at_s = time.perf_counter()
+                with self._lock:
+                    self._latest_action = dict(effective)
+                    self._action_history.append((sent_at_s, dict(effective)))
+                self._first_action.set()
+
+                next_tick += self.period_s
+                now = time.perf_counter()
+                if next_tick <= now:
+                    missed = int((now - next_tick) / self.period_s) + 1
+                    next_tick += missed * self.period_s
+                precise_sleep(max(next_tick - time.perf_counter(), 0.0))
+        except BaseException as exc:
+            self._exception = exc
+            self._first_action.set()
+            self._stop.set()

src/lerobot_robot_ufactory/utils/web_preview.py

@@ -0,0 +1,332 @@
+"""Best-effort web preview for frames already captured by a recording loop.
+
+The recorder only publishes references to its latest frames.  Resizing, JPEG
+encoding, and network writes happen on background threads, and preview frames
+are deliberately dropped whenever those threads cannot keep up.
+"""
+
+from __future__ import annotations
+
+import http.server
+import json
+import logging
+import threading
+import time
+from dataclasses import dataclass
+from typing import Any
+from urllib.parse import parse_qs, urlsplit
+
+import cv2
+import numpy as np
+
+
+logger = logging.getLogger(__name__)
+
+
+@dataclass
+class WebPreviewConfig:
+    """Configuration for the optional recording-time camera preview."""
+
+    enabled: bool = False
+    host: str = "0.0.0.0"
+    port: int = 8765
+    fps: float = 8.0
+    width: int = 480
+    jpeg_quality: int = 65
+
+    def validate(self) -> None:
+        if not self.host:
+            raise ValueError("web_preview.host must not be empty")
+        if not 1 <= self.port <= 65535:
+            raise ValueError("web_preview.port must be between 1 and 65535")
+        if self.fps <= 0:
+            raise ValueError("web_preview.fps must be positive")
+        if self.width <= 0:
+            raise ValueError("web_preview.width must be positive")
+        if not 1 <= self.jpeg_quality <= 100:
+            raise ValueError("web_preview.jpeg_quality must be between 1 and 100")
+
+
+class _PreviewFeed:
+    def __init__(self, name: str) -> None:
+        self.name = name
+        self.condition = threading.Condition()
+        self.jpeg: bytes | None = None
+        self.frame_id = 0
+        self.clients = 0
+        self.stopped = False
+
+    def add_client(self) -> None:
+        with self.condition:
+            self.clients += 1
+
+    def remove_client(self) -> None:
+        with self.condition:
+            self.clients = max(0, self.clients - 1)
+
+    def publish_jpeg(self, jpeg: bytes) -> None:
+        with self.condition:
+            self.jpeg = jpeg
+            self.frame_id += 1
+            self.condition.notify_all()
+
+    def wait_for_jpeg(self, previous_id: int) -> tuple[bytes | None, int, bool]:
+        with self.condition:
+            self.condition.wait_for(
+                lambda: self.frame_id > previous_id or self.stopped,
+                timeout=1.0,
+            )
+            return self.jpeg, self.frame_id, self.stopped
+
+    def stop(self) -> None:
+        with self.condition:
+            self.stopped = True
+            self.condition.notify_all()
+
+
+class RecordingWebPreview:
+    """Serve a lossy, asynchronous preview of recording observations."""
+
+    def __init__(self, config: WebPreviewConfig) -> None:
+        config.validate()
+        self.config = config
+        self._condition = threading.Condition()
+        self._latest_frames: dict[str, np.ndarray] = {}
+        self._source_generation = 0
+        self._encoded_frames = 0
+        self._last_encode_ms = 0.0
+        self._max_encode_ms = 0.0
+        self._stop_event = threading.Event()
+        self._feeds: dict[str, _PreviewFeed] = {}
+        self._encoder_thread: threading.Thread | None = None
+        self._server: http.server.ThreadingHTTPServer | None = None
+        self._server_thread: threading.Thread | None = None
+
+    @property
+    def url(self) -> str:
+        host = "127.0.0.1" if self.config.host == "0.0.0.0" else self.config.host
+        return f"http://{host}:{self.config.port}/"
+
+    def start(self) -> None:
+        if self._server is not None:
+            return
+        handler = type("RecordingPreviewHandler", (_PreviewHandler,), {"preview": self})
+        self._server = http.server.ThreadingHTTPServer(
+            (self.config.host, self.config.port), handler
+        )
+        self._server.daemon_threads = True
+        self._encoder_thread = threading.Thread(
+            target=self._encode_loop,
+            name="uf-recording-preview-encoder",
+            daemon=True,
+        )
+        self._server_thread = threading.Thread(
+            target=self._server.serve_forever,
+            name="uf-recording-preview-http",
+            daemon=True,
+        )
+        self._encoder_thread.start()
+        self._server_thread.start()
+
+    def publish(self, observation: dict[str, Any]) -> None:
+        """Non-blockingly replace the latest previewable image references."""
+        frames = {
+            key: value
+            for key, value in observation.items()
+            if isinstance(value, np.ndarray) and value.ndim == 3 and value.shape[2] in (3, 4)
+        }
+        if not frames:
+            return
+        with self._condition:
+            self._latest_frames = frames
+            for name in frames:
+                self._feeds.setdefault(name, _PreviewFeed(name))
+            self._source_generation += 1
+            self._condition.notify()
+
+    def camera_names(self) -> list[str]:
+        with self._condition:
+            return sorted(self._feeds)
+
+    def feed(self, name: str | None) -> _PreviewFeed | None:
+        if name is None:
+            return None
+        with self._condition:
+            return self._feeds.get(name)
+
+    def timing_stats(self) -> dict[str, float | int]:
+        """Return a cheap snapshot for recording synchronization diagnostics."""
+        with self._condition:
+            return {
+                "preview_clients": sum(feed.clients for feed in self._feeds.values()),
+                "preview_source_generation": self._source_generation,
+                "preview_encoded_frames": self._encoded_frames,
+                "preview_last_encode_ms": self._last_encode_ms,
+                "preview_max_encode_ms": self._max_encode_ms,
+            }
+
+    def _has_clients(self) -> bool:
+        return any(feed.clients > 0 for feed in self._feeds.values())
+
+    def _encode_loop(self) -> None:
+        last_generation = 0
+        next_encode_at = 0.0
+        period = 1.0 / self.config.fps
+        while not self._stop_event.is_set():
+            with self._condition:
+                self._condition.wait_for(
+                    lambda: self._stop_event.is_set()
+                    or (
+                        self._source_generation > last_generation
+                        and self._has_clients()
+                    ),
+                    timeout=1.0,
+                )
+                if self._stop_event.is_set():
+                    return
+                generation = self._source_generation
+                frames = dict(self._latest_frames)
+
+            delay = next_encode_at - time.monotonic()
+            if delay > 0 and self._stop_event.wait(delay):
+                return
+            next_encode_at = time.monotonic() + period
+            last_generation = generation
+
+            for name, frame in frames.items():
+                feed = self.feed(name)
+                if feed is None or feed.clients == 0:
+                    continue
+                try:
+                    encode_started = time.perf_counter()
+                    jpeg = self._encode_frame(frame)
+                    encode_ms = (time.perf_counter() - encode_started) * 1000
+                except Exception:
+                    logger.exception("Failed to encode web preview frame for %s", name)
+                    continue
+                feed.publish_jpeg(jpeg)
+                with self._condition:
+                    self._encoded_frames += 1
+                    self._last_encode_ms = encode_ms
+                    self._max_encode_ms = max(self._max_encode_ms, encode_ms)
+
+    def _encode_frame(self, frame: np.ndarray) -> bytes:
+        height, width = frame.shape[:2]
+        if width != self.config.width:
+            target_height = max(1, round(height * self.config.width / width))
+            frame = cv2.resize(
+                frame,
+                (self.config.width, target_height),
+                interpolation=cv2.INTER_AREA,
+            )
+        if frame.shape[2] == 4:
+            frame = cv2.cvtColor(frame, cv2.COLOR_RGBA2BGR)
+        else:
+            frame = cv2.cvtColor(frame, cv2.COLOR_RGB2BGR)
+        ok, buffer = cv2.imencode(
+            ".jpg", frame, [cv2.IMWRITE_JPEG_QUALITY, self.config.jpeg_quality]
+        )
+        if not ok:
+            raise RuntimeError("JPEG encoding failed")
+        return buffer.tobytes()
+
+    def stop(self) -> None:
+        if self._stop_event.is_set():
+            return
+        self._stop_event.set()
+        with self._condition:
+            self._condition.notify_all()
+        for feed in list(self._feeds.values()):
+            feed.stop()
+        if self._server is not None:
+            self._server.shutdown()
+            self._server.server_close()
+        if self._server_thread is not None:
+            self._server_thread.join(timeout=2.0)
+        if self._encoder_thread is not None:
+            self._encoder_thread.join(timeout=2.0)
+
+
+_WEB_PAGE = """<!doctype html>
+<html lang="zh-CN"><head><meta charset="utf-8">
+<meta name="viewport" content="width=device-width,initial-scale=1">
+<title>LeRobot 录制预览</title>
+<style>
+:root{color-scheme:dark;font-family:system-ui,sans-serif}body{margin:0;background:#101214;color:#eee}
+header{padding:14px 18px;background:#191c20;border-bottom:1px solid #333}h1{margin:0;font-size:20px}
+#status{margin-top:7px;color:#aeb5bd;font-size:13px}.grid{display:grid;grid-template-columns:repeat(auto-fit,minmax(320px,1fr));gap:12px;padding:14px}
+.tile{overflow:hidden;background:#191c20;border:1px solid #333;border-radius:7px}.tile h2{margin:0;padding:9px 11px;font-size:14px}.tile img{display:block;width:100%;background:#08090a}
+</style></head><body><header><h1>LeRobot 录制相机预览</h1><div id="status">正在等待相机帧…</div></header>
+<main id="grid" class="grid"></main><script>
+let signature='';async function refresh(){try{const r=await fetch('/api/status',{cache:'no-store'});const p=await r.json();
+document.getElementById('status').textContent=p.cameras.length?`${p.cameras.length} 路相机 · 预览 ${p.fps} FPS · 采集优先，预览允许丢帧`:'正在等待相机帧…';
+const next=p.cameras.join('|');if(next!==signature){signature=next;const grid=document.getElementById('grid');grid.replaceChildren();
+for(const name of p.cameras){const tile=document.createElement('section');tile.className='tile';const h=document.createElement('h2');h.textContent=name;
+const img=document.createElement('img');img.alt=name;img.src='/stream?camera='+encodeURIComponent(name);tile.append(h,img);grid.appendChild(tile);}}}catch(e){document.getElementById('status').textContent='预览服务不可用：'+e}}
+refresh();setInterval(refresh,2000);</script></body></html>""".encode()
+
+
+class _PreviewHandler(http.server.BaseHTTPRequestHandler):
+    preview: RecordingWebPreview
+
+    def do_GET(self) -> None:
+        parsed = urlsplit(self.path)
+        if parsed.path == "/":
+            self._send(200, "text/html; charset=utf-8", _WEB_PAGE)
+        elif parsed.path == "/api/status":
+            body = json.dumps(
+                {"ok": True, "cameras": self.preview.camera_names(), "fps": self.preview.config.fps}
+            ).encode()
+            self._send(200, "application/json; charset=utf-8", body)
+        elif parsed.path == "/stream":
+            self._stream(parse_qs(parsed.query).get("camera", [None])[0])
+        elif parsed.path == "/favicon.ico":
+            self._send(204, "text/plain", b"")
+        else:
+            self.send_error(404)
+
+    def _send(self, status: int, content_type: str, body: bytes) -> None:
+        try:
+            self.send_response(status)
+            self.send_header("Content-Type", content_type)
+            self.send_header("Content-Length", str(len(body)))
+            self.end_headers()
+            self.wfile.write(body)
+        except (BrokenPipeError, ConnectionResetError):
+            pass
+
+    def _stream(self, name: str | None) -> None:
+        feed = self.preview.feed(name)
+        if feed is None:
+            self._send(404, "text/plain; charset=utf-8", b"Unknown camera\n")
+            return
+        feed.add_client()
+        with self.preview._condition:
+            self.preview._condition.notify()
+        try:
+            self.send_response(200)
+            self.send_header("Cache-Control", "no-store")
+            self.send_header("Content-Type", "multipart/x-mixed-replace; boundary=frame")
+            self.end_headers()
+            frame_id = 0
+            while True:
+                jpeg, next_id, stopped = feed.wait_for_jpeg(frame_id)
+                if stopped:
+                    return
+                if jpeg is None or next_id == frame_id:
+                    continue
+                frame_id = next_id
+                self.wfile.write(
+                    b"--frame\r\nContent-Type: image/jpeg\r\n"
+                    + f"Content-Length: {len(jpeg)}\r\n\r\n".encode()
+                    + jpeg
+                    + b"\r\n"
+                )
+                self.wfile.flush()
+        except (BrokenPipeError, ConnectionResetError):
+            pass
+        finally:
+            feed.remove_client()
+
+    def log_message(self, format: str, *args: Any) -> None:
+        pass

tests/test_gello_reset.py

@@ -3,12 +3,14 @@ import pytest
 
 from lerobot_robot_ufactory.teleoperators.gello_teleop import gello_teleop as gello_module
 from lerobot_robot_ufactory.scripts.uf_lerobot_record import _prepare_recording_episode
+from lerobot_robot_ufactory.teleoperators.gello_teleop.gello_adapter import (
+    ContinuousDynamixelRobot,
+)
 
 
 class FakeDriver:
-    def __init__(self, positions, follow_commands=True):
+    def __init__(self, positions):
         self.positions = np.asarray(positions, dtype=float)
-        self.follow_commands = follow_commands
         self.commands = []
 
     def get_joints(self):
@@ -16,16 +18,14 @@ class FakeDriver:
 
     def set_joints(self, positions):
         self.commands.append(np.asarray(positions, dtype=float))
-        if self.follow_commands:
-            self.positions = self.commands[-1].copy()
 
     def close(self):
         pass
 
 
 class FakeGelloRobot:
-    def __init__(self, follow_commands=True):
-        self._driver = FakeDriver([0.0, 0.0, 0.0], follow_commands=follow_commands)
+    def __init__(self):
+        self._driver = FakeDriver([0.7, -0.2, 1.5])
         self._joint_signs = np.array([1.0, -1.0, 1.0])
         self._joint_offsets = np.array([0.1, 0.2, 0.0])
         self.gripper_open_close = (0.0, 1.0)
@@ -36,29 +36,19 @@ class FakeGelloRobot:
         self.torque_calls.append(enabled)
 
 
-def make_teleop(robot):
+def make_teleop(robot, align_gripper_to_current=True):
     teleop = gello_module.GelloTeleop.__new__(gello_module.GelloTeleop)
     teleop.id = "test_gello"
     teleop._is_connected = True
     teleop._teleop_enabled = False
     teleop._needs_alignment = True
+    teleop._align_gripper_to_current = align_gripper_to_current
     teleop.dof = 2
     teleop.gello_agent = type("FakeAgent", (), {"_robot": robot})()
     return teleop
 
 
-def patch_clock(monkeypatch):
-    clock = [0.0]
-    monkeypatch.setattr(gello_module.time, "perf_counter", lambda: clock[0])
-    monkeypatch.setattr(
-        gello_module.time,
-        "sleep",
-        lambda seconds: clock.__setitem__(0, clock[0] + seconds),
-    )
-
-
-def test_gello_reset_moves_to_robot_observation_and_disables_torque(monkeypatch):
-    patch_clock(monkeypatch)
+def test_gello_alignment_maps_current_pose_without_moving():
     robot = FakeGelloRobot()
     teleop = make_teleop(robot)
 
@@ -66,29 +56,58 @@ def test_gello_reset_moves_to_robot_observation_and_disables_torque(monkeypatch)
         {"J1.pos": 0.3, "J2.pos": -0.4, "gripper.pos": 0.5}
     )
 
-    assert robot.torque_calls == [True, False]
-    assert np.allclose(robot._driver.positions, [0.4, 0.6, 0.5])
+    assert robot.torque_calls == [False]
+    assert robot._driver.commands == []
+    assert np.allclose(robot._joint_offsets[:2], [0.4, -0.6])
+    assert np.allclose(robot.gripper_open_close, [1.0, 2.0])
     assert robot._last_pos is None
     assert teleop._teleop_enabled is False
     assert teleop._needs_alignment is False
 
 
-def test_gello_reset_failure_leaves_torque_off_and_teleop_disabled(monkeypatch):
-    patch_clock(monkeypatch)
-    robot = FakeGelloRobot(follow_commands=False)
+def test_gello_enable_requires_robot_observation():
+    robot = FakeGelloRobot()
     teleop = make_teleop(robot)
 
-    with pytest.raises(RuntimeError, match="did not reach"):
-        teleop.reset_to_robot_observation(
-            {"J1.pos": 0.3, "J2.pos": -0.4, "gripper.pos": 0.5}
-        )
+    with pytest.raises(ValueError, match="Robot observation"):
+        teleop.set_teleop_enabled(True)
 
-    assert robot.torque_calls == [True, False]
+    assert robot.torque_calls == []
     assert teleop._teleop_enabled is False
 
 
-def test_gello_enable_after_pause_realigns_before_output(monkeypatch):
-    patch_clock(monkeypatch)
+def test_fixed_gripper_endpoints_are_not_shifted_during_arm_alignment():
+    robot = FakeGelloRobot()
+    robot.gripper_open_close = (3.45, 2.72)
+    teleop = make_teleop(robot, align_gripper_to_current=False)
+
+    teleop.reset_to_robot_observation(
+        {"J1.pos": 0.3, "J2.pos": -0.4, "gripper.pos": 0.5}
+    )
+
+    assert robot.gripper_open_close == (3.45, 2.72)
+    assert robot._driver.commands == []
+
+
+def test_dynamixel_arm_joint_is_continuous_across_encoder_wrap():
+    robot = ContinuousDynamixelRobot(
+        joint_ids=[1],
+        joint_offsets=[0.0],
+        joint_signs=[1],
+        real=False,
+    )
+    robot._alpha = 1.0
+    robot._driver._joint_angles = np.array([2 * np.pi - 0.05])
+    before_wrap = robot.get_joint_state()[0]
+
+    robot._driver._joint_angles = np.array([0.05])
+    after_wrap = robot.get_joint_state()[0]
+
+    assert after_wrap > before_wrap
+    assert after_wrap - before_wrap == pytest.approx(0.1)
+
+
+def test_gello_enable_after_pause_realigns_current_pose_before_output():
     robot = FakeGelloRobot()
     teleop = make_teleop(robot)
 
@@ -97,10 +116,21 @@ def test_gello_enable_after_pause_realigns_before_output(monkeypatch):
         {"J1.pos": 0.3, "J2.pos": -0.4, "gripper.pos": 0.5},
     )
 
-    assert robot.torque_calls == [True, False]
-    assert np.allclose(robot._driver.positions, [0.4, 0.6, 0.5])
+    assert robot.torque_calls == [False]
+    assert robot._driver.commands == []
     assert teleop._teleop_enabled is True
 
+    teleop.set_teleop_enabled(False)
+    robot._driver.positions = np.array([1.0, 0.5, 1.8])
+    teleop.set_teleop_enabled(
+        True,
+        {"J1.pos": 0.1, "J2.pos": 0.2, "gripper.pos": 0.25},
+    )
+
+    assert np.allclose(robot._joint_offsets[:2], [0.9, 0.7])
+    assert np.allclose(robot.gripper_open_close, [1.55, 2.55])
+    assert robot._driver.commands == []
+
 
 def test_gello_disconnect_closes_driver():
     robot = FakeGelloRobot()
@@ -129,15 +159,11 @@ def test_recording_reset_disables_before_robot_and_enables_after_alignment():
         def set_teleop_enabled(self, enabled, obs=None):
             calls.append(f"teleop_{enabled}")
 
-        def reset_to_robot_observation(self, obs):
-            calls.append("gello_alignment")
-
     _prepare_recording_episode(FakeRobot(), FakeTeleop(), True, False)
 
     assert calls == [
         "teleop_False",
         "robot_reset",
         "observation",
-        "gello_alignment",
         "teleop_True",
     ]

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_local_kinematics.py

@@ -0,0 +1,209 @@
+from threading import Lock
+from types import SimpleNamespace
+
+import numpy as np
+import pytest
+
+from lerobot_robot_ufactory.robots.uf_robot.local_kinematics import XArm7Kinematics
+from lerobot_robot_ufactory.robots.uf_robot.uf_robot import UFRobot
+
+
+NOMINAL_XARM7_ORIGINS = np.asarray(
+    [
+        [0.0, 0.0, 0.267, 0.0, 0.0, 0.0],
+        [0.0, 0.0, 0.0, -1.5708, 0.0, 0.0],
+        [0.0, -0.293, 0.0, 1.5708, 0.0, 0.0],
+        [0.0525, 0.0, 0.0, 1.5708, 0.0, 0.0],
+        [0.0775, -0.3425, 0.0, 1.5708, 0.0, 0.0],
+        [0.0, 0.0, 0.0, 1.5708, 0.0, 0.0],
+        [0.076, 0.097, 0.0, -1.5708, 0.0, 0.0],
+    ],
+    dtype=np.float64,
+)
+
+
+def test_height_jacobian_matches_finite_difference():
+    model = XArm7Kinematics(NOMINAL_XARM7_ORIGINS, tcp_offset=[0, 0, 80, 0, 0, 0])
+    joints = np.asarray([0.2, -0.4, 0.3, 0.7, -0.2, 0.5, 0.4])
+
+    _, analytic = model.tcp_z_and_jacobian(joints)
+    numeric = np.empty(7)
+    epsilon = 1e-6
+    for index in range(7):
+        plus = joints.copy()
+        minus = joints.copy()
+        plus[index] += epsilon
+        minus[index] -= epsilon
+        numeric[index] = (
+            model.tcp_position(plus)[2] - model.tcp_position(minus)[2]
+        ) / (2 * epsilon)
+
+    assert analytic == pytest.approx(numeric, abs=1e-5)
+
+
+def test_tcp_offset_is_applied_in_tool_frame():
+    model = XArm7Kinematics(NOMINAL_XARM7_ORIGINS)
+    offset_model = XArm7Kinematics(NOMINAL_XARM7_ORIGINS, tcp_offset=[0, 0, 100, 0, 0, 0])
+    joints = np.asarray([0.2, -0.4, 0.3, 0.7, -0.2, 0.5, 0.4])
+
+    base_transform = model.forward_matrix(joints)
+    expected = base_transform[:3, 3] + base_transform[:3, 2] * 100.0
+
+    assert offset_model.tcp_position(joints) == pytest.approx(expected)
+    assert np.linalg.norm(offset_model.tcp_position(joints) - base_transform[:3, 3]) == pytest.approx(
+        100.0
+    )
+
+
+class HeightModel:
+    """Simple local model with z controlled by J1 and J7 in millimetres."""
+
+    def tcp_position(self, joints):
+        joints = np.asarray(joints)
+        return np.asarray([0.0, 0.0, 100.0 + 100.0 * joints[0] + 20.0 * joints[6]])
+
+    def tcp_z_and_jacobian(self, joints):
+        return float(self.tcp_position(joints)[2]), np.asarray([100.0, 0, 0, 0, 0, 0, 20.0])
+
+
+def make_local_guard_robot():
+    robot = UFRobot.__new__(UFRobot)
+    robot._dof = 7
+    robot._tcp_z_guard_backend = "local_projection"
+    robot._local_kinematics = HeightModel()
+    robot._min_tcp_z_mm = 95.0
+    robot._tcp_z_soft_floor_mm = 100.0
+    robot._last_safe_joint_target = np.zeros(7)
+    robot._last_guard_path = "not_run"
+    robot._tcp_z_is_clamped = False
+    robot._tcp_z_last_log_time = 0.0
+    robot._tcp_z_last_error_log_time = 0.0
+    robot.real_arm = SimpleNamespace()
+    return robot
+
+
+def test_local_guard_keeps_tangent_motion_and_projects_height():
+    robot = make_local_guard_robot()
+    desired = np.asarray([-0.1, 0.08, 0.0, 0.0, 0.0, 0.0, 0.1])
+
+    result = robot._guard_joint_target(desired)
+
+    assert robot._last_guard_path == "local_projected"
+    assert robot._local_kinematics.tcp_position(result)[2] >= 100.0 - 1e-3
+    assert result[1] == pytest.approx(0.08)
+    assert result[6] != pytest.approx(0.0)
+
+
+def test_local_guard_safe_path_does_not_touch_controller():
+    class ControllerThatMustNotBeCalled:
+        def __getattr__(self, name):
+            raise AssertionError(f"unexpected controller call: {name}")
+
+    robot = make_local_guard_robot()
+    robot.real_arm = ControllerThatMustNotBeCalled()
+
+    result = robot._guard_joint_target([0.05, 0, 0, 0, 0, 0, 0.1])
+
+    assert result == pytest.approx([0.05, 0, 0, 0, 0, 0, 0.1])
+    assert robot._last_guard_path == "local_safe"
+
+
+def test_local_guard_holds_after_persistent_rt_model_mismatch():
+    robot = make_local_guard_robot()
+    robot._rt_report_normal = True
+    robot._update_lock = Lock()
+    robot.rt_actual_joint_pos = np.zeros(7)
+    robot.rt_actual_tcp_pose = [0.0, 0.0, 110.0, 0.0, 0.0, 0.0]
+    robot._local_model_fault_count = 0
+    robot.config = SimpleNamespace(local_kinematics_max_error_mm=2.0)
+    robot.logs = {}
+
+    robot._guard_joint_target([0.05, 0, 0, 0, 0, 0, 0])
+    robot._guard_joint_target([0.05, 0, 0, 0, 0, 0, 0])
+    result = robot._guard_joint_target([0.05, 0, 0, 0, 0, 0, 0])
+
+    assert result == pytest.approx([0.05, 0, 0, 0, 0, 0, 0])
+    assert robot._last_guard_path == "model_fault"
+    assert robot.logs["local_kinematics_error_mm"] == pytest.approx(10.0)
+
+
+def test_local_projection_config_requires_joint_xarm7_and_floor():
+    from lerobot_robot_ufactory.robots.uf_robot.uf_robot_config import UFRobotConfig
+
+    with pytest.raises(ValueError, match="joint control on an xArm7"):
+        UFRobotConfig(
+            robot_dof=6,
+            control_space="joint",
+            min_tcp_z_mm=50.0,
+            tcp_z_guard_backend="local_projection",
+        )
+    with pytest.raises(ValueError, match="requires min_tcp_z_mm"):
+        UFRobotConfig(robot_dof=7, tcp_z_guard_backend="local_projection")
+
+
+def test_controller_boundary_is_configured_once_and_verified():
+    class BoundaryArm:
+        def __init__(self):
+            self.calls = []
+
+        def set_reduced_tcp_boundary(self, boundary):
+            self.calls.append(("boundary", boundary))
+            return 0
+
+        def set_fence_mode(self, enabled):
+            self.calls.append(("fence", enabled))
+            return [0]
+
+        def get_reduced_states(self, **kwargs):
+            states = [False, [9999, -9999, 9999, -9999, 9999, 50], 0, 0, [], True, False]
+            return 0, states
+
+    robot = UFRobot.__new__(UFRobot)
+    robot.real_arm = BoundaryArm()
+    robot._min_tcp_z_mm = 50.0
+
+    robot._configure_controller_safety_boundary()
+
+    assert robot.real_arm.calls == [
+        ("boundary", [9999, -9999, 9999, -9999, 9999, 50]),
+        ("fence", True),
+    ]
+
+
+def test_startup_validation_compares_controller_fk_to_flange_not_tcp():
+    flange_model = XArm7Kinematics(NOMINAL_XARM7_ORIGINS)
+    controller_model = XArm7Kinematics(
+        NOMINAL_XARM7_ORIGINS,
+        tcp_offset=[0.0, 0.0, 172.0, 0.0, 0.0, 0.0],
+    )
+
+    def matrix_to_rpy(rotation):
+        pitch = np.arcsin(np.clip(-rotation[2, 0], -1.0, 1.0))
+        roll = np.arctan2(rotation[2, 1], rotation[2, 2])
+        yaw = np.arctan2(rotation[1, 0], rotation[0, 0])
+        return np.asarray([roll, pitch, yaw])
+
+    class FlangeFkArm:
+        tcp_offset = [0.0, 0.0, 172.0, 0.0, 0.0, 0.0]
+        world_offset = [0.0] * 6
+        default_is_radian = True
+
+        def get_joint_states(self, **kwargs):
+            return 0, [np.zeros(7)]
+
+        def get_forward_kinematics(self, joints, **kwargs):
+            transform = controller_model.forward_matrix(joints)
+            rpy = matrix_to_rpy(transform[:3, :3])
+            return 0, [*transform[:3, 3], *rpy]
+
+    robot = UFRobot.__new__(UFRobot)
+    robot.real_arm = FlangeFkArm()
+    robot._local_joint_origins = NOMINAL_XARM7_ORIGINS
+    robot._min_tcp_z_mm = -100.0
+    robot.config = SimpleNamespace(local_kinematics_max_error_mm=2.0)
+
+    robot._initialize_local_kinematics()
+
+    flange_position = flange_model.tcp_position(np.zeros(7))
+    tcp_position = robot._local_kinematics.tcp_position(np.zeros(7))
+    assert np.linalg.norm(tcp_position - flange_position) == pytest.approx(172.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
@@ -131,7 +132,7 @@ def test_manual_mode_robot_enters_teaching_mode_without_sending_actions(monkeypa
             "set_servo_angle",
             {
                 "angle": arm.initial_point,
-                "speed": 60,
+                "speed": 20,
                 "is_radian": False,
                 "wait": True,
             },
@@ -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
@@ -177,7 +183,7 @@ def test_robot_reset_uses_sdk_initial_point_in_normal_mode(monkeypatch, tmp_path
             "set_servo_angle",
             {
                 "angle": arm.initial_point,
-                "speed": 60,
+                "speed": 20,
                 "is_radian": False,
                 "wait": True,
             },
@@ -187,6 +193,76 @@ def test_robot_reset_uses_sdk_initial_point_in_normal_mode(monkeypatch, tmp_path
     robot.disconnect()
 
 
+def test_normal_mode_waits_for_gripper_to_open_before_control(monkeypatch, tmp_path):
+    from lerobot_robot_ufactory.robots.uf_robot import uf_robot as uf_robot_module
+
+    arm = FakeXArm("192.168.1.245")
+    arm.gripper_position = 400
+    monkeypatch.setattr(uf_robot_module, "XArmAPI", lambda robot_ip: arm)
+    monkeypatch.setattr(uf_robot_module.time, "sleep", lambda _: None)
+
+    config = UFRobotConfig(
+        id="test_wait_for_gripper",
+        calibration_dir=tmp_path,
+        robot_ip=arm.robot_ip,
+        robot_dof=6,
+        control_space="joint",
+        gripper_type=1,
+    )
+    robot = uf_robot_module.UFRobot(config)
+    robot.connect()
+
+    open_calls = [call for call in arm.calls if call[0] == "set_gripper_position"]
+    assert open_calls == [("set_gripper_position", 800, {"wait": True})]
+    assert robot._last_gripper_command == 0.0
+
+    before_writes = len(
+        [call for call in arm.calls if call[0] == "getset_tgpio_modbus_data"]
+    )
+    robot._send_gripper_action(0.0)
+    after_writes = len(
+        [call for call in arm.calls if call[0] == "getset_tgpio_modbus_data"]
+    )
+    assert after_writes == before_writes
+
+    robot.disconnect()
+
+
+def test_gripper_rs485_commands_are_rate_limited(monkeypatch, tmp_path):
+    from lerobot_robot_ufactory.robots.uf_robot import uf_robot as uf_robot_module
+
+    arm = FakeXArm("192.168.1.245")
+    monkeypatch.setattr(uf_robot_module, "XArmAPI", lambda robot_ip: arm)
+    monkeypatch.setattr(uf_robot_module.time, "sleep", lambda _: None)
+    config = UFRobotConfig(
+        id="test_gripper_rate_limit",
+        calibration_dir=tmp_path,
+        robot_ip=arm.robot_ip,
+        robot_dof=6,
+        control_space="joint",
+        gripper_type=1,
+        gripper_command_interval_s=0.1,
+        gripper_error_log_path=None,
+    )
+    robot = uf_robot_module.UFRobot(config)
+    robot.connect()
+
+    robot._send_gripper_action(0.2)
+    robot._send_gripper_action(0.4)
+    writes = [call for call in arm.calls if call[0] == "set_gripper_position"]
+    # One initialization/open write and one runtime write; the second runtime
+    # target is coalesced by the RS485 rate limiter.
+    assert len(writes) == 2
+    assert writes[-1][2] == {
+        "wait": False,
+        "wait_motion": False,
+        "check_baud": False,
+        "check_err": False,
+    }
+
+    robot.disconnect()
+
+
 def test_manual_mode_config_rejects_cartesian_control(tmp_path):
     with pytest.raises(ValueError, match="control_space='joint'"):
         UFRobotConfig(
@@ -263,6 +339,7 @@ def test_manual_mode_initializes_gripper_without_opening_and_sends_only_gripper(
         control_space="joint",
         gripper_type=1,
         manual_mode=True,
+        gripper_error_log_path=None,
     )
     robot = uf_robot_module.UFRobot(config)
     robot.connect()
@@ -274,17 +351,50 @@ def test_manual_mode_initializes_gripper_without_opening_and_sends_only_gripper(
 
     robot.send_action({"J1.pos": 1.0, "gripper.pos": 0.5})
 
-    assert any(call[0] == "getset_tgpio_modbus_data" for call in arm.calls)
+    runtime_writes = [call for call in arm.calls if call[0] == "set_gripper_position"]
+    assert len(runtime_writes) == 1
+    assert runtime_writes[0][2]["wait_motion"] is False
     assert not any(call[0] == "set_servo_angle" for call in arm.calls)
     robot.disconnect()
 
 
+def test_gripper_command_is_only_sent_after_target_changes(monkeypatch, tmp_path):
+    from lerobot_robot_ufactory.robots.uf_robot import uf_robot as uf_robot_module
+
+    arm = FakeXArm("192.168.1.245")
+    monkeypatch.setattr(uf_robot_module, "XArmAPI", lambda robot_ip: arm)
+    monkeypatch.setattr(uf_robot_module.time, "sleep", lambda _: None)
+
+    config = UFRobotConfig(
+        id="test_gripper_command_threshold",
+        calibration_dir=tmp_path,
+        robot_ip=arm.robot_ip,
+        robot_dof=6,
+        control_space="joint",
+        gripper_type=1,
+        manual_mode=True,
+        gripper_command_threshold=0.01,
+        gripper_command_interval_s=0.0,
+        gripper_error_log_path=None,
+    )
+    robot = uf_robot_module.UFRobot(config)
+    robot.connect()
+
+    robot.send_action({"gripper.pos": 0.5})
+    robot.send_action({"gripper.pos": 0.505})
+    robot.send_action({"gripper.pos": 0.52})
+
+    writes = [call for call in arm.calls if call[0] == "set_gripper_position"]
+    assert len(writes) == 2
+    robot.disconnect()
+
+
 def test_manual_record_config_has_no_teleop(monkeypatch):
     config_path = Path("config/manual_mode/xarm7_manual_record_config.yaml").resolve()
     monkeypatch.setattr(
         sys,
         "argv",
-        ["uf-lerobot-record", "--config_path", str(config_path)],
+        ["record", "--config_path", str(config_path)],
     )
 
     config = get_cfg()

tests/test_realtime_teleop.py

@@ -0,0 +1,100 @@
+import time
+
+import pytest
+
+from lerobot_robot_ufactory.teleoperators.gello_teleop.gello_teleop_config import (
+    GelloTeleopConfig,
+)
+from lerobot_robot_ufactory.utils.realtime_teleop import RealtimeTeleopController
+
+
+class FakeTeleop:
+    def __init__(self):
+        self.count = 0
+
+    def get_action(self):
+        self.count += 1
+        return {"J1.pos": float(self.count)}
+
+
+class FakeRobot:
+    def __init__(self):
+        self.actions = []
+
+    def send_action(self, action):
+        self.actions.append(dict(action))
+        return action
+
+
+def identity_action_processor(value):
+    return value[0]
+
+
+def test_gello_realtime_control_fps_defaults_to_60():
+    assert GelloTeleopConfig().realtime_control_fps == 60
+
+
+def test_gello_realtime_control_fps_must_be_positive():
+    with pytest.raises(ValueError, match="realtime_control_fps"):
+        GelloTeleopConfig(realtime_control_fps=0)
+
+
+def test_realtime_controller_sends_without_waiting_for_observation_owner():
+    robot = FakeRobot()
+    controller = RealtimeTeleopController(
+        robot,
+        FakeTeleop(),
+        identity_action_processor,
+        identity_action_processor,
+        fps=100,
+        initial_observation={"J1.pos": 0.0},
+    )
+
+    controller.start()
+    time.sleep(0.06)
+    controller.heartbeat()
+    controller.stop()
+
+    assert len(robot.actions) >= 4
+    assert controller.latest_action() == robot.actions[-1]
+
+
+def test_action_at_never_selects_a_future_command():
+    controller = RealtimeTeleopController(
+        FakeRobot(),
+        FakeTeleop(),
+        identity_action_processor,
+        identity_action_processor,
+        fps=100,
+        initial_observation={"J1.pos": 0.0},
+    )
+    controller.start()
+    time.sleep(0.035)
+    controller.stop()
+
+    with controller._lock:
+        history = list(controller._action_history)
+    assert len(history) >= 2
+    sample_time = (history[0][0] + history[1][0]) / 2
+    assert controller.action_at(sample_time) == history[0][1]
+    action, sent_at = controller.action_sample_at(sample_time)
+    assert action == history[0][1]
+    assert sent_at == history[0][0]
+
+
+def test_realtime_controller_propagates_send_failures():
+    class FailingRobot:
+        def send_action(self, action):
+            raise ValueError("servo failed")
+
+    controller = RealtimeTeleopController(
+        FailingRobot(),
+        FakeTeleop(),
+        identity_action_processor,
+        identity_action_processor,
+        fps=60,
+        initial_observation={},
+    )
+
+    with pytest.raises(RuntimeError, match="Realtime ServoJ control thread failed"):
+        controller.start()

tests/test_tcp_z_safety.py

@@ -0,0 +1,243 @@
+from pathlib import Path
+from types import SimpleNamespace
+from threading import Lock
+
+import numpy as np
+import pytest
+
+from lerobot_robot_ufactory.robots.uf_robot.uf_robot import UFRobot
+from lerobot_robot_ufactory.robots.uf_robot.uf_robot_config import UFRobotConfig
+from lerobot_robot_ufactory.scripts.uf_read_tcp_z import read_tcp_z
+
+
+class FakeKinematicsArm:
+    def __init__(self, requested_pose=None, safe_pose=None):
+        self.requested_pose = requested_pose or [300.0, 10.0, 90.0, 0.1, 0.2, 0.3]
+        self.safe_pose = safe_pose or [300.0, 10.0, 100.0, 0.1, 0.2, 0.3]
+        self.inverse_result = [0.3] * 7
+        self.inverse_calls = []
+        self.fail_forward = False
+        self.fail_inverse = False
+        self.joint_limit = False
+
+    def get_forward_kinematics(self, angles, **kwargs):
+        if self.fail_forward:
+            return 1, []
+        if np.allclose(angles, self.inverse_result):
+            return 0, self.safe_pose.copy()
+        return 0, self.requested_pose.copy()
+
+    def get_inverse_kinematics(self, pose, **kwargs):
+        self.inverse_calls.append((pose.copy(), kwargs))
+        if self.fail_inverse:
+            return 2, []
+        return 0, self.inverse_result.copy()
+
+    def is_joint_limit(self, target, **kwargs):
+        return 0, self.joint_limit
+
+
+def make_guard_robot(arm, min_tcp_z_mm=100.0, control_space="joint"):
+    robot = UFRobot.__new__(UFRobot)
+    robot._dof = 7
+    robot._control_space = control_space
+    robot._min_tcp_z_mm = min_tcp_z_mm
+    robot._last_safe_joint_target = np.asarray([0.25] * 7)
+    robot._tcp_z_is_clamped = False
+    robot._tcp_z_last_log_time = 0.0
+    robot._tcp_z_last_error_log_time = 0.0
+    robot.real_arm = arm
+    return robot
+
+
+def test_joint_target_above_floor_is_unchanged():
+    arm = FakeKinematicsArm(requested_pose=[300.0, 10.0, 101.0, 0.1, 0.2, 0.3])
+    robot = make_guard_robot(arm)
+    requested = [0.1] * 7
+
+    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)
+
+
+def test_joint_guard_uses_rt_report_fast_path_far_above_floor():
+    arm = FakeKinematicsArm()
+    robot = make_guard_robot(arm)
+    robot._rt_report_normal = True
+    robot._update_lock = Lock()
+    robot.rt_actual_tcp_pose = [0.0, 0.0, 200.0, 0.0, 0.0, 0.0]
+    robot._tcp_z_guard_activation_margin_mm = 50.0
+
+    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 == []
+
+
+def test_joint_target_below_floor_clamps_only_tcp_z_before_inverse_kinematics():
+    arm = FakeKinematicsArm()
+    robot = make_guard_robot(arm)
+    requested = [0.1] * 7
+
+    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
+    assert inverse_kwargs["ref_angles"] == pytest.approx([0.25] * 7)
+    assert np.allclose(robot._last_safe_joint_target, arm.inverse_result)
+
+
+def test_successive_clamped_ik_uses_last_accepted_solution_as_reference():
+    arm = FakeKinematicsArm()
+    robot = make_guard_robot(arm)
+
+    first_result = robot._guard_joint_target([0.1] * 7)
+    arm.inverse_result = [0.32] * 7
+    second_result = robot._guard_joint_target([0.05] * 7)
+
+    assert first_result == pytest.approx([0.3] * 7)
+    assert second_result == pytest.approx([0.32] * 7)
+    assert arm.inverse_calls[1][1]["ref_angles"] == pytest.approx([0.3] * 7)
+
+
+@pytest.mark.parametrize("failed_stage", ["forward", "inverse", "verification"])
+def test_joint_guard_holds_last_safe_target_when_kinematics_fails(failed_stage):
+    arm = FakeKinematicsArm()
+    if failed_stage == "forward":
+        arm.fail_forward = True
+    elif failed_stage == "inverse":
+        arm.fail_inverse = True
+    else:
+        arm.safe_pose[2] = 99.0
+    robot = make_guard_robot(arm)
+
+    result = robot._guard_joint_target([0.1] * 7)
+
+    assert np.allclose(result, [0.25] * 7)
+
+
+def test_joint_guard_holds_last_safe_target_when_ik_hits_joint_limit():
+    arm = FakeKinematicsArm()
+    arm.joint_limit = True
+    robot = make_guard_robot(arm)
+
+    result = robot._guard_joint_target([0.1] * 7)
+
+    assert np.allclose(result, [0.25] * 7)
+
+
+def test_joint_guard_rejects_discontinuous_ik_solution():
+    arm = FakeKinematicsArm()
+    arm.inverse_result = [1.5] * 7
+    arm.safe_pose[2] = 100.0
+    robot = make_guard_robot(arm)
+
+    result = robot._guard_joint_target([0.1] * 7)
+
+    assert np.allclose(result, [0.25] * 7)
+
+
+def test_send_action_sends_and_returns_clamped_joint_target():
+    arm = FakeKinematicsArm()
+    arm.error_code = 0
+    arm.mode = 1
+    arm.sent_joint_targets = []
+    arm.set_servo_angle_j = lambda target, **kwargs: arm.sent_joint_targets.append(target) or 0
+    robot = make_guard_robot(arm)
+    robot._is_connected = True
+    robot._last_logged_controller_error = 0
+    robot._cmd_cnt = 20
+    robot._max_joint_velocity = 1.0
+    robot._gripper_type = 0
+    robot.prefix = ""
+    robot.logs = {}
+    robot.config = SimpleNamespace(
+        manual_mode=False,
+        no_action=False,
+        joint_command_mode=1,
+        gripper_error_log_path=None,
+    )
+    action = {f"J{i + 1}.pos": 0.1 for i in range(7)}
+
+    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
+    )
+    assert [action[f"J{i + 1}.pos"] for i in range(7)] == pytest.approx([0.1] * 7)
+
+
+def test_non_finite_tcp_floor_is_rejected():
+    with pytest.raises(ValueError, match="min_tcp_z_mm"):
+        UFRobotConfig(robot_dof=7, min_tcp_z_mm=float("nan"))
+
+
+class FakeMeasurementArm:
+    def __init__(self, robot_ip):
+        self.robot_ip = robot_ip
+        self.connected = True
+        self.axis = 7
+        self.disconnected = False
+        self.forward_calls = []
+
+    def get_joint_states(self, **kwargs):
+        return 0, [[0.1] * 7]
+
+    def get_forward_kinematics(self, joints, **kwargs):
+        self.forward_calls.append((joints, kwargs))
+        return 0, [300.0, 0.0, 87.25, 0.0, 0.0, 0.0]
+
+    def disconnect(self):
+        self.disconnected = True
+
+
+def write_measurement_config(path: Path):
+    path.write_text(
+        "robot:\n  robot_ip: '192.168.1.245'\n  robot_dof: 7\n",
+        encoding="utf-8",
+    )
+
+
+def test_read_tcp_z_adds_margin_and_disconnects(tmp_path):
+    config_path = tmp_path / "gello.yaml"
+    write_measurement_config(config_path)
+    arms = []
+
+    def arm_factory(robot_ip):
+        arm = FakeMeasurementArm(robot_ip)
+        arms.append(arm)
+        return arm
+
+    measured, recommended = read_tcp_z(config_path, margin_mm=5.0, arm_factory=arm_factory)
+
+    assert measured == pytest.approx(87.25)
+    assert recommended == pytest.approx(92.25)
+    assert arms[0].robot_ip == "192.168.1.245"
+    assert arms[0].disconnected is True
+    assert arms[0].forward_calls[0][1] == {
+        "input_is_radian": True,
+        "return_is_radian": True,
+    }
+
+
+def test_read_tcp_z_disconnects_when_fk_fails(tmp_path):
+    config_path = tmp_path / "gello.yaml"
+    write_measurement_config(config_path)
+    arm = FakeMeasurementArm("192.168.1.245")
+    arm.get_forward_kinematics = lambda *args, **kwargs: (1, [])
+
+    with pytest.raises(RuntimeError, match="get_forward_kinematics"):
+        read_tcp_z(config_path, arm_factory=lambda _: arm)
+
+    assert arm.disconnected is True

tests/test_web_preview.py

@@ -0,0 +1,122 @@
+import socket
+import threading
+import time
+from pathlib import Path
+from urllib.request import urlopen
+
+import numpy as np
+import pytest
+import yaml
+
+from lerobot_robot_ufactory.utils.web_preview import (
+    RecordingWebPreview,
+    WebPreviewConfig,
+    _WEB_PAGE,
+)
+
+
+def _preview(**overrides):
+    values = {
+        "enabled": True,
+        "host": "127.0.0.1",
+        "port": 8765,
+        "fps": 30,
+        "width": 32,
+        "jpeg_quality": 60,
+    }
+    values.update(overrides)
+    return RecordingWebPreview(WebPreviewConfig(**values))
+
+
+def _start_encoder(preview):
+    preview._encoder_thread = threading.Thread(target=preview._encode_loop, daemon=True)
+    preview._encoder_thread.start()
+
+
+def test_config_rejects_invalid_resource_limits():
+    with pytest.raises(ValueError, match="fps"):
+        WebPreviewConfig(fps=0).validate()
+    with pytest.raises(ValueError, match="jpeg_quality"):
+        WebPreviewConfig(jpeg_quality=101).validate()
+
+
+def test_embedded_page_has_parseable_camera_signature_expression():
+    page = _WEB_PAGE.decode()
+
+    assert "const next=p.cameras.join('|')" in page
+    assert "join('\n')" not in page
+
+
+def test_gello_record_config_enables_low_rate_web_preview():
+    config_path = Path("config/gello/xarm7_gello_record_config.yaml")
+    config = yaml.safe_load(config_path.read_text())
+
+    assert config["web_preview"]["enabled"] is True
+    assert config["web_preview"]["fps"] == 8
+    assert config["web_preview"]["width"] == 480
+
+
+def test_publish_keeps_only_latest_frame_references():
+    preview = _preview()
+    first = np.zeros((24, 32, 3), dtype=np.uint8)
+    second = np.ones((24, 32, 3), dtype=np.uint8)
+
+    preview.publish({"J1.pos": 0.0, "camera": first})
+    preview.publish({"camera": second})
+
+    assert preview.camera_names() == ["camera"]
+    assert preview._latest_frames["camera"] is second
+    assert preview._source_generation == 2
+
+
+def test_encoder_is_idle_without_a_browser_client(monkeypatch):
+    preview = _preview()
+    calls = []
+    monkeypatch.setattr(preview, "_encode_frame", lambda frame: calls.append(frame) or b"jpeg")
+    _start_encoder(preview)
+    try:
+        preview.publish({"camera": np.zeros((24, 32, 3), dtype=np.uint8)})
+        time.sleep(0.08)
+        assert calls == []
+    finally:
+        preview.stop()
+
+
+def test_client_receives_background_encoded_latest_frame():
+    preview = _preview()
+    _start_encoder(preview)
+    try:
+        frame = np.zeros((24, 32, 3), dtype=np.uint8)
+        frame[:, :, 0] = 255
+        preview.publish({"camera": frame})
+        feed = preview.feed("camera")
+        feed.add_client()
+        with preview._condition:
+            preview._condition.notify()
+        jpeg, frame_id, stopped = feed.wait_for_jpeg(0)
+        feed.remove_client()
+
+        assert jpeg is not None and jpeg.startswith(b"\xff\xd8")
+        assert frame_id == 1
+        assert not stopped
+    finally:
+        preview.stop()
+
+
+def test_status_endpoint_lists_published_cameras():
+    try:
+        with socket.socket() as sock:
+            sock.bind(("127.0.0.1", 0))
+            port = sock.getsockname()[1]
+    except PermissionError:
+        pytest.skip("local sockets are disabled by the test sandbox")
+    preview = _preview(port=port)
+    preview.start()
+    try:
+        preview.publish({"camera2": np.zeros((24, 32, 3), dtype=np.uint8)})
+        with urlopen(preview.url + "api/status", timeout=2) as response:
+            body = response.read()
+        assert response.status == 200
+        assert b'"camera2"' in body
+    finally:
+        preview.stop()

uv.lock

@@ -743,10 +743,14 @@ wheels = [
 [[package]]
 name = "dynamixel-sdk"
 version = "4.0.5"
-source = { git = "https://github.com/ROBOTIS-GIT/DynamixelSDK.git?subdirectory=python#2ded684dff05a40ac78d6a16105c6ddc1b3b9930" }
+source = { registry = "https://pypi.org/simple" }
 dependencies = [
     { name = "pyserial" },
 ]
+sdist = { url = "https://files.pythonhosted.org/packages/de/ad/05bb6c7fe54c01d2712398872b300891a5b6a0181e69335f4e1717d72805/dynamixel_sdk-4.0.5.tar.gz", hash = "sha256:498ba2090f5f9844ac0610553cc70b8c79e3f6f52f7911425cdb2857210b9630", size = 29695, upload-time = "2026-05-06T02:12:08.389Z" }
+wheels = [
+    { url = "https://files.pythonhosted.org/packages/52/a5/319d15afd31997e54e5c88b2fe1d53d15c9e63c3b8d51eda40ddff629443/dynamixel_sdk-4.0.5-py3-none-any.whl", hash = "sha256:36f9c0c078cbb8e87f5413bfcf76da8f50ce07d17690c52e52ad0f0180a7d6d8", size = 103493, upload-time = "2026-05-06T02:12:06.77Z" },
+]
 
 [[package]]
 name = "einops"
@@ -1246,7 +1250,7 @@ spacemouse = [
 
 [package.metadata]
 requires-dist = [
-    { name = "dynamixel-sdk", marker = "extra == 'gello'", git = "https://github.com/ROBOTIS-GIT/DynamixelSDK.git?subdirectory=python" },
+    { name = "dynamixel-sdk", marker = "extra == 'gello'", specifier = ">=4.0.5" },
     { name = "gello", marker = "extra == 'gello'", git = "https://github.com/xArm-Developer/gello_software.git" },
     { name = "lerobot", extras = ["intelrealsense"], specifier = "==0.4.3" },
     { name = "mypy", marker = "extra == 'dev'", specifier = ">=1.10" },