Add data replay

Fix bug in camera config
Add gripper control via keyboards in manual mode
2026-08-07 17:49:10 +08:00 · 2026-08-07 16:44:18 +08:00 · 2026-08-07 16:16:55 +08:00 · 2026-08-07 16:04:08 +08:00 · 2026-08-07 15:56:54 +08:00 · 2026-08-07 14:47:48 +08:00
31 changed files with 5909 additions and 235 deletions
--- a/.gitignore
+++ b/.gitignore
@ -88,3 +88,5 @@ models/
 *.trt
 *.xvcd
 ufactory_usage/
+.history/
+datasets/
--- a/README.md
+++ b/README.md
@ -55,12 +55,9 @@ UFACTORY robot arm integration with the LeRobot framework for robot learning, da
 git clone https://github.com/xArm-Developer/lerobot_robot_ufactory.git
 cd lerobot_robot_ufactory

-# Create conda environment
-conda create -n uf_lerobot python=3.10 -y
-conda activate uf_lerobot
-
-# Install project
-pip install -e .
+# Create a uv virtual environment and sync project dependencies
+uv venv --python 3.10
+uv sync
 ```

 Includes: `lerobot==0.4.3`, `xarm-python-sdk`, `numpy`, `pyyaml`. LeRobot already pulls in torch, opencv, wandb, etc.
@ -77,7 +74,7 @@ Dynamixel-based leader arm, joint-space control.

 ```bash
 # 1. Install GELLO module
-pip install -e ".[gello]"
+uv sync --extra gello

 # 2. Add serial port permissions (re-login required)
 sudo usermod -aG dialout $USER
@ -91,7 +88,7 @@ Pika Sense handheld + Vive Tracker, Cartesian-space control.

 ```bash
 # 1. Install peripheral deps (skip transitive deps)
-pip install pysurvive agx-pypika --no-deps
+uv pip install pysurvive agx-pypika --no-deps

 # 2. Install udev rules (re-plug devices afterwards)
 sudo cp src/rules/*.rules /etc/udev/rules.d/
@ -110,7 +107,7 @@ curl -sL https://raw.githubusercontent.com/xArm-Developer/ufactory_resources/mai
 sudo apt install -y --fix-broken

 # 2. Install peripheral deps
-pip install pysurvive --no-deps
+uv pip install pysurvive --no-deps

 # 3. Install udev rules (re-plug devices afterwards)
 sudo cp src/rules/*.rules /etc/udev/rules.d/
@ -135,7 +132,7 @@ sudo reboot

 ```bash
 # 1. Install SpaceMouse module
