From 172bec5fe69d8e69adc931550f68c24c6350d83c Mon Sep 17 00:00:00 2001
From: ChenYuhan <2514158309@qq.com>
Date: Mon, 17 Aug 2026 21:03:44 +0800
Subject: [PATCH] Fix smooth safe GELLO joint recording

---
 README.md                                     |  13 +-
 README_ZH.md                                  |  13 +-
 config/gello/xarm7_gello_record_config.yaml   |  21 +-
 docs/gello_xarm7_smooth_safe_recording_zh.md  | 174 +++++++++
 .../robots/uf_robot/local_kinematics.py       | 139 +++++++
 .../robots/uf_robot/uf_robot.py               | 359 +++++++++++++++++-
 .../robots/uf_robot/uf_robot_config.py        |  26 ++
 .../scripts/uf_lerobot_record.py              |  96 ++++-
 .../scripts/uf_robot_teleop.py                |  58 ++-
 .../utils/realtime_teleop.py                  | 118 ++++++
 tests/test_local_kinematics.py                | 209 ++++++++++
 tests/test_manual_mode.py                     |  44 ++-
 tests/test_realtime_teleop.py                 |  88 +++++
 13 files changed, 1318 insertions(+), 40 deletions(-)
 create mode 100644 docs/gello_xarm7_smooth_safe_recording_zh.md
 create mode 100644 src/lerobot_robot_ufactory/robots/uf_robot/local_kinematics.py
 create mode 100644 src/lerobot_robot_ufactory/utils/realtime_teleop.py
 create mode 100644 tests/test_local_kinematics.py
 create mode 100644 tests/test_realtime_teleop.py

diff --git a/README.md b/README.md
index 6b29089..6623165 100644
--- a/README.md
+++ b/README.md
@@ -140,9 +140,10 @@ uv run uf-robot-teleop \
 ```
 
 Press `Space` to reset and start. While staying safe, include motion both far
-from and near the configured height floor. The CSV `guard_path` column marks
-`rt_fast_path`, `fk_safe`, `fk_ik_clamp`, or `fallback`, and the terminal prints
-per-path summaries. Results are written to `logs/gello_guard_latency_<time>.csv`.
+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
 
@@ -154,7 +155,11 @@ uv run uf-read-tcp-z \
   --margin-mm 5
 ```
 
-The command does not move the arm. Put the recommended `min_tcp_z_mm` value in the GELLO YAML. Teleop, recording, and other control entry points using that robot configuration will then enforce the floor immediately before sending each command. For joint control, targets below the floor retain their TCP x/y position and orientation while z is clamped.
+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.
 