-pip install -e ".[spacemouse]"
+uv sync --extra spacemouse

 # 2. Install udev rules (re-plug device afterwards)
 sudo cp src/rules/*.rules /etc/udev/rules.d/
@ -151,36 +148,47 @@ Test teleop-to-robot control loop without recording.

 ```bash
 # Generic usage
-uf-robot-teleop --config_path path/to/config.yaml
-uf-robot-teleop --config_path path/to/config.yaml --fps 60  # specify frequency
+uv run uf-robot-teleop --config_path path/to/config.yaml
+uv run uf-robot-teleop --config_path path/to/config.yaml --fps 60  # specify frequency

 # Example: xArm6 + UMI teleop
-uf-robot-teleop --config_path config/umi/xarm6_umi_record_config.yaml
+uv run uf-robot-teleop --config_path config/umi/xarm6_umi_record_config.yaml
 ```

-### 2. Data Collection
+### 2. Manual Drag Data Collection
+
+Manual drag recording uses `manual_mode: true` in the robot configuration and does not configure a teleoperator. During recording, the actual joint state is written as both the observation and action in the LeRobot dataset. When a gripper is configured, hold `C` to close it slowly and `O` to open it slowly. Adjust the speed with `manual_gripper_speed`, which defaults to `0.5`:
+
+```bash
+./start_manual_record.sh
+./start_manual_record.sh -r
+```
+
+The launcher creates a dataset on its first run and automatically resumes an existing valid dataset. If it reports an empty or incomplete dataset directory, choose a new `dataset.root` or remove that directory after confirming it contains no data. Press `Space` to start an episode, `Right` to save it, `Left` to discard and re-record it, and `Esc` to stop recording. Reset the arm manually between episodes.
+
+### 3. Teleop Data Collection

 Record datasets via teleop.

 ```bash
 # Generic usage
-uf-lerobot-record --config_path path/to/record_config.yaml
-uf-lerobot-record --config_path path/to/config.yaml --resume true        # resume recording
+uv run uf-lerobot-record --config_path path/to/record_config.yaml
+uv run uf-lerobot-record --config_path path/to/config.yaml --resume true        # resume recording

 # Example: xArm6 + UMI data collection
-uf-lerobot-record --config_path config/umi/xarm6_umi_record_config.yaml
+uv run uf-lerobot-record --config_path config/umi/xarm6_umi_record_config.yaml
 ```

-### 3. Policy Training
+### 5. Policy Training

 Train imitation learning policies on collected data.

 ```bash
 # Generic usage
-lerobot-train --policy act --dataset your_dataset_name
+uv run lerobot-train --policy act --dataset your_dataset_name

 # Example: train ACT on xArm6 UMI dataset
-lerobot-train --policy act --dataset ufactory/xarm6_umi_datas
+uv run lerobot-train --policy act --dataset ufactory/xarm6_umi_datas
 ```

 Important parameters:
@ -189,7 +197,7 @@ Important parameters:
 # Note: repo_id is the same as in the record config
 # Policy type: ACT, training steps: 800k
 # Checkpoints saved every 20k steps, output to lerobot_datas/train (sibling of lerobot directory)
-lerobot-train \
+uv run lerobot-train \
  --dataset.root=../../../../lerobot_datas/record/ufactory/xarm6_umi_datas \
  --dataset.repo_id=ufactory/xarm6_umi_datas \
  --policy.type=act \
@ -202,16 +210,16 @@ lerobot-train \
  --save_freq=20000
 ```

-### 4. Inference & Evaluation
+### 6. Inference & Evaluation

 Run inference with a trained policy.

 ```bash
 # Generic usage
-uf-lerobot-eval --config_path path/to/config.yaml --policy.path your_train_path
+uv run uf-lerobot-eval --config_path path/to/config.yaml --policy.path your_train_path

 # Example: run inference with trained ACT policy
-uf-lerobot-eval --config_path config/umi/xarm6_umi_record_config.yaml --policy.path ../../../../lerobot_datas/train/xarm6_umi_datas/checkpoints/last/pretrained_model/
+uv run uf-lerobot-eval --config_path config/umi/xarm6_umi_record_config.yaml --policy.path ../../../../lerobot_datas/train/xarm6_umi_datas/checkpoints/last/pretrained_model/
 ```

 ## Tools
@ -221,11 +229,11 @@ uf-lerobot-eval --config_path config/umi/xarm6_umi_record_config.yaml --policy.p
 View and stitch multiple camera feeds.

 ```bash
-uf-camera-view -l                           # list all cameras
-uf-camera-view -l -T xvisio                 # list XVisio cameras only
-uf-camera-view -T xvisio                    # view XVisio cameras (default 1280x1280 YU12)
-uf-camera-view -T xvisio -W 640 -H 1920 -F NV12  # specify format
-uf-camera-view -T other                     # view other camera types
+uv run uf-camera-view -l                           # list all cameras
+uv run uf-camera-view -l -T xvisio                 # list XVisio cameras only
+uv run uf-camera-view -T xvisio                    # view XVisio cameras (default 1280x1280 YU12)
+uv run uf-camera-view -T xvisio -W 640 -H 1920 -F NV12  # specify format
+uv run uf-camera-view -T other                     # view other camera types
 ```

 ### 2. LeRobot Dataset Tools
@ -235,7 +243,7 @@ LeRobot provides dataset utilities for inspecting, editing and managing collecte
 #### View an episode:
 e.g. view episode index 17:
 ```bash
-lerobot-dataset-viz \
+uv run lerobot-dataset-viz \
  --root=../../../../lerobot_datas/record/ufactory/xarm7_record_datas \
  --repo-id ufactory/xarm7_record_datas \
  --display-compressed-images true \
@ -245,7 +253,7 @@ lerobot-dataset-viz \
 #### Delete specific episodes:
 e.g. delete episodes 18 and 19:
 ```bash
-lerobot-edit-dataset \
+uv run lerobot-edit-dataset \
  --root=../../../../lerobot_datas/record/ufactory/xarm7_record_datas \
  --repo_id ufactory/xarm7_record_datas \
  --new_repo_id ../xarm7_record_datas_new \
@ -255,7 +263,7 @@ lerobot-edit-dataset \

 #### Merge datasets:
 ```bash
-lerobot-edit-dataset \
+uv run lerobot-edit-dataset \
  --root=../../../../lerobot_datas/record \
  --repo_id ufactory/xarm7_record_datas_merge_1_2 \
  --operation.type merge \
--- a/README_ZH.md
+++ b/README_ZH.md
@ -55,12 +55,9 @@ UFACTORY(深圳市众为创造科技有限公司) 机械臂与 LeRobot 框架集
 git clone https://github.com/xArm-Developer/lerobot_robot_ufactory.git
 cd lerobot_robot_ufactory

-# 创建 conda 环境
-conda create -n uf_lerobot python=3.10 -y
-conda activate uf_lerobot
-
-# 安装项目
-pip install -e .
+# 创建 uv 虚拟环境并同步项目依赖
+uv venv --python 3.10
+uv sync
 ```

 包含：`lerobot==0.4.3`、`xarm-python-sdk`、`numpy`、`pyyaml`（lerobot 已自动携带 torch、opencv、wandb 等训练相关依赖）。
@ -77,7 +74,7 @@ pip install -e .

 ```bash
 # 1. 安装 GELLO 模块
-pip install -e ".[gello]"
+uv sync --extra gello

 # 2. 添加串口权限（重新登录后生效）
 sudo usermod -aG dialout $USER
@ -91,7 +88,7 @@ sudo usermod -aG dialout $USER

 ```bash
 # 1. 安装外设依赖（不需要它们的间接依赖）
-pip install pysurvive agx-pypika --no-deps
+uv pip install pysurvive agx-pypika --no-deps

 # 2. 安装 udev 规则（重新插拔设备后生效）
 sudo cp rules/*.rules /etc/udev/rules.d/
@ -110,7 +107,7 @@ curl -sL https://raw.githubusercontent.com/xArm-Developer/ufactory_resources/mai
 sudo apt install -y --fix-broken

 # 2. 安装外设依赖
-pip install pysurvive --no-deps
+uv pip install pysurvive --no-deps

 # 3. 安装 udev 规则（重新插拔设备后生效）
 sudo cp rules/*.rules /etc/udev/rules.d/
@ -135,7 +132,7 @@ sudo reboot

 ```bash
 # 1. 安装 SpaceMouse 模块
-pip install -e ".[spacemouse]"
+uv sync --extra spacemouse

 # 2. 安装 udev 规则（重新插拔设备后生效）
 sudo cp rules/*.rules /etc/udev/rules.d/
@ -150,33 +147,60 @@ sudo udevadm control --reload-rules && sudo udevadm trigger

 ```bash
 # 通用格式
-uf-robot-teleop --config_path path/to/config.yaml
-uf-robot-teleop --config_path path/to/config.yaml --fps 60  # 指定频率
+uv run uf-robot-teleop --config_path path/to/config.yaml
+uv run uf-robot-teleop --config_path path/to/config.yaml --fps 60  # 指定频率

 # 示例: xArm6 + UMI 遥操作
-uf-robot-teleop --config_path config/umi/xarm6_umi_record_config.yaml
+uv run uf-robot-teleop --config_path config/umi/xarm6_umi_record_config.yaml
 ```

-### 2. 数据采集
+### 2. 人工拖拽数据采集
+
+人工拖拽录制使用 robot 配置中的 `manual_mode: true`，不需要配置 teleop。录制过程中，机械臂的实际关节状态会作为 observation 和 action 写入 LeRobot 数据集；如果配置了夹爪，还可以按住 `C` 缓慢闭合、按住 `O` 缓慢张开。夹爪速度通过 `manual_gripper_speed` 配置，默认值为 `0.5`：
+
+```bash
+./start_manual_record.sh
+./start_manual_record.sh -r
+```
+
+启动脚本首次运行时创建数据集，检测到有效数据集时会自动续录。如果提示数据目录为空或不完整，请修改 `dataset.root`，或者确认没有数据后删除该目录。按 `Space` 开始当前 episode，按 `Right` 保存，按 `Left` 放弃并重录当前 episode，按 `Esc` 停止录制。episode 之间可以手动复位机械臂。
+
+### 3. 遥操作数据采集

 通过遥操作录制数据集。

 ```bash
 # 通用格式
-uf-lerobot-record --config_path path/to/record_config.yaml
-uf-lerobot-record --config_path path/to/config.yaml --resume true     # 续录
+uv run uf-lerobot-record --config_path path/to/record_config.yaml
+uv run uf-lerobot-record --config_path path/to/config.yaml --resume true     # 续录

 # 示例: xArm6 + UMI 数据采集
-uf-lerobot-record --config_path config/umi/xarm6_umi_record_config.yaml
+uv run uf-lerobot-record --config_path config/umi/xarm6_umi_record_config.yaml
 ```

-### 3. Lerobot训练
+### 4. 数据重放
+
+`datasets/xarm7_manual_replay` 是人工拖拽录制的 LeRobot 数据集。回放脚本使用其中的
+`observation.state`，将 7 个关节弧度值和归一化夹爪位置作为**绝对目标值**发送给 xArm7，
+不会将相邻帧相减，也不会累加成相对动作。脚本默认按数据集的 30 FPS 播放一个 episode。
+
+启动前会要求确认，连接后会先自动移动到 xArm SDK 初始点；播放结束后保持最后一帧姿态并断开连接：
+
+```bash
+uv run uf-lerobot-replay \
+  --dataset-root /home/wsx/code/lerobot_robot_ufactory/datasets/xarm7_manual_replay \
+  --robot-ip 192.168.1.245
+```
+
+无人值守运行时可以使用 `--yes` 跳过确认。执行前请确认机械臂工作空间无障碍物，且数据中的初始姿态与当前设备匹配。
+
+### 5. Lerobot训练

 采集数据后，使用 LeRobot 训练管道进行模仿学习训练。

 ```bash
 # 通用格式
-lerobot-train --policy act --dataset your_dataset_name
+uv run lerobot-train --policy act --dataset your_dataset_name
 ```

 参数示例：
@ -185,7 +209,7 @@ lerobot-train --policy act --dataset your_dataset_name
 # 注意: repo_id就是采集时配置文件里面的repo_id
 # 这里训练策略policy.type选用act，训练steps为80w次
 # 训练过程每2w次保存一次结果，结果输出到和lerobot同级目录下的lerobot_datas/train里面
-lerobot-train \
+uv run lerobot-train \
  --dataset.root=../../../../lerobot_datas/record/ufactory/xarm6_umi_datas \
  --dataset.repo_id=ufactory/xarm6_umi_datas \
  --policy.type=act \
@ -198,16 +222,16 @@ lerobot-train \
  --save_freq=20000 
 ```

-### 4. 推理
+### 6. 推理

 指定模型进行推理

 ```bash
 # 通用格式
-uf-lerobot-eval --config_path path/to/config.yaml --policy.path your_train_path
+uv run uf-lerobot-eval --config_path path/to/config.yaml --policy.path your_train_path

 # 示例：使用训练好的 ACT 策略进行推理
-uf-lerobot-eval --config_path config/umi/xarm6_umi_record_config.yaml --policy.path ../../../../lerobot_datas/train/xarm6_umi_datas/checkpoints/last/pretrained_model/
+uv run uf-lerobot-eval --config_path config/umi/xarm6_umi_record_config.yaml --policy.path ../../../../lerobot_datas/train/xarm6_umi_datas/checkpoints/last/pretrained_model/
 ```

 ## 工具集
@ -217,11 +241,11 @@ uf-lerobot-eval --config_path config/umi/xarm6_umi_record_config.yaml --policy.p
 查看和拼接多路摄像头画面。

 ```bash
-uf-camera-view -l                           # 列出所有摄像头
-uf-camera-view -l -T xvisio                 # 仅列出 XVisio 摄像头
-uf-camera-view -T xvisio                    # 查看 XVisio 摄像头（默认 1280x1280 YU12）
-uf-camera-view -T xvisio -W 640 -H 1920 -F NV12  # 指定格式
-uf-camera-view -T other                     # 查看其他类型摄像头
+uv run uf-camera-view -l                           # 列出所有摄像头
+uv run uf-camera-view -l -T xvisio                 # 仅列出 XVisio 摄像头
+uv run uf-camera-view -T xvisio                    # 查看 XVisio 摄像头（默认 1280x1280 YU12）
+uv run uf-camera-view -T xvisio -W 640 -H 1920 -F NV12  # 指定格式
+uv run uf-camera-view -T other                     # 查看其他类型摄像头
 ```

 ### 2. Lerobot数据集工具
@ -230,7 +254,7 @@ Lerobot提供一些数据集工具，方便对采集的数据集进行增删查
 ### 查看某个索引的episode:
 例如查看索引号为17的episode:
 ```bash
-lerobot-dataset-viz \
+uv run lerobot-dataset-viz \
  --root=../../../../lerobot_datas/record/ufactory/xarm7_record_datas \
  --repo-id ufactory/xarm7_record_datas \
  --display-compressed-images true \
@ -240,7 +264,7 @@ lerobot-dataset-viz \
 ### 删除某些索引的episodes:
 例如删除索引号为18和19的episode:
 ```bash
-lerobot-edit-dataset \
+uv run lerobot-edit-dataset \
  --root=../../../../lerobot_datas/record/ufactory/xarm7_record_datas \
  --repo_id ufactory/xarm7_record_datas \
  --new_repo_id ../xarm7_record_datas_new \
@ -250,7 +274,7 @@ lerobot-edit-dataset \

 ### 合并数据集
 ```bash
-lerobot-edit-dataset \
+uv run lerobot-edit-dataset \
  --root=../../../../lerobot_datas/record \
  --repo_id ufactory/xarm7_record_datas_merge_1_2 \
  --operation.type merge \
@ -312,4 +336,3 @@ lerobot_robot_ufactory/
 ## 许可证

 本项目基于 Apache License 2.0 发布，详见 [LICENSE](LICENSE) 文件。
-
--- a/config/gello/xarm5_gello_record_config.yaml
+++ b/config/gello/xarm5_gello_record_config.yaml
@ -3,9 +3,8 @@ robot:
  id: "uf_robot"
  robot_dof: 5
  control_space: "joint"
-  robot_ip: "192.168.1.75"
+  robot_ip: "192.168.1.245"
  gripper_type: 1
-  start_joints: [0, 0, -90, 90, 0]

 # make sure to edit with your correct configurations!
 teleop:
@ -15,7 +14,6 @@ teleop:
  joint_signs: [1, 1, -1, 1, 1]
  start_joints: [0, 0, -90, 90, 0]
  gripper_id: 8
-  torque_joint_ids: [3, 5]

 dataset:
  # root of local repo: /home/<user_name>/.cache/huggingface/lerobot (default)
--- a/config/gello/xarm6_gello_record_config.yaml
+++ b/config/gello/xarm6_gello_record_config.yaml
@ -3,9 +3,8 @@ robot:
  id: "uf_robot"
  robot_dof: 6
  control_space: "joint"
-  robot_ip: "192.168.1.68"
+  robot_ip: "192.168.1.245"
  gripper_type: 1
-  start_joints: [0, 0, -90, 0, 90, 0]

 # make sure to edit with your correct configurations!
 teleop:
@ -15,7 +14,6 @@ teleop:
  joint_signs: [1, 1, -1, 1, 1, 1]
  start_joints: [0, 0, -90, 0, 90, 0]
  gripper_id: 8
-  torque_joint_ids: [3]

 dataset:
  # root of local repo: /home/<user_name>/.cache/huggingface/lerobot (default)
--- a/config/gello/xarm7_gello_record_config.yaml
+++ b/config/gello/xarm7_gello_record_config.yaml
@ -3,16 +3,18 @@ robot:
  id: "uf_robot"
  robot_dof: 7
  control_space: "joint"
-  robot_ip: "192.168.1.85"
+  robot_ip: "192.168.1.245"
  gripper_type: 1
-  start_joints: [0, 0, 0, 90, 0, 90, 0]
+  # 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"
-  start_joints: [0, 0, 0, 90, 0, 90, 0]
+  # GELLO calibration reference; matches the xArm SDK initial point.
+  start_joints: [0, -30, 0, 0, 0, 30, 0]

 dataset:
  # root of local repo: /home/<user_name>/.cache/huggingface/lerobot (default)
--- a/config/manual_mode/xarm7_manual_record_config.yaml
+++ b/config/manual_mode/xarm7_manual_record_config.yaml
@ -0,0 +1,41 @@
+robot:
+  type: uf::robot
+  id: "uf_manual_robot"
+  robot_dof: 7
+  control_space: "joint"
+  # IP address of the xArm controller.
+  robot_ip: "192.168.1.245"
+  # Gripper type: 1 is the xArm gripper.
+  gripper_type: 1
+  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
+  # Whether to record joint velocities in observations.
+  observe_joint_vel: false
+
+  cameras:
+    camera:
+      type: intelrealsense
+      serial_number_or_name: "148522072685"
+      width: 640
+      height: 480
+      fps: 30
+
+dataset:
+  root: "/home/wsx/code/lerobot_robot_ufactory/datasets/xarm7_manual_replay_pick_pen"
+  repo_id: "ufactory/xarm7_manual_datas"
+  # Task description stored with each recorded frame.
+  single_task: "Describe the task being demonstrated."
+  fps: 30
+  episode_time_s: 60
+  # Reserved reset duration between episodes in seconds.
+  reset_time_s: 20
+  # Number of episodes to record.
+  num_episodes: 50
+  # Store camera observations as videos.
+  video: true
+  push_to_hub: false
+
+
--- a/config/pika/xarm7_pika_record_config.yaml
+++ b/config/pika/xarm7_pika_record_config.yaml
@ -6,8 +6,6 @@ robot:
  robot_ip: "192.168.1.85"
  gripper_type: 10
  max_linear_velocity: 200
-  start_joints: [-0.2, -13.9, -0.3, 52.6, 0.9, 66.6, 0.2]
-  start_tcp_pose: [400, 0, 400, 180, 0, 0]
  cameras:
    fisheye:
      type: opencv
--- a/config/umi/dual_xarm6_umi_record_config.yaml
+++ b/config/umi/dual_xarm6_umi_record_config.yaml
@ -13,8 +13,6 @@ robot:
      robot_ip: "192.168.1.29"
      gripper_type: 2
      max_linear_velocity: 200
-      start_joints: [-14.7, 25.6, -49.6, -282.5, 95.9, -65.9]
-      start_tcp_pose: [400, 0, 150, -90, 0, 0]
      cameras:
        fisheye:
          type: opencv
@ -31,8 +29,6 @@ robot:
      robot_ip: "192.168.1.83"
      gripper_type: 2
      max_linear_velocity: 200
-      start_joints: [14.5, 25.5, -49.6, -76.8, 95.9, 66.6]
-      start_tcp_pose: [400, 0, 150, 90, 0, 0]
      cameras:
        fisheye:
          type: opencv
@ -75,4 +71,3 @@ dataset:
  num_episodes: 100
  push_to_hub: False

-
--- a/config/umi/xarm6_umi_record_config.yaml
+++ b/config/umi/xarm6_umi_record_config.yaml
@ -6,8 +6,6 @@ robot:
  robot_ip: "192.168.1.83"
  gripper_type: 2
  max_linear_velocity: 250
-  start_joints: [0, -14.8, -52.6, 0, 67.4, 0]
-  start_tcp_pose: [400, 0, 400, 180, 0, 0]
  cameras:
    fisheye:
      type: opencv
--- a/pyproject.toml
+++ b/pyproject.toml
@ -25,8 +25,9 @@ classifiers = [
 ]
 dependencies = [
    "numpy>=1.24",
+    "pyarrow>=14.0",
    "pyyaml",
-    "lerobot==0.4.3",
+    "lerobot[intelrealsense]==0.4.3",
    "xarm-python-sdk",
    "opencv-python",
 ]
@ -35,8 +36,10 @@ dependencies = [
 uf-robot-teleop = "lerobot_robot_ufactory.scripts.uf_robot_teleop:main"
 uf-lerobot-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"
 uf-vive-calibrate = "lerobot_robot_ufactory.scripts.vive_calibrate:main"
 uf-camera-view = "lerobot_robot_ufactory.scripts.uf_camera_view:main"
+uf-camera-test = "lerobot_robot_ufactory.scripts.uf_camera_test:main"

 [project.optional-dependencies]
 # GELLO 遥操作
--- a/src/lerobot_robot_ufactory/devices/umi/xvlib/libtbb.so.2
+++ b/src/lerobot_robot_ufactory/devices/umi/xvlib/libtbb.so.2
--- a/src/lerobot_robot_ufactory/devices/umi/xvlib/xvlib.py
+++ b/src/lerobot_robot_ufactory/devices/umi/xvlib/xvlib.py
@ -569,6 +569,36 @@ class XVLib:
            return sys_lib
        return None

+    @classmethod
+    def _load_tbb_lib(cls, lib_dir):
+        """预加载 libtbb.so.2，解决 bundled OpenCV 的 TBB 依赖。
+
+        策略: 系统路径(apt 安装) → 本地 bundled 版本。
+        bundled libopencv_core.so.4.2 依赖旧版 libtbb.so.2 (Intel TBB 2.x ABI)，
+        而 Ubuntu 22.04+ 的 oneAPI TBB (libtbb.so.12) 不兼容，必须显式预加载。
+        """
+        tbb_paths = [
+            '/usr/lib/x86_64-linux-gnu/libtbb.so.2',
+            '/usr/lib/libtbb.so.2',
+            '/usr/local/lib/libtbb.so.2',
+        ]
+        for p in tbb_paths:
+            if os.path.exists(p):
+                try:
+                    ctypes.CDLL(p, mode=ctypes.RTLD_GLOBAL)
+                    logger.info(f'Loaded libtbb.so.2 from system ({p})')
+                    return
+                except OSError:
+                    pass
+
+        local_path = os.path.join(lib_dir, 'libtbb.so.2')
+        if os.path.exists(local_path):
+            ctypes.CDLL(local_path, mode=ctypes.RTLD_GLOBAL)
+            logger.info('Loaded libtbb.so.2 from local (bundled)')
+            return
+
+        logger.warning('libtbb.so.2 not found, OpenCV loading may fail')
+
    @classmethod
    def _load_opencv_lib(cls, lib_dir, fname):
        """加载 opencv .so: 系统(精确版本) → 本地目录."""
@ -600,6 +630,7 @@ class XVLib:
            cls._check_xvsdk()
            lib_dir = os.path.dirname(__file__)

+            cls._load_tbb_lib(lib_dir)
            cls._load_opencv_lib(lib_dir, 'libopencv_core.so.4.2')
            cls._load_opencv_lib(lib_dir, 'libopencv_imgproc.so.4.2')

--- a/src/lerobot_robot_ufactory/robots/uf_mock_robot/multiple_uf_mock_robot.py
+++ b/src/lerobot_robot_ufactory/robots/uf_mock_robot/multiple_uf_mock_robot.py
@ -57,6 +57,10 @@ class MultipleUFMockRobot(Robot):
        for robot in self.robots.values():
            robot.configure()

+    def reset_to_initial(self) -> None:
+        for robot in self.robots.values():
+            robot.reset_to_initial()
+
    def disconnect(self) -> None:
        for robot in self.robots.values():
            robot.disconnect()
--- a/src/lerobot_robot_ufactory/robots/uf_mock_robot/uf_mock_robot.py
+++ b/src/lerobot_robot_ufactory/robots/uf_mock_robot/uf_mock_robot.py
@ -93,6 +93,9 @@ class UFMockRobot(Robot):
    def configure(self) -> None:
        pass

+    def reset_to_initial(self) -> None:
+        pass
+
    def calibrate(self) -> None:
        self._is_calibrated = True
        pass # CHECK! currently No-op
@ -130,4 +133,3 @@ class UFMockRobot(Robot):
        return self._is_connected


-
--- a/src/lerobot_robot_ufactory/robots/uf_robot/multiple_uf_robot.py
+++ b/src/lerobot_robot_ufactory/robots/uf_robot/multiple_uf_robot.py
@ -101,6 +101,30 @@ class MultipleUFRobot(Robot):
            for robot in self.robots.values():
                robot.configure()

+    def reset_to_initial(self) -> None:
+        """Reset every arm to its SDK-configured initial point."""
+        if self._is_async_configure:
+            errors = []
+
+            def reset_robot(robot):
+                try:
+                    robot.reset_to_initial()
+                except BaseException as exc:
+                    errors.append(exc)
+
+            threads = []
+            for robot in self.robots.values():
+                thread = threading.Thread(target=reset_robot, args=(robot,), daemon=True)
+                threads.append(thread)
+                thread.start()
+            for thread in threads:
+                thread.join()
+            if errors:
+                raise errors[0]
+        else:
+            for robot in self.robots.values():
+                robot.reset_to_initial()
+
    def disconnect(self) -> None:
        for robot in self.robots.values():
            robot.disconnect()
--- 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,7 @@ from xarm.core.utils import convert

 ## Configurations:
 INIT_SYNC_JOINT_VELOCITY_RAD = 0.2
+ROBOT_RESET_SPEED_DEG = 60

 CARTESIAN_OBS_KEYS = [
    "pose.x", "pose.y", "pose.z", "pose.rx", "pose.ry", "pose.rz",
@ -75,6 +76,7 @@ class UFRobot(Robot, Thread):
        self._control_space = self.config.control_space

        self.real_arm = None
+        self._initial_point = None
        cameras_args = self.config.cameras_args or {}
        self.camera_width = cameras_args.get('w', 0)
        self.camera_height = cameras_args.get('h', 0)
@ -90,15 +92,6 @@ class UFRobot(Robot, Thread):
        self._max_joint_velocity = math.radians(self.config.max_joint_velocity)
        self._max_linear_velocity = self.config.max_linear_velocity

-        if self.config.start_tcp_pose and len(self.config.start_tcp_pose) >= 6:
-            self._start_tcp_pose = list(self.config.start_tcp_pose[:3]) + list(map(math.radians, self.config.start_tcp_pose[3:6]))
-        else:
-            self._start_tcp_pose = None
-        if self.config.start_joints:
-            self._start_joints = list(map(math.radians, self.config.start_joints))
-        else:
-            self._start_joints = None
-
        self.report_stop_event = Event()
        self._rt_report_normal = False
        self._update_lock = Lock()
@ -193,6 +186,13 @@ class UFRobot(Robot, Thread):
            self._is_connected = False
            raise ConnectionError()

+        code, initial_point = self.real_arm.get_initial_point()
+        if code != 0:
+            raise RuntimeError(f"get_initial_point failed, code={code}")
+        if initial_point is None or len(initial_point) < self._dof:
+            raise RuntimeError(f"Invalid initial point returned by xArm: {initial_point}")
+        self._initial_point = list(initial_point[:self._dof])
+
        for cam in self.cameras.values():
            cam.connect()
            self._is_connected = self._is_connected and cam.is_connected
@ -206,7 +206,10 @@ class UFRobot(Robot, Thread):
        #         print('Could not connect to pika gripper.')
        #         raise ConnectionError()

-        self.configure()
+        if self.config.manual_mode:
+            self.configure()
+        else:
+            self.reset_to_initial()
        if calibrate:  
            self.calibrate()

@ -214,6 +217,33 @@ class UFRobot(Robot, Thread):

        self._is_connected = True

+    def reset_to_initial(self) -> None:
+        if not self._is_connected or self.real_arm is None:
+            raise ConnectionError("UF Robot is not connected")
+        if self._initial_point is None:
+            raise RuntimeError("xArm initial point has not been loaded")
+
+        # The controller requires motion to be enabled again after an
+        # emergency stop has been released, before any reset motion command.
+        code = self.real_arm.motion_enable(enable=True)
+        self._check_motion_code("motion_enable", code)
+        code = self.real_arm.clean_error()
+        self._check_motion_code("clean_error", code)
+        code = self.real_arm.set_mode(0)
+        self._check_motion_code("set_mode(0)", code)
+        code = self.real_arm.set_state(0)
+        self._check_motion_code("set_state(0)", code)
+        code = self.real_arm.set_servo_angle(
+            angle=self._initial_point,
+            speed=ROBOT_RESET_SPEED_DEG,
+            is_radian=False,
+            wait=True,
+        )
+        if code != 0:
+            raise RuntimeError(f"Failed to move to xArm initial point, code={code}")
+
+        self.configure()
+
    def configure(self) -> None:
        self.real_arm.motion_enable()
        self.real_arm.clean_error()
@ -226,46 +256,32 @@ class UFRobot(Robot, Thread):
            raise RuntimeError(f"Failed to set correct state to UF robot! Controller Error code: {err_warn[0]} !")

        if self._gripper_type > GripperType.NoGripper:
-            self.real_arm._arm._baud_checkset = True
-            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.real_arm.set_gripper_position(self._gripper_param.open_pos)
-            elif self._gripper_type == GripperType.xArmGripperG2:
-                self.real_arm.set_gripper_enable(True)
-                self.real_arm.set_gripper_mode(0)
-                self.real_arm.set_gripper_g2_position(self._gripper_param.open_pos)
-            elif self._gripper_type == GripperType.BioGripperG2:
-                _, mode = self.real_arm.get_bio_gripper_control_mode()
-                if mode != 1:
-                    self.real_arm.set_bio_gripper_control_mode(1)
-                self.real_arm.set_bio_gripper_enable(True)
-                self.real_arm.open_bio_gripper()
-            elif self._gripper_type == GripperType.PikaGripper:
-                self.pika_gripper.enable()
-                time.sleep(0.5)
-                self.pika_gripper.set_gripper_distance(self._gripper_param.open_pos)
-            elif self._gripper_type == GripperType.RobotiqGripper:
-                self.real_arm.robotiq_reset()
-                self.real_arm.robotiq_set_activate(wait=True)
-                self.real_arm.robotiq_set_position(self._gripper_param.open_pos, wait=True)
-            self._gripper_param.grippos = self._gripper_param.open_pos
-            self._gripper_param.gripper_norm = self._gripper_param.open_pos
-            self.real_arm._arm._baud_checkset = False
+            self._configure_gripper(move_to_open=not self.config.manual_mode)
+
+        if self.config.manual_mode:
+            if self.config.teach_sensitivity is not None:
+                code = self.real_arm.set_teach_sensitivity(self.config.teach_sensitivity)
+                if code != 0:
+                    raise RuntimeError(f"set_teach_sensitivity failed, code={code}")
+
+            code = self.real_arm.set_mode(2)
+            if code != 0:
+                raise RuntimeError(f"set_mode(2) failed, code={code}")
+            code = self.real_arm.set_state(0)
+            if code != 0:
+                raise RuntimeError(f"set_state(0) failed, code={code}")
+
            _, err_warn = self.real_arm.get_err_warn_code()
            if err_warn[0] != 0:
-                raise RuntimeError(f"Failed to set correct state to Gripper! Controller Error code: {err_warn[0]} !")
-        
-        if self._start_joints is not None:
-            self.real_arm.set_servo_angle(angle=self._start_joints, is_radian=True, wait=True)
-        if self._start_tcp_pose is not None:
-            self.real_arm.set_position(*self._start_tcp_pose, speed=100, is_radian=True, wait=True)
-            _, self._start_joints = self.real_arm.get_servo_angle(is_radian=True)
-            self._start_tcp_pose = None
+                raise RuntimeError(
+                    f"Failed to set manual mode for UF robot! Controller Error code: {err_warn[0]} !"
+                )
+            return

        if self._control_space == "joint":
-            self.real_arm.set_mode(6)
+            code = self.real_arm.set_mode(self.config.joint_command_mode)
+            if code != 0:
+                raise RuntimeError(f"set_mode({self.config.joint_command_mode}) failed, code={code}")
        elif self._control_space == "cartesian":
            self.real_arm.set_mode(7)
        else:
@ -281,6 +297,49 @@ class UFRobot(Robot, Thread):
            self.start()
        time.sleep(0.2)

+    def _configure_gripper(self, move_to_open: bool) -> None:
+        """Initialize the configured gripper without moving it in manual mode."""
+        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)
+                if move_to_open:
+                    self.real_arm.set_gripper_position(self._gripper_param.open_pos)
+            elif self._gripper_type == GripperType.xArmGripperG2:
+                self.real_arm.set_gripper_enable(True)
+                self.real_arm.set_gripper_mode(0)
+                if move_to_open:
+                    self.real_arm.set_gripper_g2_position(self._gripper_param.open_pos)
+            elif self._gripper_type == GripperType.BioGripperG2:
+                _, mode = self.real_arm.get_bio_gripper_control_mode()
+                if mode != 1:
+                    self.real_arm.set_bio_gripper_control_mode(1)
+                self.real_arm.set_bio_gripper_enable(True)
+                if move_to_open:
+                    self.real_arm.open_bio_gripper()
+            elif self._gripper_type == GripperType.PikaGripper:
+                self.pika_gripper.enable()
+                if move_to_open:
+                    time.sleep(0.5)
+                    self.pika_gripper.set_gripper_distance(self._gripper_param.open_pos)
+            elif self._gripper_type == GripperType.RobotiqGripper:
+                self.real_arm.robotiq_reset()
+                self.real_arm.robotiq_set_activate(wait=True)
+                if move_to_open:
+                    self.real_arm.robotiq_set_position(self._gripper_param.open_pos, wait=True)
+        finally:
+            self.real_arm._arm._baud_checkset = False
+
+        _, err_warn = self.real_arm.get_err_warn_code()
+        if err_warn[0] != 0:
+            raise RuntimeError(f"Failed to set correct state to Gripper! Controller Error code: {err_warn[0]} !")
+
+        if move_to_open:
+            self._gripper_param.grippos = self._gripper_param.open_pos
+            self._gripper_param.gripper_norm = 0.0
+
    def calibrate(self) -> None:
        self._is_calibrated = True
        pass # CHECK! currently No-op
@ -350,9 +409,64 @@ class UFRobot(Robot, Thread):

        return obs_dict

+    def _send_gripper_action(self, gripper_norm: float) -> None:
+        gripper_norm = min(max(float(gripper_norm), 0.0), 1.0)
+        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)
+        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)
+            modbus_datas = [0x08, 0x10, 0x0C, 0x00, 0x00, 0x05, 0x0A, 0x00, 0x01]
+            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)
+        elif self._gripper_type == GripperType.BioGripperG2:
+            grippos = self._gripper_param.get_grippos(gripper_norm)
+            grippos = int(grippos * 3.7342 - 265.13)
+            modbus_datas = [0x08, 0x10, 0x0C, 0x00, 0x00, 0x05, 0x0A, 0x00, 0x01]
+            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)
+        elif self._gripper_type == GripperType.PikaGripper:
+            grippos = self._gripper_param.get_grippos(gripper_norm)
+            self.pika_gripper.set_gripper_distance(grippos)
+        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)
+
+    def _motion_status(self) -> str:
+        """Return controller state details for a failed motion command."""
+        arm = self.real_arm
+        mode = getattr(arm, "mode", "unknown")
+        state = getattr(arm, "state", "unknown")
+        error_code = getattr(arm, "error_code", "unknown")
+        warn_code = getattr(arm, "warn_code", "unknown")
+        return f"mode={mode}, state={state}, error_code={error_code}, warn_code={warn_code}"
+
+    def _check_motion_code(self, command: str, code: int) -> None:
+        """Fail loudly when the SDK rejects a joint command."""
+        if code != 0:
+            raise RuntimeError(f"{command} failed, code={code}, {self._motion_status()}")
+
    def send_action(self, action: dict) -> np.ndarray:
        if not self._is_connected:
            raise ConnectionError()
+        if self.config.manual_mode:
+            gripper_key = f"{self.prefix}gripper.pos"
+            if (
+                self._gripper_type > GripperType.NoGripper
+                and gripper_key in action
+                and self.real_arm.error_code == 0
+                and not self.config.no_action
+            ):
+                self._send_gripper_action(action[gripper_key])
+            return action
        if self.real_arm.error_code != 0:
            return action
        if self.config.no_action:
@ -368,17 +482,43 @@ class UFRobot(Robot, Thread):
            for i in range(self._dof):
                cmd_list[i] = action[f"{self.prefix}J{i+1}.pos"]

-            # TODO: make mode 6 compatible with wait=True
-            if wait_== False and self.real_arm.mode != 6:
-                self.real_arm.set_mode(6)
-                self.real_arm.set_state(0)
-                time.sleep(0.1)
-            elif wait_ and self.real_arm.mode != 0:
-                self.real_arm.set_mode(0)
-                self.real_arm.set_state(0)
-                time.sleep(0.1)
+            if 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).
+                if self.real_arm.mode != 1:
+                    code = self.real_arm.set_mode(1)
+                    self._check_motion_code("set_mode(1)", code)
+                    code = self.real_arm.set_state(0)
+                    self._check_motion_code("set_state(0)", code)
+                    time.sleep(0.1)
+                code = self.real_arm.set_servo_angle_j(
+                    cmd_list[:self._dof], speed=jnt_spd, is_radian=True
+                )
+                self._check_motion_code("set_servo_angle_j", code)
+            else:
+                # 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:
+                    code = self.real_arm.set_mode(6)
+                    self._check_motion_code("set_mode(6)", code)
+                    code = self.real_arm.set_state(0)
+                    self._check_motion_code("set_state(0)", code)
+                    time.sleep(0.1)
+                elif wait_ and self.real_arm.mode != 0:
+                    code = self.real_arm.set_mode(0)
+                    self._check_motion_code("set_mode(0)", code)
+                    code = self.real_arm.set_state(0)
+                    self._check_motion_code("set_state(0)", code)
+                    time.sleep(0.1)

-            self.real_arm.set_servo_angle(angle=cmd_list[:self._dof], speed=jnt_spd, is_radian=True, wait=wait_)
+                code = self.real_arm.set_servo_angle(
+                    angle=cmd_list[:self._dof],
+                    speed=jnt_spd,
+                    is_radian=True,
+                    wait=wait_,
+                )
+                self._check_motion_code("set_servo_angle", code)
        elif self._control_space == "cartesian": # unit: mm?
            lin_spd = self._max_linear_velocity

@ -391,40 +531,7 @@ class UFRobot(Robot, Thread):
        if self._cmd_cnt < 99999:
            self._cmd_cnt += 1 # CHECK!! possibility of overflow?
        if self._gripper_type > GripperType.NoGripper:
-            gripper_norm = action[f"{self.prefix}gripper.pos"]
-            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)
-                # self.real_arm.set_gripper_position(grippos, wait=False, wait_motion=False) # CHECK! the command unit
-            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)
-                modbus_datas = [0x08, 0x10, 0x0C, 0x00, 0x00, 0x05, 0x0A, 0x00, 0x01]
-                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)
-            elif self._gripper_type == GripperType.BioGripperG2:
-                grippos = self._gripper_param.get_grippos(gripper_norm)
-                grippos = int(grippos * 3.7342 - 265.13)
-                modbus_datas = [0x08, 0x10, 0x0C, 0x00, 0x00, 0x05, 0x0A, 0x00, 0x01]
-                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)
-            elif self._gripper_type == GripperType.PikaGripper:
-                grippos = self._gripper_param.get_grippos(gripper_norm)
-                self.pika_gripper.set_gripper_distance(grippos)
-            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)
-                # self.real_arm.robotiq_set_position(
-                #     grippos, speed=self._gripper_param.speed, force=self._gripper_param.force,
-                #     wait=False, wait_motion=False,
-                # )
+            self._send_gripper_action(action[f"{self.prefix}gripper.pos"])

        self.logs["write_pos_dt_s"] = time.perf_counter() - before_write_t
        return action
--- a/src/lerobot_robot_ufactory/robots/uf_robot/uf_robot_config.py
+++ b/src/lerobot_robot_ufactory/robots/uf_robot/uf_robot_config.py
@ -1,5 +1,4 @@
 from dataclasses import dataclass, field
-from typing import Tuple
 from lerobot.cameras import CameraConfig
 from lerobot.robots import RobotConfig

@ -18,8 +17,12 @@ class UFRobotConfig(RobotConfig):
    gripper_speed: int = -1     # auto
    gripper_force: int = -1     # auto
    observe_joint_vel: bool = False # only effective in joint control mode
-    start_joints: Tuple[float, ...] = (0, 0, 0, 90, 0, 90, 0) # °
-    start_tcp_pose: Tuple[float, ...] = None # [x, y, z, roll(°), pitch(°), yaw(°)]
+    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
+    # 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
    max_linear_velocity: int = 200 # mm/s, only effective in cartesian control mode
    no_action: bool = False # only for debug
@ -27,3 +30,12 @@ class UFRobotConfig(RobotConfig):
    def __post_init__(self):
        super().__post_init__()
        self.id = 'uf_robot' if self.id is None else self.id
+        if self.manual_mode:
+            if self.control_space != "joint":
+                raise ValueError("manual_mode requires control_space='joint'")
+            if self.teach_sensitivity is not None and not 1 <= self.teach_sensitivity <= 5:
+                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 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")
--- a/src/lerobot_robot_ufactory/scripts/uf_lerobot_eval.py
+++ b/src/lerobot_robot_ufactory/scripts/uf_lerobot_eval.py
@ -232,7 +232,10 @@ def eval_loop(cfg: EvalConfig, relative=False, rx_continuous=False):

    # with torch.no_grad(), torch.autocast(device_type=device.type) if cfg.policy.use_amp else nullcontext():
    while True:
-        robot.configure()
+        reset = getattr(robot, "reset_to_initial", None)
+        if reset is None:
+            reset = robot.configure
+        reset()
        policy.reset()
        preprocessor.reset()
        postprocessor.reset()
--- a/src/lerobot_robot_ufactory/scripts/uf_lerobot_record.py
+++ b/src/lerobot_robot_ufactory/scripts/uf_lerobot_record.py
@ -6,14 +6,29 @@ import argparse
 import logging
 import shutil
 import threading
+from dataclasses import dataclass
 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.utils import init_keyboard_listener


+@dataclass
+class UFRecordConfig(LeRobotRecordConfig):
+    """RecordConfig variant that permits UFACTORY manual-mode recording."""
+
+    def __post_init__(self):
+        manual_mode = getattr(self.robot, "manual_mode", False)
+        if manual_mode:
+            if self.teleop is not None or self.policy is not None:
+                raise ValueError("manual_mode recording cannot be combined with a teleop or policy")
+            return
+        super().__post_init__()
+
+
 def _get_dataset_writer(dataset):
    return getattr(dataset, "writer", None)

@ -81,6 +96,45 @@ def _current_episode_index(dataset):
        return dataset.num_episodes


+def _manual_gripper_action_key(action_features):
+    return next((key for key in action_features if key.endswith("gripper.pos")), None)
+
+
+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}
+    if gripper_target is not None:
+        gripper_key = _manual_gripper_action_key(action_features)
+        if gripper_key is not None and gripper_key in action:
+            action[gripper_key] = float(gripper_target)
+    return action
+
+
+def _update_manual_gripper_key_state(key, pressed, key_state):
+    char = getattr(key, "char", None)
+    if not isinstance(char, str):
+        return
+
+    char = char.lower()
+    if char == "c":
+        key_state["close"] = pressed
+    elif char == "o":
+        key_state["open"] = pressed
+
+
+def _update_manual_gripper_target(target, key_state, speed, fps):
+    if target is None or fps <= 0:
+        return target
+
+    close_pressed = bool(key_state.get("close", False))
+    open_pressed = bool(key_state.get("open", False))
+    if close_pressed == open_pressed:
+        return target
+
+    direction = 1.0 if close_pressed else -1.0
+    return min(max(target + direction * speed / fps, 0.0), 1.0)
+
+
 def _create_empty_episode_buffer(dataset, episode_index, template_episode_buffer):
    writer = _get_dataset_writer(dataset)

@ -121,6 +175,7 @@ class AsyncEpisodeSaver:
        self._total_cnts = 0
        self._finish_cnts = 0
        self._exception = None
+        self._closed = False
        self._thread = threading.Thread(target=self._run, name="uf-async-episode-saver", daemon=True)
        self._thread.start()

@ -141,10 +196,13 @@ class AsyncEpisodeSaver:
        self._raise_if_failed()

    def close(self):
+        if self._closed:
+            return
        self._queue.join()
        self._queue.put(self._STOP)
        self._queue.join()
        self._thread.join()
+        self._closed = True
        self._raise_if_failed()

    def _run(self):
@ -189,6 +247,39 @@ class AsyncEpisodeSaver:
            raise RuntimeError("Async episode save failed.") from self._exception


+def _disconnect_recording_resources(robot, teleop, listener):
+    """Release recording devices while preserving cleanup after partial failures."""
+    try:
+        if getattr(robot, "_is_connected", False) or getattr(robot, "real_arm", None) is not None:
+            robot.disconnect()
+    finally:
+        try:
+            if teleop is not None and getattr(teleop, "is_connected", False):
+                teleop.disconnect()
+        finally:
+            if listener is not None:
+                listener.stop()
+
+
+class _RecordingCleanup:
+    def __init__(self, robot, teleop, listener, async_episode_saver):
+        self.robot = robot
+        self.teleop = teleop
+        self.listener = listener
+        self.async_episode_saver = async_episode_saver
+
+    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()
+        finally:
+            _disconnect_recording_resources(self.robot, self.teleop, self.listener)
+        return False
+    
+
@safe_stop_image_writer
 def record_loop(
    robot: Robot,
@ -213,6 +304,9 @@ def record_loop(
    display_data: bool = False,
    display_compressed_images: bool = False,
    frame_callback: callable = None,
+    manual_mode: bool = False,
+    manual_gripper_keys: dict[str, bool] | None = None,
+    manual_gripper_speed: float = 0.5,
 ):
    if dataset is not None and dataset.fps != fps:
        raise ValueError(f"The dataset fps should be equal to requested fps ({dataset.fps} != {fps}).")
@ -252,6 +346,10 @@ def record_loop(
    # only positional cmd for now: Remove velo from observation for cmd if needed!
    last_robot_cmd = { k: v for k,v in last_robot_cmd.items() if not "vel" in k }

+    manual_gripper_keys = manual_gripper_keys or {}
+    manual_gripper_target = None
+    manual_gripper_action_key = _manual_gripper_action_key(robot.action_features)
+
    timestamp = 0
    start_episode_t = time.perf_counter()
    while timestamp < control_time_s:
@ -285,6 +383,29 @@ def record_loop(

            act_processed_policy: RobotAction = make_robot_action(action_values, dataset.features)

+        elif policy is None and manual_mode:
+            # In manual mode the physical arm is the source of both the
+            # observation and the demonstrated target state.
+            if manual_gripper_action_key is not None and manual_gripper_target is None:
+                gripper_value = obs_processed.get(manual_gripper_action_key)
+                if gripper_value is None:
+                    gripper_value = obs.get(manual_gripper_action_key)
+                if gripper_value is not None:
+                    manual_gripper_target = min(max(float(gripper_value), 0.0), 1.0)
+
+            manual_gripper_target = _update_manual_gripper_target(
+                manual_gripper_target,
+                manual_gripper_keys,
+                manual_gripper_speed,
+                fps,
+            )
+            act = _manual_action_from_observation(
+                obs_processed,
+                robot.action_features,
+                gripper_target=manual_gripper_target,
+            )
+            act_processed_teleop = teleop_action_processor((act, obs))
+
        elif policy is None and isinstance(teleop, Teleoperator):
            act = teleop.get_action()

@ -344,7 +465,35 @@ def record_loop(
        timestamp = time.perf_counter() - start_episode_t


-def record(cfg: RecordConfig, async_save: bool = False) -> LeRobotDataset:
+def _prepare_recording_episode(robot, teleop, is_uf_teleop, manual_mode):
+    if is_uf_teleop:
+        # Stop teleop output before handing control to the xArm reset motion.
+        teleop.set_teleop_enabled(False)
+
+    if is_uf_teleop or manual_mode:
+        reset = getattr(robot, "reset_to_initial", None)
+        if reset is None:
+            reset = robot.configure
+        reset()
+
+    if is_uf_teleop:
+        obs = robot.get_observation()
+        teleop.reset_to_robot_observation(obs)
+        teleop.set_teleop_enabled(True, obs)
+
+
+def _print_record_controls(is_recorded, manual_mode):
+    if is_recorded:
+        controls = '[ESC] Exit  [←] Reset  [→] Save'
+    else:
+        start_label = 'Reset / Start' if manual_mode else 'Start'
+        controls = f'[ESC] Exit  [Space] {start_label}  [←] Reset  [→] Save'
+    if manual_mode:
+        controls += '  [C] Close  [O] Open'
+    print(f'⌨   {controls}')
+
+
+def record(cfg: UFRecordConfig, async_save: bool = False) -> LeRobotDataset:
    init_logging()
    logging.info(pformat(asdict(cfg)))
    if cfg.display_data:
@ -352,6 +501,7 @@ def record(cfg: RecordConfig, async_save: bool = False) -> LeRobotDataset:

    robot = make_robot_from_config(cfg.robot)
    teleop = make_teleoperator_from_config(cfg.teleop) if cfg.teleop is not None else None
+    manual_mode = bool(getattr(cfg.robot, "manual_mode", False))

    teleop_action_processor, robot_action_processor, robot_observation_processor = make_default_processors()

@ -413,14 +563,23 @@ def record(cfg: RecordConfig, async_save: bool = False) -> LeRobotDataset:
            },
        )

-    robot.connect()
-    if teleop is not None:
-        teleop.connect()
+    try:
+        robot.connect()
+        if teleop is not None:
+            teleop.connect()
+    except BaseException:
+        try:
+            _disconnect_recording_resources(robot, teleop, None)
+        except BaseException:
+            logging.exception("Failed to clean up after recording device connection failure")
+        raise

    is_evt = not is_headless()
    is_uf_teleop = isinstance(teleop, UFBaseTeleop)
    is_recorded = False
    key_dict = {}
+    manual_gripper_keys = {"close": False, "open": False}
+    listener = None
    events = {"exit_early": False, "rerecord_episode": False, "stop_recording": False}

    if is_evt:
@ -432,6 +591,7 @@ def record(cfg: RecordConfig, async_save: bool = False) -> LeRobotDataset:
        }

        def on_press(key):
+            _update_manual_gripper_key_state(key, True, manual_gripper_keys)
            try:
                if key == keyboard.Key.right:
                    print("Right arrow key pressed. Exiting loop...")
@ -450,12 +610,10 @@ def record(cfg: RecordConfig, async_save: bool = False) -> LeRobotDataset:
                key_dict[key] = True

        def on_release(key):
+            _update_manual_gripper_key_state(key, False, manual_gripper_keys)
            try:
                if key == keyboard.Key.enter:
-                    if not is_recorded:
-                        print('⌨   [ESC] Exit  [Space] Start  [←] Reset  [→] Save')
-                    else:
-                        print('⌨   [ESC] Exit  [←] Reset  [→] Save')
+                    _print_record_controls(is_recorded, manual_mode)
                    # is_recorded = True
            except Exception as e:
                print(f"Error handling key release: {e}")
@ -464,11 +622,9 @@ def record(cfg: RecordConfig, async_save: bool = False) -> LeRobotDataset:

        listener, events = init_keyboard_listener(events=events, on_press=on_press, on_release=on_release)
        print("\n********** Episode Record Loop Start **********")
-        print('⌨   [ESC] Exit  [Space] Start  [←] Reset  [→] Save')
+        _print_record_controls(is_recorded, manual_mode)
    else:
        input('⌨   Press Enter to start record >>> ')
-        if is_uf_teleop:
-            teleop.set_teleop_enabled(True)
        is_recorded = True
        print('\n********** Episode Record Loop Start **********')

@ -477,7 +633,7 @@ def record(cfg: RecordConfig, async_save: bool = False) -> LeRobotDataset:
    if async_episode_saver is not None:
        print('Async episode saving is enabled.')

-    with VideoEncodingManager(dataset):
+    with _RecordingCleanup(robot, teleop, listener, async_episode_saver), VideoEncodingManager(dataset):
        recorded_episodes = 0
        while recorded_episodes < cfg.dataset.num_episodes and not events["stop_recording"]:
            time.sleep(0.01)
@ -501,10 +657,8 @@ def record(cfg: RecordConfig, async_save: bool = False) -> LeRobotDataset:
            if is_recorded:
                events["rerecord_episode"] = False
                events["exit_early"] = False
-                if is_uf_teleop:
-                    robot.configure()
-                    obs = robot.get_observation()
-                    teleop.set_teleop_enabled(True, obs)
+                if is_uf_teleop or manual_mode:
+                    _prepare_recording_episode(robot, teleop, is_uf_teleop, manual_mode)
                log_say(f"Recording episode {_current_episode_index(dataset)}", cfg.play_sounds)
                record_loop(
                    robot=robot,
@ -522,6 +676,9 @@ def record(cfg: RecordConfig, async_save: bool = False) -> LeRobotDataset:
                    single_task=cfg.dataset.single_task,
                    display_data=cfg.display_data,
                    frame_callback=frame_callback,
+                    manual_mode=manual_mode,
+                    manual_gripper_keys=manual_gripper_keys,
+                    manual_gripper_speed=getattr(cfg.robot, "manual_gripper_speed", 0.5),
                )
            else:
                continue
@ -543,7 +700,7 @@ def record(cfg: RecordConfig, async_save: bool = False) -> LeRobotDataset:
                        _set_episode_buffer(dataset, empty_episode_buffer)
                is_recorded = False
                if is_evt:
-                    print('⌨   [ESC] Exit  [Space] Start  [←] Reset  [→] Save')
+                    _print_record_controls(is_recorded, manual_mode)
                else:
                    input('\n⌨   Press Enter to rerecord this episode >>>>> ')
                    is_recorded = True
@ -565,7 +722,7 @@ def record(cfg: RecordConfig, async_save: bool = False) -> LeRobotDataset:
                recorded_episodes += 1
                is_recorded = False
                if is_evt:
-                    print('⌨   [ESC] Exit  [Space] Start  [←] Reset  [→] Save')
+                    _print_record_controls(is_recorded, manual_mode)
                else:
                    input('⌨   Press Enter to record at the next episode >>>>> ')
                    is_recorded = True
@ -576,13 +733,6 @@ def record(cfg: RecordConfig, async_save: bool = False) -> LeRobotDataset:

    print("\n********** Episode Record Loop Exit **********")

-    robot.disconnect()
-    if teleop is not None:
-        teleop.disconnect()
-
-    if is_evt and listener is not None:
-        listener.stop()
-
    if cfg.dataset.push_to_hub:
        dataset.push_to_hub(tags=cfg.dataset.tags, private=cfg.dataset.private)

@ -590,7 +740,7 @@ def record(cfg: RecordConfig, async_save: bool = False) -> LeRobotDataset:
    return dataset

@parser.wrap()
-def get_cfg(cfg: RecordConfig) -> RecordConfig:
+def get_cfg(cfg: UFRecordConfig) -> UFRecordConfig:
    return cfg

 def main():
--- a/src/lerobot_robot_ufactory/scripts/uf_lerobot_replay.py
+++ b/src/lerobot_robot_ufactory/scripts/uf_lerobot_replay.py
@ -0,0 +1,272 @@
+"""Replay absolute joint states from a LeRobot dataset on an xArm robot."""
+
+from __future__ import annotations
+
+import argparse
+import json
+import math
+import sys
+import time
+from dataclasses import dataclass
+from pathlib import Path
+from typing import Any, Sequence
+
+import pyarrow.parquet as parquet
+
+import lerobot_robot_ufactory  # noqa: F401
+from lerobot.utils.robot_utils import precise_sleep
+from lerobot_robot_ufactory.robots.uf_robot.uf_robot import UFRobot
+from lerobot_robot_ufactory.robots.uf_robot.uf_robot_config import UFRobotConfig
+
+
+JOINT_STATE_NAMES = tuple(f"J{index}.pos" for index in range(1, 8))
+STATE_NAMES = JOINT_STATE_NAMES + ("gripper.pos",)
+STATE_FEATURE = "observation.state"
+DEFAULT_DATASET_ROOT = Path("datasets/xarm7_manual_replay")
+DEFAULT_ROBOT_IP = "192.168.1.245"
+
+
+@dataclass(frozen=True)
+class ReplayEpisode:
+    """Validated, ordered absolute states for one dataset episode."""
+
+    fps: float
+    episode_index: int
+    frame_indices: tuple[int, ...]
+    states: tuple[tuple[float, ...], ...]
+
+
+def _load_info(dataset_root: Path) -> dict[str, Any]:
+    info_path = dataset_root / "meta" / "info.json"
+    if not info_path.is_file():
+        raise ValueError(f"LeRobot metadata file does not exist: {info_path}")
+
+    try:
+        with info_path.open("r", encoding="utf-8") as info_file:
+            info = json.load(info_file)
+    except (OSError, json.JSONDecodeError) as exc:
+        raise ValueError(f"Could not read LeRobot metadata: {info_path}") from exc
+
+    if not isinstance(info, dict):
+        raise ValueError(f"LeRobot metadata must contain a JSON object: {info_path}")
+    return info
+
+
+def _validate_state_feature(info: dict[str, Any]) -> float:
+    try:
+        fps = float(info["fps"])
+        feature = info["features"][STATE_FEATURE]
+        names = tuple(feature["names"])
+        shape = tuple(feature["shape"])
+    except (KeyError, TypeError, ValueError) as exc:
+        raise ValueError(
+            f"Dataset metadata must define {STATE_FEATURE!r} and a positive FPS"
+        ) from exc
+
+    if not math.isfinite(fps) or fps <= 0:
+        raise ValueError(f"Dataset FPS must be a positive finite number, got {fps!r}")
+    if names != STATE_NAMES or shape != (len(STATE_NAMES),):
+        raise ValueError(
+            f"{STATE_FEATURE!r} must contain absolute fields {list(STATE_NAMES)!r}; "
+            f"got names={list(names)!r}, shape={list(shape)!r}"
+        )
+    return fps
+
+
+def _read_episode_rows(dataset_root: Path, episode_index: int) -> list[dict[str, Any]]:
+    data_files = sorted((dataset_root / "data").glob("chunk-*/file-*.parquet"))
+    if not data_files:
+        raise ValueError(f"No LeRobot data files found below {dataset_root / 'data'}")
+
+    rows: list[dict[str, Any]] = []
+    columns = [STATE_FEATURE, "episode_index", "frame_index"]
+    for data_file in data_files:
+        try:
+            table = parquet.read_table(data_file, columns=columns)
+        except Exception as exc:
+            raise ValueError(f"Could not read LeRobot data file: {data_file}") from exc
+        rows.extend(
+            row for row in table.to_pylist() if int(row["episode_index"]) == episode_index
+        )
+
+    if not rows:
+        raise ValueError(f"Episode {episode_index} does not exist in {dataset_root}")
+    rows.sort(key=lambda row: int(row["frame_index"]))
+    return rows
+
+
+def load_replay_episode(dataset_root: Path, episode_index: int = 0) -> ReplayEpisode:
+    """Load and validate one episode as absolute robot target states."""
+
+    dataset_root = Path(dataset_root).expanduser().resolve()
+    if not dataset_root.is_dir():
+        raise ValueError(f"Dataset directory does not exist: {dataset_root}")
+    if episode_index < 0:
+        raise ValueError(f"Episode index must be non-negative, got {episode_index}")
+
+    info = _load_info(dataset_root)
+    fps = _validate_state_feature(info)
+    rows = _read_episode_rows(dataset_root, episode_index)
+
+    frame_indices = tuple(int(row["frame_index"]) for row in rows)
+    expected_indices = tuple(range(len(rows)))
+    if frame_indices != expected_indices:
+        raise ValueError(
+            f"Episode {episode_index} frame_index must be contiguous from 0; "
+            f"got first={frame_indices[0]}, last={frame_indices[-1]}"
+        )
+
+    states: list[tuple[float, ...]] = []
+    for row_index, row in enumerate(rows):
+        raw_state = row[STATE_FEATURE]
+        if not isinstance(raw_state, (list, tuple)) or len(raw_state) != len(STATE_NAMES):
+            raise ValueError(
+                f"Episode {episode_index}, frame {row_index} must contain "
+                f"{len(STATE_NAMES)} state values"
+            )
+        state = tuple(float(value) for value in raw_state)
+        if not all(math.isfinite(value) for value in state):
+            raise ValueError(
+                f"Episode {episode_index}, frame {row_index} contains a non-finite value"
+            )
+        states.append(state)
+
+    return ReplayEpisode(
+        fps=fps,
+        episode_index=episode_index,
+        frame_indices=frame_indices,
+        states=tuple(states),
+    )
+
+
+def state_to_robot_action(state: Sequence[float]) -> dict[str, float]:
+    """Map one absolute dataset state to the xArm absolute command format."""
+
+    if len(state) != len(STATE_NAMES):
+        raise ValueError(f"Expected {len(STATE_NAMES)} state values, got {len(state)}")
+    values = tuple(float(value) for value in state)
+    if not all(math.isfinite(value) for value in values):
+        raise ValueError("Robot state contains a non-finite value")
+    # These values are absolute positions. Do not subtract the previous state.
+    return dict(zip(STATE_NAMES, values, strict=True))
+
+
+def replay_episode(robot: Any, episode: ReplayEpisode, show_progress: bool = True) -> None:
+    """Send every absolute state once at the dataset FPS."""
+
+    period_s = 1.0 / episode.fps
+    next_deadline = time.perf_counter()
+    total_frames = len(episode.states)
+
+    for frame_number, state in enumerate(episode.states, start=1):
+        robot.send_action(state_to_robot_action(state))
+        if show_progress:
+            print(f"\rReplaying frame {frame_number}/{total_frames}", end="", flush=True)
+
+        next_deadline += period_s
+        precise_sleep(max(next_deadline - time.perf_counter(), 0.0))
+
+    if show_progress:
+        print()
+
+
+def _build_robot(robot_ip: str) -> UFRobot:
+    config = UFRobotConfig(
+        id="xarm7_replay_robot",
+        robot_ip=robot_ip,
+        robot_dof=7,
+        control_space="joint",
+        joint_command_mode=1,
+        gripper_type=1,
+        manual_mode=False,
+        cameras={},
+    )
+    return UFRobot(config)
+
+
+def _confirm_start(episode: ReplayEpisode, robot_ip: str, skip_confirmation: bool) -> bool:
+    first_state = state_to_robot_action(episode.states[0])
+    last_state = state_to_robot_action(episode.states[-1])
+    duration_s = (len(episode.states) - 1) / episode.fps
+    print(f"Robot: xArm7 at {robot_ip}")
+    print(
+        f"Episode {episode.episode_index}: {len(episode.states)} frames, "
+        f"{episode.fps:g} FPS, about {duration_s:.2f} seconds"
+    )
+    print(f"First absolute state: {first_state}")
+    print(f"Last absolute state:  {last_state}")
+    print("The robot will first move to its xArm SDK initial point.")
+    print("After replay it will stop at the last state and disconnect.")
+
+    if skip_confirmation:
+        return True
+    if not sys.stdin.isatty():
+        raise RuntimeError("Interactive confirmation is required; use --yes to continue")
+    return input("Type 'yes' to connect and start replay: ").strip().lower() == "yes"
+
+
+def _disconnect_quietly(robot: UFRobot) -> None:
+    if getattr(robot, "real_arm", None) is None:
+        return
+    try:
+        robot.disconnect()
+    except Exception as exc:
+        print(f"Warning: failed to disconnect robot cleanly: {exc}", file=sys.stderr)
+
+
+def parse_args(argv: Sequence[str] | None = None) -> argparse.Namespace:
+    parser = argparse.ArgumentParser(
+        description="Replay absolute observation.state joint positions on an xArm7."
+    )
+    parser.add_argument(
+        "--dataset-root",
+        type=Path,
+        default=DEFAULT_DATASET_ROOT,
+        help=f"LeRobot dataset root (default: {DEFAULT_DATASET_ROOT})",
+    )
+    parser.add_argument(
+        "--robot-ip",
+        default=DEFAULT_ROBOT_IP,
+        help=f"xArm controller IP (default: {DEFAULT_ROBOT_IP})",
+    )
+    parser.add_argument(
+        "--episode-index",
+        type=int,
+        default=0,
+        help="Episode to replay (default: 0)",
+    )
+    parser.add_argument(
+        "--yes",
+        action="store_true",
+        help="Skip the interactive confirmation before connecting to the robot",
+    )
+    return parser.parse_args(argv)
+
+
+def main(argv: Sequence[str] | None = None) -> int:
+    args = parse_args(argv)
+    try:
+        episode = load_replay_episode(args.dataset_root, args.episode_index)
+        if not _confirm_start(episode, args.robot_ip, args.yes):
+            print("Replay cancelled before connecting to the robot.")
+            return 0
+
+        robot = _build_robot(args.robot_ip)
+        try:
+            robot.connect()
+            print("Robot connected and moved to the SDK initial point.")
+            replay_episode(robot, episode)
+            print("Replay complete. The robot will remain at the last state.")
+        finally:
+            _disconnect_quietly(robot)
+    except KeyboardInterrupt:
+        print("\nReplay interrupted by user.", file=sys.stderr)
+        return 130
+    except (RuntimeError, ValueError, OSError) as exc:
+        print(f"Replay failed: {exc}", file=sys.stderr)
+        return 1
+    return 0
+
+
+if __name__ == "__main__":
+    raise SystemExit(main())
--- a/src/lerobot_robot_ufactory/scripts/uf_robot_teleop.py
+++ b/src/lerobot_robot_ufactory/scripts/uf_robot_teleop.py
@ -61,7 +61,20 @@ def teleop_loop(cfg: TeleopConfig):
    is_evt = not is_headless()
    is_uf_teleop = isinstance(teleop, UFBaseTeleop)

-    is_reset = False
+    def reset_uf_control():
+        if is_uf_teleop:
+            # Stop teleop output before handing control to the xArm reset motion.
+            teleop.set_teleop_enabled(False)
+        reset = getattr(robot, "reset_to_initial", None)
+        if reset is None:
+            reset = robot.configure
+        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
@ -105,12 +118,14 @@ def teleop_loop(cfg: TeleopConfig):

        listener, events = init_keyboard_listener(events=events, on_press=on_press, on_release=on_release)
        print("\n********** Teleop Control Loop Start **********")
-        print('⌨   [ESC] Exit  [Space] Start  [←] Reset')
+        if is_uf_teleop:
+            print('⌨   [ESC] Exit  [Space] Reset / Start  [←] Reset')
+        else:
+            print('⌨   [ESC] Exit  [Space] Start  [←] Reset')
    else:
        input('⌨   Press Enter to start teleop >>> ')
        if is_uf_teleop:
-            obs = robot.get_observation()
-            teleop.set_teleop_enabled(True, obs)
+            reset_uf_control()
        is_paused = False
        is_reset = False
        print("\n********** Teleop Control Loop Start **********")
@ -143,10 +158,10 @@ def teleop_loop(cfg: TeleopConfig):
                    print('⌨   [ESC] Exit  [Space] Start  [←] Reset')
                else:
                    if is_reset:
+                        reset_uf_control()
                        is_reset = False
-                        robot.configure()
                    # print('========== Teleop is start ==========')
-                    if is_uf_teleop:
+                    elif is_uf_teleop:
                        obs = robot.get_observation()
                        teleop.set_teleop_enabled(True, obs)
                    print('⌨   [ESC] Exit  [Space] Pause  [←] Reset')
--- a/src/lerobot_robot_ufactory/teleoperators/base_teleop/base_teleop.py
+++ b/src/lerobot_robot_ufactory/teleoperators/base_teleop/base_teleop.py
@ -22,3 +22,7 @@ class UFBaseTeleop(Teleoperator):
        当enabled为True且obs不为None时, 顺便设置机械臂初始位置映射
        """
        pass
+
+    def reset_to_robot_observation(self, obs):
+        """Align a physical teleoperator with the robot after robot reset."""
+        pass
--- a/src/lerobot_robot_ufactory/teleoperators/gello_teleop/gello_teleop.py
+++ b/src/lerobot_robot_ufactory/teleoperators/gello_teleop/gello_teleop.py
@ -10,6 +10,11 @@ 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/
@ -22,6 +27,8 @@ class GelloTeleop(UFBaseTeleop):
        super().__init__(config)
        self.config = config
        self._is_connected = False
+        self._teleop_enabled = False
+        self._needs_alignment = True
        self._is_calibrated = True # CHECK!!

        from gello.dynamixel.driver import DynamixelDriver
@ -57,11 +64,6 @@ class GelloTeleop(UFBaseTeleop):
        print(self._dynamixel_robo_config)
        self.dof = len(start_joints)

-        if self.config.torque_joint_ids:
-            driver = DynamixelDriver(self.config.torque_joint_ids, port=self.config.port, baudrate=57600)
-            driver.set_torque_mode(True)
-            driver.close()
-
    @property
    def action_features(self) -> dict:
        # Add one more dof for gripper
@ -94,16 +96,22 @@ class GelloTeleop(UFBaseTeleop):
            raise DeviceAlreadyConnectedError(f"{self} already connected")
        from gello.agents.gello_agent import GelloAgent

-        self.gello_agent = GelloAgent(port=self.config.port, dynamixel_config=self._dynamixel_robo_config)
-        if not self._is_calibrated and calibrate:
-            logger.info(
-                "Mismatch between calibration values in the motor and the calibration file or no calibration file found"
-            )
-            self.calibrate()
+        try:
+            self.gello_agent = GelloAgent(port=self.config.port, dynamixel_config=self._dynamixel_robo_config)
+            self.gello_agent._robot.set_torque_mode(False)
+            if not self._is_calibrated and calibrate:
+                logger.info(
+                    "Mismatch between calibration values in the motor and the calibration file or no calibration file found"
+                )
+                self.calibrate()

-        self.configure()
-        self._is_connected = True
-        super().connect(calibrate)
+            self.configure()
+            self._is_connected = True
+            super().connect(calibrate)
+        except BaseException:
+            self._is_connected = False
+            self._close_gello_driver()
+            raise
        logger.info(f"{self} connected.")

    @property
@ -117,7 +125,78 @@ class GelloTeleop(UFBaseTeleop):
        # TODO: Go to sync position slowly? Can not 
        pass

+    def reset_to_robot_observation(self, obs):
+        """Move the physical Gello to the robot's post-reset joint state."""
+        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
+        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(
+            [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]
+
+        if gello_robot.gripper_open_close is not None and len(target_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
+
+        self._needs_alignment = False
+
+    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:
+            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)
+            self._needs_alignment = True
+        self._teleop_enabled = enabled
+        logger.info("Gello teleoperation %s", "enabled" if enabled else "disabled")
+
    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
@ -133,9 +212,20 @@ class GelloTeleop(UFBaseTeleop):
    def send_feedback(self, feedback: dict[str, float]) -> None:
        raise NotImplementedError

-    def disconnect(self) -> None:
-        if not self._is_connected:
-            DeviceNotConnectedError(f"{self} is not connected.")
+    def _close_gello_driver(self) -> None:
+        if not hasattr(self, "gello_agent"):
+            return
+        gello_robot = self.gello_agent._robot
+        try:
+            gello_robot.set_torque_mode(False)
+        finally:
+            gello_robot._driver.close()

-        self._is_connected = False
+    def disconnect(self) -> None:
+        try:
+            self._close_gello_driver()
+        finally:
+            self._is_connected = False
+            self._teleop_enabled = False
+            self._needs_alignment = True
        logger.info(f"{self} disconnected.")
--- a/src/lerobot_robot_ufactory/teleoperators/gello_teleop/gello_teleop_config.py
+++ b/src/lerobot_robot_ufactory/teleoperators/gello_teleop/gello_teleop_config.py
@ -14,9 +14,10 @@ class GelloTeleopConfig(TeleoperatorConfig):
    # 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.
    start_joints: Tuple[float, ...] = (0, 0, 0, 90, 0, 90, 0)  # °
    gripper_id: int = 8  # -1: no gripper
-    torque_joint_ids: Tuple[int, ...] = None  # the joints will activate torque mode.
+    torque_joint_ids: Tuple[int, ...] = None  # deprecated; reset controls all GELLO joints.

    def __post_init__(self):
        self.id = 'gello_teleop' if self.id is None else self.id
--- a/src/lerobot_robot_ufactory/teleoperators/pika_teleop/pika_teleop.py
+++ b/src/lerobot_robot_ufactory/teleoperators/pika_teleop/pika_teleop.py
@ -111,10 +111,11 @@ class PikaTeleop(UFBaseTeleop, Thread):
                self._teleop_enabled = True
                print(f'[{self.prefix}PIKA] Teleoperation is start')
            else:
-                obs = self._last_action
-                self._last_robot_pose = [obs[f"{self.prefix}pose.x"], obs[f"{self.prefix}pose.y"], obs[f"{self.prefix}pose.z"], obs[f"{self.prefix}pose.rx"], obs[f"{self.prefix}pose.ry"], obs[f"{self.prefix}pose.rz"]]
-                if self.config.use_gripper:
-                    self._last_gripper_pos = obs[f"{self.prefix}gripper.pos"]
+                if self._last_action is not None:
+                    obs = self._last_action
+                    self._last_robot_pose = [obs[f"{self.prefix}pose.x"], obs[f"{self.prefix}pose.y"], obs[f"{self.prefix}pose.z"], obs[f"{self.prefix}pose.rx"], obs[f"{self.prefix}pose.ry"], obs[f"{self.prefix}pose.rz"]]
+                    if self.config.use_gripper:
+                        self._last_gripper_pos = obs[f"{self.prefix}gripper.pos"]
                self._teleop_enabled = False
                self._last_action = None
                print(f'[{self.prefix}PIKA] Teleoperation has paused')
--- a/start_manual_record.sh
+++ b/start_manual_record.sh
@ -0,0 +1,50 @@
+#!/usr/bin/env bash
+
+set -euo pipefail
+
+repo_root="$(cd -- "$(dirname -- "${BASH_SOURCE[0]}")" && pwd)"
+cd "$repo_root"
+
+config_path="config/manual_mode/xarm7_manual_record_config.yaml"
+dataset_root="$(
+  sed -n '/^dataset:/,/^[^[:space:]]/s/^[[:space:]]*root:[[:space:]]*//p' "$config_path" \
+    | head -n 1 \
+    | tr -d "\"'"
+)"
+
+if [[ -z "$dataset_root" ]]; then
+  printf 'Could not read dataset.root from %s\n' "$config_path" >&2
+  exit 1
+fi
+
+if [[ "$dataset_root" != /* ]]; then
+  dataset_root="$repo_root/$dataset_root"
+fi
+
+record_args=("$@")
+resume_requested=false
+for arg in "${record_args[@]}"; do
+  if [[ "$arg" == "-r" ]]; then
+    resume_requested=true
+    break
+  fi
+done
+
+if [[ -e "$dataset_root" ]]; then
+  if [[ ! -f "$dataset_root/meta/info.json" ]]; then
+    printf 'Dataset directory exists but is not a valid LeRobot dataset: %s\n' "$dataset_root" >&2
+    printf 'Choose a new dataset.root, or remove this empty/incomplete directory before recording.\n' >&2
+    exit 1
+  fi
+
+  if [[ "$resume_requested" == false ]]; then
+    record_args=("-r" "${record_args[@]}")
+  fi
+elif [[ "$resume_requested" == true ]]; then
+  printf 'Cannot resume because the dataset directory does not exist: %s\n' "$dataset_root" >&2
+  exit 1
+fi
+
+exec uv run uf-lerobot-record \
+  --config_path "$config_path" \
+  "${record_args[@]}"
--- a/tests/test_gello_reset.py
+++ b/tests/test_gello_reset.py
@ -0,0 +1,143 @@
+import numpy as np
+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
+
+
+class FakeDriver:
+    def __init__(self, positions, follow_commands=True):
+        self.positions = np.asarray(positions, dtype=float)
+        self.follow_commands = follow_commands
+        self.commands = []
+
+    def get_joints(self):
+        return self.positions.copy()
+
+    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)
+        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)
+        self._last_pos = object()
+        self.torque_calls = []
+
+    def set_torque_mode(self, enabled):
+        self.torque_calls.append(enabled)
+
+
+def make_teleop(robot):
+    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.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)
+    robot = FakeGelloRobot()
+    teleop = make_teleop(robot)
+
+    teleop.reset_to_robot_observation(
+        {"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._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)
+    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}
+        )
+
+    assert robot.torque_calls == [True, False]
+    assert teleop._teleop_enabled is False
+
+
+def test_gello_enable_after_pause_realigns_before_output(monkeypatch):
+    patch_clock(monkeypatch)
+    robot = FakeGelloRobot()
+    teleop = make_teleop(robot)
+
+    teleop.set_teleop_enabled(
+        True,
+        {"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 teleop._teleop_enabled is True
+
+
+def test_gello_disconnect_closes_driver():
+    robot = FakeGelloRobot()
+    closed = []
+    robot._driver.close = lambda: closed.append(True)
+    teleop = make_teleop(robot)
+    teleop.disconnect()
+
+    assert closed == [True]
+    assert robot.torque_calls == [False]
+    assert teleop._is_connected is False
+
+
+def test_recording_reset_disables_before_robot_and_enables_after_alignment():
+    calls = []
+
+    class FakeRobot:
+        def reset_to_initial(self):
+            calls.append("robot_reset")
+
+        def get_observation(self):
+            calls.append("observation")
+            return {"J1.pos": 0.0}
+
+    class FakeTeleop:
+        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",
+    ]
--- a/tests/test_lerobot_replay.py
+++ b/tests/test_lerobot_replay.py
@ -0,0 +1,273 @@
+import json
+
+import pyarrow as pa
+import pyarrow.parquet as parquet
+import pytest
+
+from lerobot_robot_ufactory.scripts import uf_lerobot_replay as replay_module
+from lerobot_robot_ufactory.scripts.uf_lerobot_replay import (
+    JOINT_STATE_NAMES,
+    ReplayEpisode,
+    STATE_NAMES,
+    load_replay_episode,
+    replay_episode,
+    state_to_robot_action,
+)
+
+
+def _write_dataset(tmp_path, states, episode_indices=None, frame_indices=None, names=None):
+    dataset_root = tmp_path / "dataset"
+    (dataset_root / "data" / "chunk-000").mkdir(parents=True)
+    (dataset_root / "meta").mkdir()
+
+    episode_indices = episode_indices or [0] * len(states)
+    frame_indices = frame_indices or list(range(len(states)))
+    names = names or list(STATE_NAMES)
+    info = {
+        "fps": 30,
+        "features": {
+            "observation.state": {
+                "dtype": "float32",
+                "names": names,
+                "shape": [len(names)],
+            }
+        },
+    }
+    (dataset_root / "meta" / "info.json").write_text(
+        json.dumps(info), encoding="utf-8"
+    )
+    table = pa.table(
+        {
+            "observation.state": states,
+            "episode_index": episode_indices,
+            "frame_index": frame_indices,
+        }
+    )
+    parquet.write_table(table, dataset_root / "data" / "chunk-000" / "file-000.parquet")
+    return dataset_root
+
+
+def test_load_replay_episode_sorts_and_maps_absolute_states(tmp_path):
+    state_a = [0.1, -0.2, 0.3, -0.4, 0.5, -0.6, 0.7, 1.00375]
+    state_b = [0.2, -0.3, 0.4, -0.5, 0.6, -0.7, 0.8, 0.4]
+    dataset_root = _write_dataset(
+        tmp_path,
+        states=[state_b, state_a, [9.0] * 8],
+        episode_indices=[0, 0, 1],
+        frame_indices=[1, 0, 0],
+    )
+
+    episode = load_replay_episode(dataset_root, episode_index=0)
+
+    assert episode.fps == 30
+    assert episode.frame_indices == (0, 1)
+    assert episode.states == (tuple(state_a), tuple(state_b))
+    assert state_to_robot_action(state_a) == dict(zip(STATE_NAMES, state_a, strict=True))
+
+
+def test_replay_sends_absolute_values_without_delta_accumulation(monkeypatch):
+    class FakeRobot:
+        def __init__(self):
+            self.actions = []
+
+        def send_action(self, action):
+            self.actions.append(action.copy())
+
+    monkeypatch.setattr(replay_module, "precise_sleep", lambda _: None)
+    state_a = (0.1, 0.2, 0.3, 0.4, 0.5, 0.6, 0.7, 0.8)
+    state_b = (0.2, 0.4, 0.6, 0.8, 1.0, 1.2, 1.4, 0.9)
+    episode = ReplayEpisode(
+        fps=30,
+        episode_index=0,
+        frame_indices=(0, 1),
+        states=(state_a, state_b),
+    )
+    robot = FakeRobot()
+
+    replay_episode(robot, episode, show_progress=False)
+
+    assert robot.actions == [
+        dict(zip(STATE_NAMES, state_a, strict=True)),
+        dict(zip(STATE_NAMES, state_b, strict=True)),
+    ]
+    assert robot.actions[1]["J1.pos"] == state_b[0]
+
+
+def test_ufactory_robot_sends_absolute_radian_joint_targets(monkeypatch):
+    from lerobot_robot_ufactory.robots.uf_robot import uf_robot as uf_robot_module
+    from lerobot_robot_ufactory.robots.uf_robot.uf_robot_config import UFRobotConfig
+
+    class FakeArm:
+        error_code = 0
+        mode = 6
+
+        def __init__(self):
+            self.calls = []
+
+        def set_mode(self, mode):
+            self.calls.append(("set_mode", mode))
+            self.mode = mode
+            return 0
+
+        def set_state(self, state):
+            self.calls.append(("set_state", state))
+            return 0
+
+        def set_servo_angle(self, **kwargs):
+            self.calls.append(("set_servo_angle", kwargs))
+            return 0
+
+    monkeypatch.setattr(uf_robot_module.time, "sleep", lambda _: None)
+    robot = uf_robot_module.UFRobot(
+        UFRobotConfig(
+            id="replay-test",
+            robot_dof=7,
+            control_space="joint",
+            gripper_type=0,
+            cameras={},
+        )
+    )
+    arm = FakeArm()
+    robot.real_arm = arm
+    robot._is_connected = True
+    target = state_to_robot_action((0.1, -0.2, 0.3, -0.4, 0.5, -0.6, 0.7, 0.8))
+
+    robot.send_action(target)
+
+    servo_calls = [kwargs for name, kwargs in arm.calls if name == "set_servo_angle"]
+    assert servo_calls == [
+        {
+            "angle": [0.1, -0.2, 0.3, -0.4, 0.5, -0.6, 0.7],
+            "speed": 0.2,
+            "is_radian": True,
+            "wait": True,
+        }
+    ]
+
+
+def test_reset_to_initial_enables_robot_before_motion(monkeypatch):
+    from lerobot_robot_ufactory.robots.uf_robot import uf_robot as uf_robot_module
+    from lerobot_robot_ufactory.robots.uf_robot.uf_robot_config import UFRobotConfig
+
+    class FakeArm:
+        mode = 0
+        state = 0
+        error_code = 0
+        warn_code = 0
+
+        def __init__(self):
+            self.calls = []
+
+        def motion_enable(self, enable=True):
+            self.calls.append(("motion_enable", enable))
+            return 0
+
+        def clean_error(self):
+            self.calls.append(("clean_error",))
+            return 0
+
+        def set_mode(self, mode):
+            self.calls.append(("set_mode", mode))
+            self.mode = mode
+            return 0
+
+        def set_state(self, state):
+            self.calls.append(("set_state", state))
+            self.state = state
+            return 0
+
+        def set_servo_angle(self, **kwargs):
+            self.calls.append(("set_servo_angle", kwargs))
+            return 0
+
+    monkeypatch.setattr(uf_robot_module.time, "sleep", lambda _: None)
+    robot = uf_robot_module.UFRobot(
+        UFRobotConfig(
+            id="replay-reset-test",
+            robot_dof=7,
+            control_space="joint",
+            gripper_type=0,
+            cameras={},
+        )
+    )
+    arm = FakeArm()
+    robot.real_arm = arm
+    robot._is_connected = True
+    robot._initial_point = [0.0] * 7
+    robot.configure = lambda: None
+
+    robot.reset_to_initial()
+
+    assert [name for name, *_ in arm.calls] == [
+        "motion_enable",
+        "clean_error",
+        "set_mode",
+        "set_state",
+        "set_servo_angle",
+    ]
+    assert arm.calls[0] == ("motion_enable", True)
+
+
+def test_ufactory_robot_replay_path_sends_absolute_servo_j_targets():
+    from lerobot_robot_ufactory.robots.uf_robot import uf_robot as uf_robot_module
+    from lerobot_robot_ufactory.robots.uf_robot.uf_robot_config import UFRobotConfig
+
+    class FakeArm:
+        error_code = 0
+        mode = 1
+        state = 0
+        warn_code = 0
+
+        def __init__(self):
+            self.calls = []
+
+        def set_servo_angle_j(self, angles, **kwargs):
+            self.calls.append(("set_servo_angle_j", angles, kwargs))
+            return 0
+
+    robot = uf_robot_module.UFRobot(
+        UFRobotConfig(
+            id="replay-servoj-test",
+            robot_dof=7,
+            control_space="joint",
+            joint_command_mode=1,
+            gripper_type=0,
+            cameras={},
+        )
+    )
+    arm = FakeArm()
+    robot.real_arm = arm
+    robot._is_connected = True
+
+    target = state_to_robot_action((0.1, -0.2, 0.3, -0.4, 0.5, -0.6, 0.7, 0.8))
+    robot.send_action(target)
+
+    assert arm.calls == [
+        (
+            "set_servo_angle_j",
+            [0.1, -0.2, 0.3, -0.4, 0.5, -0.6, 0.7],
+            {"speed": 0.2, "is_radian": True},
+        )
+    ]
+
+
+def test_load_replay_episode_rejects_wrong_state_schema(tmp_path):
+    states = [[0.0] * 8]
+    dataset_root = _write_dataset(tmp_path, states, names=list(JOINT_STATE_NAMES) + ["wrong"])
+
+    with pytest.raises(ValueError, match="absolute fields"):
+        load_replay_episode(dataset_root)
+
+
+def test_load_replay_episode_rejects_non_contiguous_frames(tmp_path):
+    dataset_root = _write_dataset(tmp_path, [[0.0] * 8, [1.0] * 8], frame_indices=[0, 2])
+
+    with pytest.raises(ValueError, match="contiguous"):
+        load_replay_episode(dataset_root)
+
+
+def test_state_to_robot_action_rejects_non_finite_values():
+    state = [0.0] * 7 + [float("nan")]
+
+    with pytest.raises(ValueError, match="non-finite"):
+        state_to_robot_action(state)
--- a/tests/test_manual_mode.py
+++ b/tests/test_manual_mode.py
@ -0,0 +1,410 @@
+import sys
+from pathlib import Path
+
+import numpy as np
+import pytest
+from lerobot.datasets.lerobot_dataset import LeRobotDataset
+
+from lerobot_robot_ufactory.robots.uf_robot.uf_robot_config import UFRobotConfig
+from lerobot_robot_ufactory.scripts import uf_lerobot_record as record_module
+from lerobot_robot_ufactory.scripts.uf_lerobot_record import (
+    _manual_action_from_observation,
+    _update_manual_gripper_key_state,
+    _update_manual_gripper_target,
+    _prepare_recording_episode,
+    get_cfg,
+)
+
+
+class FakeXArm:
+    def __init__(self, robot_ip):
+        self.robot_ip = robot_ip
+        self.connected = True
+        self.axis = 6
+        self.error_code = 0
+        self.mode = 0
+        self.initial_point = [0.0, -30.0, 0.0, 0.0, 0.0, 30.0]
+        self._arm = type("FakeArmTransport", (), {"_baud_checkset": False})()
+        self.gripper_position = 800
+        self.calls = []
+
+    def motion_enable(self, **kwargs):
+        self.calls.append(("motion_enable", kwargs))
+
+    def clean_error(self):
+        self.calls.append(("clean_error",))
+
+    def set_teach_sensitivity(self, sensitivity):
+        self.calls.append(("set_teach_sensitivity", sensitivity))
+        return 0
+
+    def set_mode(self, mode):
+        self.calls.append(("set_mode", mode))
+        self.mode = mode
+        return 0
+
+    def set_state(self, state):
+        self.calls.append(("set_state", state))
+        return 0
+
+    def get_initial_point(self):
+        self.calls.append(("get_initial_point",))
+        return 0, self.initial_point
+
+    def set_servo_angle(self, **kwargs):
+        self.calls.append(("set_servo_angle", kwargs))
+        return 0
+
+    def get_err_warn_code(self):
+        return 0, [0, 0]
+
+    def set_linear_spd_limit_factor(self, factor):
+        self.calls.append(("set_linear_spd_limit_factor", factor))
+        return 0
+
+    def set_gripper_enable(self, enable):
+        self.calls.append(("set_gripper_enable", enable))
+        return 0
+
+    def set_gripper_mode(self, mode):
+        self.calls.append(("set_gripper_mode", mode))
+        return 0
+
+    def set_gripper_speed(self, speed):
+        self.calls.append(("set_gripper_speed", speed))
+        return 0
+
+    def set_gripper_position(self, position, **kwargs):
+        self.calls.append(("set_gripper_position", position, kwargs))
+        self.gripper_position = position
+        return 0
+
+    def get_gripper_position(self):
+        self.calls.append(("get_gripper_position",))
+        return 0, self.gripper_position
+
+    def getset_tgpio_modbus_data(self, data):
+        self.calls.append(("getset_tgpio_modbus_data", data))
+        return 0, []
+
+    def get_joint_states(self, is_radian=True, num=3):
+        positions = np.arange(6, dtype=np.float64)
+        velocities = np.zeros(6, dtype=np.float64)
+        return 0, [positions, velocities, velocities]
+
+    def disconnect(self):
+        self.calls.append(("disconnect",))
+        self.connected = False
+
+
+def test_manual_mode_robot_enters_teaching_mode_without_sending_actions(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_manual_robot",
+        calibration_dir=tmp_path,
+        robot_ip=arm.robot_ip,
+        robot_dof=6,
+        control_space="joint",
+        gripper_type=0,
+        manual_mode=True,
+        teach_sensitivity=4,
+    )
+    robot = uf_robot_module.UFRobot(config)
+
+    robot.connect()
+    assert arm.mode == 2
+    assert ("set_teach_sensitivity", 4) in arm.calls
+    assert robot._initial_point == arm.initial_point
+    assert not any(call[0] == "set_servo_angle" for call in arm.calls)
+
+    robot.reset_to_initial()
+    reset_calls = [call for call in arm.calls if call[0] == "set_servo_angle"]
+    assert reset_calls == [
+        (
+            "set_servo_angle",
+            {
+                "angle": arm.initial_point,
+                "speed": 60,
+                "is_radian": False,
+                "wait": True,
+            },
+        )
+    ]
+    assert arm.mode == 2
+
+    action = {"J1.pos": 1.0}
+    assert robot.send_action(action) is action
+
+    observation = robot.get_observation()
+    assert observation["J1.pos"] == 0.0
+    assert observation["J6.pos"] == 5.0
+
+    robot.disconnect()
+    assert arm.mode == 0
+    assert ("disconnect",) in arm.calls
+
+
+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
+
+    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_normal_robot",
+        calibration_dir=tmp_path,
+        robot_ip=arm.robot_ip,
+        robot_dof=6,
+        control_space="joint",
+        gripper_type=0,
+    )
+    robot = uf_robot_module.UFRobot(config)
+    assert not hasattr(config, "start_joints")
+    assert not hasattr(config, "start_tcp_pose")
+    robot.connect()
+
+    reset_calls = [call for call in arm.calls if call[0] == "set_servo_angle"]
+    assert reset_calls == [
+        (
+            "set_servo_angle",
+            {
+                "angle": arm.initial_point,
+                "speed": 60,
+                "is_radian": False,
+                "wait": True,
+            },
+        )
+    ]
+
+    robot.disconnect()
+
+
+def test_manual_mode_config_rejects_cartesian_control(tmp_path):
+    with pytest.raises(ValueError, match="control_space='joint'"):
+        UFRobotConfig(
+            id="test_manual_robot",
+            calibration_dir=tmp_path,
+            robot_dof=6,
+            control_space="cartesian",
+            manual_mode=True,
+        )
+
+
+def test_manual_gripper_speed_is_configurable_and_non_negative(tmp_path):
+    config = UFRobotConfig(
+        id="test_manual_robot",
+        calibration_dir=tmp_path,
+        robot_dof=6,
+        manual_mode=True,
+        manual_gripper_speed=0.25,
+    )
+    assert config.manual_gripper_speed == 0.25
+
+    with pytest.raises(ValueError, match="manual_gripper_speed"):
+        UFRobotConfig(
+            id="test_manual_robot",
+            calibration_dir=tmp_path,
+            robot_dof=6,
+            manual_mode=True,
+            manual_gripper_speed=-0.1,
+        )
+
+
+def test_manual_action_filters_non_action_observation_fields():
+    observation = {
+        "J1.pos": 1.0,
+        "J1.vel": 2.0,
+        "gripper.pos": 0.5,
+        "camera": np.zeros((2, 2, 3), dtype=np.uint8),
+    }
+    action_features = {"J1.pos": float, "gripper.pos": float}
+
+    assert _manual_action_from_observation(observation, action_features) == {
+        "J1.pos": 1.0,
+        "gripper.pos": 0.5,
+    }
+
+
+def test_manual_gripper_keys_update_target_in_expected_direction_and_bounds():
+    key_state = {"close": False, "open": False}
+
+    _update_manual_gripper_key_state(type("Key", (), {"char": "C"})(), True, key_state)
+    assert key_state == {"close": True, "open": False}
+    assert _update_manual_gripper_target(0.5, key_state, speed=1.0, fps=10) == pytest.approx(0.6)
+
+    _update_manual_gripper_key_state(type("Key", (), {"char": "C"})(), False, key_state)
+    _update_manual_gripper_key_state(type("Key", (), {"char": "o"})(), True, key_state)
+    assert _update_manual_gripper_target(0.05, key_state, speed=1.0, fps=10) == 0.0
+
+    _update_manual_gripper_key_state(type("Key", (), {"char": "c"})(), True, key_state)
+    assert _update_manual_gripper_target(0.99, key_state, speed=1.0, fps=10) == 0.99
+
+
+def test_manual_mode_initializes_gripper_without_opening_and_sends_only_gripper(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_manual_gripper_robot",
+        calibration_dir=tmp_path,
+        robot_ip=arm.robot_ip,
+        robot_dof=6,
+        control_space="joint",
+        gripper_type=1,
+        manual_mode=True,
+    )
+    robot = uf_robot_module.UFRobot(config)
+    robot.connect()
+
+    assert ("set_gripper_enable", True) in arm.calls
+    assert ("set_gripper_mode", 0) in arm.calls
+    assert ("set_gripper_speed", 5000) in arm.calls
+    assert not any(call[0] == "set_gripper_position" for call in arm.calls)
+
+    robot.send_action({"J1.pos": 1.0, "gripper.pos": 0.5})
+
+    assert any(call[0] == "getset_tgpio_modbus_data" for call in arm.calls)
+    assert not any(call[0] == "set_servo_angle" for call in arm.calls)
+    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)],
+    )
+
+    config = get_cfg()
+
+    assert config.robot.manual_mode is True
+    assert config.robot.robot_dof == 7
+    assert config.robot.manual_gripper_speed == 0.5
+    assert config.teleop is None
+    assert config.dataset.fps == 30
+
+
+def test_manual_record_loop_writes_actual_state_as_action(tmp_path):
+    class FakeRobot:
+        name = "fake_manual_robot"
+        robot_type = name
+        action_features = {"J1.pos": float, "J2.pos": float}
+        observation_features = action_features
+
+        def __init__(self):
+            self.observation_count = 0
+            self.sent_actions = []
+
+        def get_observation(self):
+            self.observation_count += 1
+            value = float(self.observation_count)
+            return {"J1.pos": value, "J2.pos": value + 1}
+
+        def send_action(self, action):
+            self.sent_actions.append(action.copy())
+            return action
+
+    robot = FakeRobot()
+    action_pipeline, robot_pipeline, observation_pipeline = record_module.make_default_processors()
+    features = record_module.combine_feature_dicts(
+        record_module.aggregate_pipeline_dataset_features(
+            pipeline=action_pipeline,
+            initial_features=record_module.create_initial_features(action=robot.action_features),
+            use_videos=False,
+        ),
+        record_module.aggregate_pipeline_dataset_features(
+            pipeline=observation_pipeline,
+            initial_features=record_module.create_initial_features(
+                observation=robot.observation_features
+            ),
+            use_videos=False,
+        ),
+    )
+    dataset = LeRobotDataset.create(
+        "test/manual-record",
+        fps=30,
+        features=features,
+        root=tmp_path / "dataset",
+        robot_type=robot.robot_type,
+        use_videos=False,
+    )
+
+    record_module.record_loop(
+        robot=robot,
+        events={"exit_early": False},
+        fps=30,
+        teleop_action_processor=action_pipeline,
+        robot_action_processor=robot_pipeline,
+        robot_observation_processor=observation_pipeline,
+        dataset=dataset,
+        control_time_s=0.001,
+        single_task="test task",
+        manual_mode=True,
+    )
+
+    assert dataset.episode_buffer["size"] == 1
+    assert robot.sent_actions == [{"J1.pos": 2.0, "J2.pos": 3.0}]
+    assert dataset.episode_buffer["action"][0].tolist() == [2.0, 3.0]
+
+    dataset.save_episode()
+    dataset.finalize()
+
+
+def test_manual_recording_episode_resets_before_recording():
+    class FakeRobot:
+        def __init__(self):
+            self.calls = []
+
+        def reset_to_initial(self):
+            self.calls.append("reset_to_initial")
+
+    robot = FakeRobot()
+
+    _prepare_recording_episode(robot, teleop=None, is_uf_teleop=False, manual_mode=True)
+
+    assert robot.calls == ["reset_to_initial"]
+
+
+def test_manual_record_loop_applies_keyboard_gripper_target():
+    class FakeRobot:
+        name = "fake_manual_robot"
+        robot_type = name
+        action_features = {"J1.pos": float, "gripper.pos": float}
+
+        def __init__(self):
+            self.sent_actions = []
+
+        def get_observation(self):
+            return {"J1.pos": 1.0, "gripper.pos": 0.5}
+
+        def send_action(self, action):
+            self.sent_actions.append(action.copy())
+            return action
+
+    robot = FakeRobot()
+    action_pipeline, robot_pipeline, observation_pipeline = record_module.make_default_processors()
+
+    record_module.record_loop(
+        robot=robot,
+        events={"exit_early": False},
+        fps=10,
+        teleop_action_processor=action_pipeline,
+        robot_action_processor=robot_pipeline,
+        robot_observation_processor=observation_pipeline,
+        control_time_s=0.001,
+        manual_mode=True,
+        manual_gripper_keys={"close": True, "open": False},
+        manual_gripper_speed=1.0,
+    )
+
+    assert robot.sent_actions == [{"J1.pos": 1.0, "gripper.pos": 0.6}]
--- a/uv.lock
+++ b/uv.lock
Author	SHA1	Message	Date
Saberlve	b5d46c4edd	Add data replay	2026-08-07 17:49:10 +08:00
Saberlve	d336b73aa3	Fix bug in camera config	2026-08-07 16:44:18 +08:00
Saberlve	c04fcf4427	Add gripper control via keyboards in manual mode	2026-08-07 16:16:55 +08:00
Saberlve	da5a076f4c	Fix bug	2026-08-07 16:04:08 +08:00
Saberlve	2f7077fcbf	Add gello and robot reset pose logic.	2026-08-07 15:56:54 +08:00
Saberlve	b51dfa128a	Install realsense dependencies	2026-08-07 14:47:48 +08:00
Saberlve	651e9ca313	Add manual mode data collection script	2026-08-07 14:14:55 +08:00
Saberlve	75bbbed689	Add reset after manual mode in temp script	2026-08-07 13:15:46 +08:00
Saberlve	4dfed85048	Add simple manual mode script and uv lock	2026-08-07 13:00:21 +08:00
Vinman	e492233348	fix(umi): 预加载 libtbb.so.2，解决 bundled OpenCV 兼容性问题 bundled 的 libopencv_core.so.4.2 依赖旧版 libtbb.so.2 (Intel TBB 2.x ABI)， Ubuntu 22.04+ 的 oneAPI TBB (libtbb.so.12) 不兼容，导致 OpenCV 加载失败。按系统路径 → 本地 bundled 的顺序预加载 libtbb.so.2 解决此问题。	2026-07-23 16:14:33 +08:00