diff --git a/README_ZH.md b/README_ZH.md
index ff27222..7b13d99 100644
--- a/README_ZH.md
+++ b/README_ZH.md
@@ -130,12 +130,16 @@ uv run uf-robot-teleop \
 ```
 
 按 `Space` 复位并开始。实验期间可在确保安全的前提下分别经过远离高度下限和接近
-高度下限的区域。CSV 的 `guard_path` 会标记 `rt_fast_path`、`fk_safe`、
-`fk_ik_clamp` 或 `fallback`，终端也会按路径输出分组统计。结果写入
+高度下限的区域。使用 `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 移到最低安全位置，然后只读当前高度：
@@ -146,7 +150,10 @@ uv run uf-read-tcp-z \
   --margin-mm 5
 ```
 
-该命令不会移动机械臂。把输出的 `min_tcp_z_mm` 建议值填入 GELLO YAML；测试遥操作、数据采集及其他使用该机器人配置的控制入口都会在最终下发前启用保护。关节控制下，低于下限的目标会保留 TCP 的 x/y 和姿态，只把 z 钳制到下限。
+该命令不会移动机械臂。将硬下限填入 `min_tcp_z_mm`；CPU 本地投影会在其上
+额外叠加 `tcp_z_soft_margin_mm`。xArm7 GELLO 关节路径会保留全部七个关节目标，
+只投影会穿过 TCP 软高度面的运动分量；控制器 Safety Boundary 则在硬下限处
+作为最后一道停止保护。
 
 > 该限制只保护 TCP 不低于一个水平面，不能检测机械臂连杆、肘部或夹爪外形与桌子的碰撞，也不能替代急停。更换工具、TCP 偏置、底座或桌面位置后必须重新测量。
 
diff --git a/config/gello/xarm7_gello_record_config.yaml b/config/gello/xarm7_gello_record_config.yaml
index 0d86b14..a3cf4ae 100644
--- a/config/gello/xarm7_gello_record_config.yaml
+++ b/config/gello/xarm7_gello_record_config.yaml
@@ -5,11 +5,22 @@ robot:
   control_space: "joint"
   robot_ip: "192.168.1.245"
   gripper_type: 1
+  # 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
+  realtime_control_fps: 60
   max_joint_velocity: 120
   # TCP z floor in the xArm base coordinate system (mm).
-  min_tcp_z_mm: -2.0
+  min_tcp_z_mm: 70.0
+  # CPU-local FK/Jacobian projection keeps ServoJ free of synchronous SDK queries.
+  tcp_z_guard_backend: "local_projection"
+  tcp_z_soft_margin_mm: 5.0
+  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:
@@ -27,7 +38,6 @@ robot:
       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
@@ -41,10 +51,11 @@ teleop:
 
 dataset:
   # Dataset path relative to the directory where the command is started.
-  root: "datasets/xarm7_gello_datas"
-  repo_id: "ufactory/xarm7_gello_datas"
+  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."
-  fps: 60
+  # 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
   push_to_hub: False
diff --git a/docs/gello_xarm7_smooth_safe_recording_zh.md b/docs/gello_xarm7_smooth_safe_recording_zh.md
new file mode 100644
index 0000000..61b5ca7
--- /dev/null
+++ b/docs/gello_xarm7_smooth_safe_recording_zh.md
@@ -0,0 +1,174 @@
+# 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
+  realtime_control_fps: 60
+
+  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
+
+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 uf-lerobot-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 或设备错误日志；它们用于本机诊断，不属于训练数据和源代码。
diff --git a/src/lerobot_robot_ufactory/robots/uf_robot/local_kinematics.py b/src/lerobot_robot_ufactory/robots/uf_robot/local_kinematics.py
new file mode 100644
index 0000000..73cb505
--- /dev/null
+++ b/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
diff --git a/src/lerobot_robot_ufactory/robots/uf_robot/uf_robot.py b/src/lerobot_robot_ufactory/robots/uf_robot/uf_robot.py
index 5421353..31b4697 100644
--- a/src/lerobot_robot_ufactory/robots/uf_robot/uf_robot.py
+++ b/src/lerobot_robot_ufactory/robots/uf_robot/uf_robot.py
@@ -17,6 +17,12 @@ 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__)
 
@@ -26,6 +32,14 @@ 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",
@@ -97,6 +111,7 @@ class UFRobot(Robot, Thread):
 
         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)
@@ -104,6 +119,15 @@ class UFRobot(Robot, Thread):
 
         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_safe_cartesian_target = None
         self._last_guard_path = "not_run"
@@ -197,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
@@ -216,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
@@ -233,6 +262,13 @@ 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:  
@@ -284,6 +320,15 @@ class UFRobot(Robot, Thread):
             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"
+                    )
         else:
             code, pose = self.real_arm.get_position_aa(is_radian=True)
             if code != 0 or len(pose) < 6:
@@ -297,6 +342,91 @@ class UFRobot(Robot, Thread):
         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()
@@ -320,6 +450,110 @@ class UFRobot(Robot, Thread):
             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)
+            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:
@@ -651,6 +885,68 @@ class UFRobot(Robot, Thread):
 
         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!")
+        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()
+            frame = camera.async_read()
+            after_camera_t = time.perf_counter()
+            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
+            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,
+            )
+        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)
         if (
@@ -660,11 +956,29 @@ class UFRobot(Robot, Thread):
         ):
             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 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)))
-            result = 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)
@@ -691,11 +1005,33 @@ class UFRobot(Robot, Thread):
             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 code not in (None, 0):
-            self._log_gripper_error("send_gripper_action", code, f"target={gripper_norm:.6f}")
+            self._log_gripper_error(
+                "send_gripper_action",
+                code,
+                f"target={gripper_norm:.6f}, pulse={grippos}, dt_ms={command_dt_ms:.3f}",
+            )
             return
+        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 = (
@@ -893,18 +1229,21 @@ class UFRobot(Robot, Thread):
         pass
 
     def disconnect(self) -> None:
-        if not self._is_connected:
+        if not self._is_connected and self.real_arm is None:
             return
-        self.real_arm.set_state(4) # stop
-        self.real_arm.set_mode(0)
         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
 
diff --git a/src/lerobot_robot_ufactory/robots/uf_robot/uf_robot_config.py b/src/lerobot_robot_ufactory/robots/uf_robot/uf_robot_config.py
index 1e17d3f..626b86b 100644
--- a/src/lerobot_robot_ufactory/robots/uf_robot/uf_robot_config.py
+++ b/src/lerobot_robot_ufactory/robots/uf_robot/uf_robot_config.py
@@ -18,12 +18,14 @@ class UFRobotConfig(RobotConfig):
     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"
     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
     teach_sensitivity: int | None = None  # xArm teaching sensitivity, valid range: 1-5
     joint_command_mode: int = 6  # 1: servo-angle-j, 6: online trajectory planning
+    realtime_control_fps: int = 60  # independent of camera/dataset sampling fps
     # start_joints and start_tcp_pose are intentionally disabled.
     # Reset uses the xArm SDK initial_point instead of configuration poses.
     max_joint_velocity: int = 90   # °/s, only effective in joint control mode
@@ -36,6 +38,12 @@ class UFRobotConfig(RobotConfig):
     # 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__()
@@ -49,8 +57,12 @@ class UFRobotConfig(RobotConfig):
             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.realtime_control_fps <= 0:
+            raise ValueError("realtime_control_fps must be positive")
         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 (
@@ -58,3 +70,17 @@ class UFRobotConfig(RobotConfig):
             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")
diff --git a/src/lerobot_robot_ufactory/scripts/uf_lerobot_record.py b/src/lerobot_robot_ufactory/scripts/uf_lerobot_record.py
index 15b2a92..df741c1 100644
--- a/src/lerobot_robot_ufactory/scripts/uf_lerobot_record.py
+++ b/src/lerobot_robot_ufactory/scripts/uf_lerobot_record.py
@@ -1,4 +1,5 @@
 import sys
+import csv
 import copy
 import time
 import queue
@@ -13,6 +14,7 @@ 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
 
 
@@ -350,6 +352,47 @@ def record_loop(
     manual_gripper_target = None
     manual_gripper_action_key = _manual_gripper_action_key(robot.action_features)
 
+    realtime_controller = None
+    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=getattr(robot.config, "realtime_control_fps", fps),
+            initial_observation=last_robot_cmd,
+        )
+        realtime_controller.start()
+        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')}_{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",
+                "frame_loop_ms",
+            ],
+        )
+        sync_log_writer.writeheader()
+        logging.info("Realtime dataset synchronization log: %s", sync_log_path)
+
     timestamp = 0
     start_episode_t = time.perf_counter()
     while timestamp < control_time_s:
@@ -360,7 +403,17 @@ def record_loop(
             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", time.perf_counter()
+            )
+            realtime_controller.update_observation(obs)
+            matched_action, matched_action_sent_s = realtime_controller.action_sample_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)
@@ -407,7 +460,11 @@ 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()
 
             # GELLO reports absolute joint positions. In Cartesian robot mode,
             # convert them with the xArm FK before the normal action pipeline;
@@ -421,17 +478,19 @@ def record_loop(
                 getattr(robot, "_control_space", None) == "cartesian"
                 and hasattr(robot, "joint_action_to_cartesian")
                 and joint_action_keys
+                and act is not None
                 and all(key in act for key in joint_action_keys)
             ):
                 act = robot.joint_action_to_cartesian(act)
 
             # (space mouse) from delta Cartesian cmd to absolute command
-            if "pose.dx" in act:
+            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()
@@ -460,7 +519,10 @@ 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):
@@ -474,6 +536,25 @@ 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", {})
+            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),
+                    "frame_loop_ms": f"{(time.perf_counter() - start_loop_t) * 1000:.3f}",
+                }
+            )
+            sync_frame_index += 1
+
         if display_data:
             log_rerun_data(
                 observation=obs_processed, action=action_values, compress_images=display_compressed_images
@@ -484,6 +565,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:
diff --git a/src/lerobot_robot_ufactory/scripts/uf_robot_teleop.py b/src/lerobot_robot_ufactory/scripts/uf_robot_teleop.py
index 709a29d..f6439bf 100644
--- a/src/lerobot_robot_ufactory/scripts/uf_robot_teleop.py
+++ b/src/lerobot_robot_ufactory/scripts/uf_robot_teleop.py
@@ -30,6 +30,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
@@ -283,6 +284,35 @@ def teleop_loop(cfg: TeleopConfig):
     latency_samples: list[GuardLatencyTiming] = []
     experiment_start_t = None
     previous_command_t = None
+    realtime_controller = None
+
+    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,
+            cfg.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()
@@ -293,6 +323,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')
@@ -303,6 +334,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 ==========')
@@ -315,6 +347,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:
@@ -357,19 +390,22 @@ def teleop_loop(cfg: TeleopConfig):
             if cycle_end_t - experiment_start_t >= cfg.experiment_duration_s:
                 events["exit"] = True
         else:
-            # Get robot observation
-            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))
+            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 **********")
+    stop_realtime_controller()
     if latency_samples:
         output_path = _write_guard_latency_timings(latency_samples, cfg.timing_log_dir, cfg.fps)
         print(f"Guard latency timing log: {output_path}")
diff --git a/src/lerobot_robot_ufactory/utils/realtime_teleop.py b/src/lerobot_robot_ufactory/utils/realtime_teleop.py
new file mode 100644
index 0000000..3161413
--- /dev/null
+++ b/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()
diff --git a/tests/test_local_kinematics.py b/tests/test_local_kinematics.py
new file mode 100644
index 0000000..169bc5b
--- /dev/null
+++ b/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)
diff --git a/tests/test_manual_mode.py b/tests/test_manual_mode.py
index 80d7feb..4f9aea6 100644
--- a/tests/test_manual_mode.py
+++ b/tests/test_manual_mode.py
@@ -228,6 +228,41 @@ def test_normal_mode_waits_for_gripper_to_open_before_control(monkeypatch, tmp_p
     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(
@@ -304,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()
@@ -315,7 +351,9 @@ 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()
 
@@ -336,6 +374,8 @@ def test_gripper_command_is_only_sent_after_target_changes(monkeypatch, tmp_path
         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()
@@ -344,7 +384,7 @@ def test_gripper_command_is_only_sent_after_target_changes(monkeypatch, tmp_path
     robot.send_action({"gripper.pos": 0.505})
     robot.send_action({"gripper.pos": 0.52})
 
-    writes = [call for call in arm.calls if call[0] == "getset_tgpio_modbus_data"]
+    writes = [call for call in arm.calls if call[0] == "set_gripper_position"]
     assert len(writes) == 2
     robot.disconnect()
 
diff --git a/tests/test_realtime_teleop.py b/tests/test_realtime_teleop.py
new file mode 100644
index 0000000..97fa50f
--- /dev/null
+++ b/tests/test_realtime_teleop.py
@@ -0,0 +1,88 @@
+import time
+
+import pytest
+
+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_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()