add model inference evaluation

Use gripper g2 in all configs
2026-08-19 08:01:38 +00:00 · 2026-08-18 16:12:40 +08:00
18 changed files with 773 additions and 91 deletions
--- a/README.md
+++ b/README.md
@ -67,11 +67,14 @@ Predefined configs are provided under `config/`:

 - `robot.robot_ip` — xArm controller IP (e.g. `192.168.1.245`)
 - `robot.robot_dof` — `7`
- `robot.gripper_type` — `1` for the xArm gripper
+- `robot.gripper_type` — `2` for the xArm Gripper G2
+- `robot.gripper_speed` — G2 opening/closing speed in mm/s (`15`–`225`; configured as `100`)
+- `robot.gripper_force` — G2 gripping force (`1`–`100`; configured as `50`)
 - `teleop.port` — GELLO serial port (`/dev/serial/by-id/...`)
 - `teleop.joint_ids` / `teleop.joint_signs` — per-arm servo mapping and direction
 - `teleop.start_joints` — GELLO calibration reference, should match the xArm SDK initial point (degrees)
 - `teleop.gripper_id` — GELLO gripper servo ID (`8`; `-1` disables it)
+- `teleop.gripper_open_deg` / `teleop.gripper_close_deg` — GELLO servo calibration angles; these are independent of the G2's 0–84 mm opening range
 - `teleop.realtime_control_fps` — independent GELLO-to-xArm command loop rate; it is separate from `dataset.fps`
 - `dataset.root` / `dataset.repo_id` — where the dataset is stored
 - `dataset.single_task` — task description saved with each frame
--- a/README_ZH.md
+++ b/README_ZH.md
@ -67,11 +67,14 @@ ls /dev/serial/by-id/

 - `robot.robot_ip` — xArm 控制器 IP（如 `192.168.1.245`）
 - `robot.robot_dof` — `7`
- `robot.gripper_type` — `1` 表示 xArm 夹爪
+- `robot.gripper_type` — `2` 表示 xArm Gripper G2
+- `robot.gripper_speed` — G2 开合速度，单位 mm/s（范围 `15`–`225`，当前配置为 `100`）
+- `robot.gripper_force` — G2 夹持力（范围 `1`–`100`，当前配置为 `50`）
 - `teleop.port` — GELLO 串口路径（`/dev/serial/by-id/...`）
 - `teleop.joint_ids` / `teleop.joint_signs` — 各型号机械臂的舵机映射与方向
 - `teleop.start_joints` — GELLO 校准参考值（角度），应与 xArm SDK 初始点一致
 - `teleop.gripper_id` — GELLO 夹爪舵机 ID（`8`；`-1` 表示无夹爪）
+- `teleop.gripper_open_deg` / `teleop.gripper_close_deg` — GELLO 舵机的开闭标定角，与 G2 的 0–84 mm 行程相互独立
 - `teleop.realtime_control_fps` — GELLO 到 xArm 的独立实时控制频率，与 `dataset.fps` 分开
 - `dataset.root` / `dataset.repo_id` — 数据集保存位置
 - `dataset.single_task` — 随每一帧保存的任务描述
--- a/config/gello/xarm5_gello_record_config.yaml
+++ b/config/gello/xarm5_gello_record_config.yaml
@ -1,28 +0,0 @@
-robot:
-  type: uf::robot
-  id: "uf_robot"
-  robot_dof: 5
-  control_space: "joint"
-  robot_ip: "192.168.1.245"
-  gripper_type: 1
-  enable_logs: false
-
-# make sure to edit with your correct configurations!
-teleop:
-  type: uf::gello_teleop
-  realtime_control_fps: 60
-  port: "/dev/serial/by-id/usb-FTDI_USB__-__Serial_Converter_FTAJZYC7-if00-port0"
-  joint_ids: [1, 2, 4, 6, 7]
-  joint_signs: [1, 1, -1, 1, 1]
-  start_joints: [0, 0, -90, 90, 0]
-  gripper_id: 8
-
-dataset:
-  # Dataset path relative to the directory where the command is started.
-  root: "datasets/xarm5_gello_datas"
-  repo_id: "ufactory/xarm5_gello_datas"
-  single_task: "Pick up the purple grape and drop into the box on the left."
-  fps: 30
-  episode_time_s: 60  # max duration for one episode
-  reset_time_s: 20    # time for resetting env between episodes
-  push_to_hub: False
--- a/config/gello/xarm6_gello_record_config.yaml
+++ b/config/gello/xarm6_gello_record_config.yaml
@ -1,28 +0,0 @@
-robot:
-  type: uf::robot
-  id: "uf_robot"
-  robot_dof: 6
-  control_space: "joint"
-  robot_ip: "192.168.1.245"
-  gripper_type: 1
-  enable_logs: false
-
-# make sure to edit with your correct configurations!
-teleop:
-  type: uf::gello_teleop
-  realtime_control_fps: 60
-  port: "/dev/serial/by-id/usb-FTDI_USB__-__Serial_Converter_FTAJZYC7-if00-port0"
-  joint_ids: [1, 2, 4, 5, 6, 7]
-  joint_signs: [1, 1, -1, 1, 1, 1]
-  start_joints: [0, 0, -90, 0, 90, 0]
-  gripper_id: 8
-
-dataset:
-  # Dataset path relative to the directory where the command is started.
-  root: "datasets/xarm6_gello_datas"
-  repo_id: "ufactory/xarm6_gello_datas"
-  single_task: "Pick up the purple grape and drop into the box on the left."
-  fps: 30
-  episode_time_s: 60  # max duration for one episode
-  reset_time_s: 20    # time for resetting env between episodes
-  push_to_hub: False
--- a/config/gello/xarm7_gello_cartesian_record_config.yaml
+++ b/config/gello/xarm7_gello_cartesian_record_config.yaml
@ -4,7 +4,12 @@ robot:
  robot_dof: 7
  control_space: "cartesian"
  robot_ip: "192.168.1.245"
-  gripper_type: 1
+  # 2: xArm Gripper G2 (0-84 mm opening range).
+  gripper_type: 2
+  # xArm Gripper G2 speed, 15-225 mm/s; 100 is the SDK default.
+  gripper_speed: 100
+  # xArm Gripper G2 gripping force, 1-100 percent; 50 is the SDK default.
+  gripper_force: 50
  max_linear_velocity: 200
  min_tcp_z_mm: -2.0
  gripper_error_log_path: "logs/xarm7_gripper_errors.log"
--- a/config/gello/xarm7_gello_pick_potato_chips.yaml
+++ b/config/gello/xarm7_gello_pick_potato_chips.yaml
@ -0,0 +1,76 @@
+robot:
+  type: uf::robot
+  id: "uf_robot"
+  robot_dof: 7
+  control_space: "joint"
+  robot_ip: "192.168.1.245"
+  # 2: xArm Gripper G2 (0-84 mm opening range).
+  gripper_type: 2
+  enable_logs: false
+  # Up to 60 Hz goal updates; unchanged targets are filtered below.
+  gripper_command_interval_s: 0.0166667
+  # xArm Gripper G2 speed, 15-225 mm/s; 100 is the SDK default.
+  gripper_speed: 50
+  # xArm Gripper G2 gripping force, 1-100 percent; 50 is the SDK default.
+  gripper_force: 1
+  # Use the high-frequency servo interface for lower-latency GELLO tracking.
+  joint_command_mode: 1
+  max_joint_velocity: 120
+  # TCP z floor in the xArm base coordinate system (mm).
+  min_tcp_z_mm: -2.0
+  # CPU-local FK/Jacobian projection keeps ServoJ free of synchronous SDK queries.
+  tcp_z_guard_backend: "local_projection"
+  tcp_z_soft_margin_mm: 0.5
+  local_kinematics_max_error_mm: 2.0
+  controller_safety_boundary: true
+  # Append gripper initialization/read/write failures here.
+  gripper_error_log_path: "logs/xarm7_gripper_errors.log"
+  cameras:
+    camera:
+      type: intelrealsense
+      serial_number_or_name: "242622070583"
+      width: 640
+      height: 480
+      fps: 30
+    camera2:
+      type: intelrealsense
+      serial_number_or_name: "148522072685"
+      width: 640
+      height: 480
+      fps: 30
+  # Redundant args, indicating the initial pose of xarm7. Set by 192.168.1.245:18333
+  # start_joints: [0, -30, 0, 0, 0, 30, 0]
+# make sure to edit with your correct configurations!
+teleop:
+  type: uf::gello_teleop
+  id: "gello_teleop"
+  # Independent GELLO -> xArm command loop; dataset.fps remains the recording rate.
+  realtime_control_fps: 60
+  port: "/dev/serial/by-id/usb-FTDI_USB__-__Serial_Converter_FTB9HYVD-if00-port0"
+  joint_ids: [1, 2, 3, 4, 5, 6, 7]
+  joint_signs: [1, 1, 1, 1, 1, 1, 1]
+  gripper_id: 8
+  gripper_open_deg: 198.28125
+  gripper_close_deg: 155.75
+
+# Lossy browser preview of the frames already captured for the dataset.
+# JPEG encoding and network I/O run in the background; preview frames are
+# dropped instead of delaying the 30 Hz recorder or 60 Hz GELLO control loop.
+web_preview:
+  enabled: false
+  host: "0.0.0.0"
+  port: 8765
+  fps: 8
+  width: 480
+  jpeg_quality: 65
+
+dataset:
+  # Dataset path relative to the directory where the command is started.
+  root: "datasets/xarm7_gello_pick_potato_chips"
+  repo_id: "ufactory/xarm7_gello_pick_potato_chips"
+  single_task: "Pick up the potato chips and drop."
+  # Cameras deliver 30 Hz; arm ServoJ remains independently fixed at 60 Hz.
+  fps: 30
+  episode_time_s: 60  # max duration for one episode
+  reset_time_s: 20    # time for resetting env between episodes
+  push_to_hub: False
--- a/config/gello/xarm7_gello_record_config.yaml
+++ b/config/gello/xarm7_gello_record_config.yaml
@ -4,13 +4,15 @@ robot:
  robot_dof: 7
  control_space: "joint"
  robot_ip: "192.168.1.245"
-  gripper_type: 1
+  # 2: xArm Gripper G2 (0-84 mm opening range).
+  gripper_type: 2
  enable_logs: false
-  # Up to 60 Hz goal updates; the first command after idle is immediate.
-  # Keeping the gripper below its 5000 maximum speed avoids C19 in testing.
+  # Up to 60 Hz goal updates; unchanged targets are filtered below.
  gripper_command_interval_s: 0.0166667
-  # Avoid the previous maximum-speed (5000) default on the tool RS485 device.
-  gripper_speed: 1500
+  # xArm Gripper G2 speed, 15-225 mm/s; 100 is the SDK default.
+  gripper_speed: 100
+  # xArm Gripper G2 gripping force, 1-100 percent; 50 is the SDK default.
+  gripper_force: 50
  # Use the high-frequency servo interface for lower-latency GELLO tracking.
  joint_command_mode: 1
  max_joint_velocity: 120
@ -64,8 +66,8 @@ web_preview:

 dataset:
  # Dataset path relative to the directory where the command is started.
-  root: "datasets/xarm7_gello_joint_safe_datas"
-  repo_id: "ufactory/xarm7_gello_joint_safe_datas"
+  root: "datasets/xarm7_gello_joint_safe_datas1"
+  repo_id: "ufactory/xarm7_gello_joint_safe_datas1"
  single_task: "Pick up the purple grape and drop into the box on the left."
  # Cameras deliver 30 Hz; arm ServoJ remains independently fixed at 60 Hz.
  fps: 30
--- a/config/gello/xarm7_gello_test.yaml
+++ b/config/gello/xarm7_gello_test.yaml
@ -0,0 +1,76 @@
+robot:
+  type: uf::robot
+  id: "uf_robot"
+  robot_dof: 7
+  control_space: "joint"
+  robot_ip: "192.168.1.245"
+  # 2: xArm Gripper G2 (0-84 mm opening range).
+  gripper_type: 2
+  enable_logs: false
+  # Up to 60 Hz goal updates; unchanged targets are filtered below.
+  gripper_command_interval_s: 0.0166667
+  # xArm Gripper G2 speed, 15-225 mm/s; 100 is the SDK default.
+  gripper_speed: 50
+  # xArm Gripper G2 gripping force, 1-100 percent; 50 is the SDK default.
+  gripper_force: 1
+  # Use the high-frequency servo interface for lower-latency GELLO tracking.
+  joint_command_mode: 1
+  max_joint_velocity: 120
+  # TCP z floor in the xArm base coordinate system (mm).
+  min_tcp_z_mm: -2.0
+  # CPU-local FK/Jacobian projection keeps ServoJ free of synchronous SDK queries.
+  tcp_z_guard_backend: "local_projection"
+  tcp_z_soft_margin_mm: 0.5
+  local_kinematics_max_error_mm: 2.0
+  controller_safety_boundary: true
+  # Append gripper initialization/read/write failures here.
+  gripper_error_log_path: "logs/xarm7_gripper_errors.log"
+  cameras:
+    camera:
+      type: intelrealsense
+      serial_number_or_name: "242622070583"
+      width: 640
+      height: 480
+      fps: 30
+    camera2:
+      type: intelrealsense
+      serial_number_or_name: "148522072685"
+      width: 640
+      height: 480
+      fps: 30
+  # Redundant args, indicating the initial pose of xarm7. Set by 192.168.1.245:18333
+  # start_joints: [0, -30, 0, 0, 0, 30, 0]
+# make sure to edit with your correct configurations!
+teleop:
+  type: uf::gello_teleop
+  id: "gello_teleop"
+  # Independent GELLO -> xArm command loop; dataset.fps remains the recording rate.
+  realtime_control_fps: 60
+  port: "/dev/serial/by-id/usb-FTDI_USB__-__Serial_Converter_FTB9HYVD-if00-port0"
+  joint_ids: [1, 2, 3, 4, 5, 6, 7]
+  joint_signs: [1, 1, 1, 1, 1, 1, 1]
+  gripper_id: 8
+  gripper_open_deg: 198.28125
+  gripper_close_deg: 155.75
+
+# Lossy browser preview of the frames already captured for the dataset.
+# JPEG encoding and network I/O run in the background; preview frames are
+# dropped instead of delaying the 30 Hz recorder or 60 Hz GELLO control loop.
+web_preview:
+  enabled: false
+  host: "0.0.0.0"
+  port: 8765
+  fps: 8
+  width: 480
+  jpeg_quality: 65
+
+dataset:
+  # Dataset path relative to the directory where the command is started.
+  root: "datasets/xarm7_gello_test"
+  repo_id: "ufactory/xarm7_gello_test"
+  single_task: "Test episode for GELLO control."
+  # Cameras deliver 30 Hz; arm ServoJ remains independently fixed at 60 Hz.
+  fps: 30
+  episode_time_s: 360  # max duration for one episode
+  reset_time_s: 20    # time for resetting env between episodes
+  push_to_hub: False
--- a/config/manual_mode/xarm7_manual_record_config.yaml
+++ b/config/manual_mode/xarm7_manual_record_config.yaml
@ -6,7 +6,9 @@ robot:
  # IP address of the xArm controller.
  robot_ip: "192.168.1.245"
  # Gripper type: 1 is the xArm gripper.
-  gripper_type: 1
+  gripper_type: 2
+  # xArm Gripper G2 gripping force, 1-100 percent; 50 is the SDK default.
+  gripper_force: 50
  enable_logs: false
  manual_mode: true
  # Normalized gripper position change per second while holding C/O.
--- a/config/pika/xarm7_pika_record_config.yaml
+++ b/config/pika/xarm7_pika_record_config.yaml
@ -5,6 +5,8 @@ robot:
  control_space: "cartesian"
  robot_ip: "192.168.1.85"
  gripper_type: 10
+  # xArm Gripper G2 gripping force, 1-100 percent; 50 is the SDK default.
+  gripper_force: 50
  enable_logs: false
  max_linear_velocity: 200
  cameras:
--- a/config/spacemouse/xarm7_spacemouse_record_config.yaml
+++ b/config/spacemouse/xarm7_spacemouse_record_config.yaml
@ -5,6 +5,8 @@ robot:
  control_space: "cartesian"
  robot_ip: "192.168.1.127"
  gripper_type: 0
+  # xArm Gripper G2 gripping force, 1-100 percent; 50 is the SDK default.
+  gripper_force: 50
  enable_logs: false

  cameras:
--- a/config/umi/dual_xarm6_umi_record_config.yaml
+++ b/config/umi/dual_xarm6_umi_record_config.yaml
@ -12,6 +12,8 @@ robot:
      control_space: "cartesian"
      robot_ip: "192.168.1.29"
      gripper_type: 2
+      # xArm Gripper G2 gripping force, 1-100 percent; 50 is the SDK default.
+      gripper_force: 50
      enable_logs: false
      max_linear_velocity: 200
      cameras:
@ -29,6 +31,8 @@ robot:
      control_space: "cartesian"
      robot_ip: "192.168.1.83"
      gripper_type: 2
+      # xArm Gripper G2 gripping force, 1-100 percent; 50 is the SDK default.
+      gripper_force: 50
      enable_logs: false
      max_linear_velocity: 200
      cameras:
--- a/config/umi/xarm6_umi_record_config.yaml
+++ b/config/umi/xarm6_umi_record_config.yaml
@ -5,6 +5,8 @@ robot:
  control_space: "cartesian"
  robot_ip: "192.168.1.83"
  gripper_type: 2
+  # xArm Gripper G2 gripping force, 1-100 percent; 50 is the SDK default.
+  gripper_force: 50
  enable_logs: false
  max_linear_velocity: 250
  cameras:
--- a/docs/gello_xarm7_smooth_safe_recording_zh.md
+++ b/docs/gello_xarm7_smooth_safe_recording_zh.md
@ -12,7 +12,7 @@
 - 控制器 Safety Boundary 仍然开启，作为本地保护之外的最后一道硬保护。
 - 相机和数据集按真实的 30 Hz 记录，机械臂控制仍保持独立的 60 Hz。
 - 每条 action 带发送时刻；每次 RT joint state 采样也带时刻。写入数据集时，为 state 选择当时已经发送的最近 action，绝不拿“未来的 action”配较早的 state。
- xArm Gripper 保持 60 Hz 的目标检查能力，但只有变化超过阈值才真正发送；夹爪速度从最大值 5000 降到 1500，解决 Error 19。
+- xArm Gripper G2 使用 SDK 专用接口和物理单位；目标最多按 60 Hz 检查，但只有变化超过阈值才真正发送。初始速度采用 SDK 默认的 100 mm/s。

 当前使用的配置是：

@ -28,7 +28,9 @@ robot:

  gripper_command_interval_s: 0.0166667
  gripper_command_threshold: 0.01
-  gripper_speed: 1500
+  gripper_type: 2
+  gripper_speed: 100
+  gripper_force: 50

 teleop:
  realtime_control_fps: 60
@ -118,7 +120,7 @@ logs/gello_record_sync_<时间>.csv

 30 Hz 数据集每帧记录的是该采样时刻有效的 60 Hz 控制命令，这是正常的降采样，不是不同步。

-## 四、夹爪 Error 19 是怎么消除的
+## 四、xArm Gripper G2 配置与 Error 19 排查

 现象是只要连续控制夹爪，就出现：

@ -127,18 +129,16 @@ set_rs485_data -> code=1
 controller_error=19
 ```

-排查过程里先试过降低夹爪命令频率：2 Hz、5 Hz、6.7 Hz 都不报错，但低频带来明显跟手延迟。继续对照实验后发现，真正与故障一致的变量不是频率，而是夹爪速度：
+旧配置将实际安装的 G2 错写成了 `gripper_type: 1`，因此走的是老款 xArm Gripper 的 0–800 pulse、50–5000 r/min 参数和 `set_gripper_position` 路径。旧实验中“5000 降到 1500”的结论不适用于 G2，不能继续作为 G2 的速度依据。

- 报错时使用默认最大速度 5000。
- 速度降到 1500 后，从 2 Hz 一直提高到 20 Hz 都稳定。
- 最后恢复到最高 60 Hz，仍然稳定。
-
-运行期发送也改成 SDK 专用的非阻塞 `set_gripper_position`，不再手工拼通用 RS485 数据包；持续 ServoJ 时关闭 `wait_motion`，否则 SDK 会等待机械臂停止。
+现在使用 `gripper_type: 2`。G2 的位置范围是 0–84 mm，速度范围是 15–225 mm/s，夹持力范围是 1–100。运行期直接调用 SDK 的非阻塞 `set_gripper_g2_position`，由 SDK 完成几何换算和 Modbus 帧构造；持续 ServoJ 时关闭 `wait_motion`，避免等待机械臂停止。

 最终参数为：

 ```yaml
-gripper_speed: 1500
+gripper_type: 2
+gripper_speed: 100
+gripper_force: 50
 gripper_command_interval_s: 0.0166667
 gripper_command_threshold: 0.01
 ```
@ -151,9 +151,9 @@ gripper_command_threshold: 0.01
 logs/xarm7_gripper_errors.log
 ```

-成功记录包含目标值、脉冲位置、调用耗时和返回码。最终实验中单次调用通常只需要约 1.3–2.2 ms，60 Hz、速度 1500 时没有再次出现 C19。
+成功记录包含归一化目标、G2 开口位置、调用耗时和返回码。由于旧日志来自错误的夹爪类型配置，切换到 G2 后必须重新进行连续开合实机验收，确认没有新增 Error 19。

-如果以后更换夹爪、线缆或固件后 C19 再次出现，应先把速度降下来验证。如果低速也报错，就应该检查腕部线缆、接头、末端供电和末端 IO 板固件，而不是无限降低控制频率。
+如果仍出现 C19，应先降低 G2 的 mm/s 速度并降低发送频率做对照；低速低频仍报错时，再检查腕部线缆、接头、末端供电和末端 IO 板固件。

 ## 五、运行和验收

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

-    def _log_gripper_command(self, target: float, pulse: int, dt_ms: float) -> None:
+    def _log_gripper_command(self, target: float, position: int, dt_ms: float) -> None:
        log_path = self.config.gripper_error_log_path
        if not log_path:
            return
@ -996,7 +1014,7 @@ class UFRobot(Robot, Thread):
            with path.open("a", encoding="utf-8") as stream:
                stream.write(
                    f"{timestamp} gripper command: target={target:.6f}, "
-                    f"pulse={pulse}, dt_ms={dt_ms:.3f}, code=0\n"
+                    f"position={position}, dt_ms={dt_ms:.3f}, code=0\n"
                )
        except OSError:
            logging.exception("Failed to write gripper command log to %s", log_path)
--- a/src/lerobot_robot_ufactory/robots/uf_robot/uf_robot_config.py
+++ b/src/lerobot_robot_ufactory/robots/uf_robot/uf_robot_config.py
@ -59,6 +59,11 @@ class UFRobotConfig(RobotConfig):
            raise ValueError("gripper_command_threshold must be between 0 and 1")
        if not math.isfinite(self.gripper_command_interval_s) or self.gripper_command_interval_s < 0:
            raise ValueError("gripper_command_interval_s must be finite and non-negative")
+        if self.gripper_type == 2:
+            if self.gripper_speed != -1 and not 15 <= self.gripper_speed <= 225:
+                raise ValueError("xArm Gripper G2 gripper_speed must be -1 or between 15 and 225 mm/s")
+            if self.gripper_force != -1 and not 1 <= self.gripper_force <= 100:
+                raise ValueError("xArm Gripper G2 gripper_force must be -1 or between 1 and 100")
        if self.control_space == "joint" and self.joint_command_mode not in (1, 6):
            raise ValueError("joint_command_mode must be 1 or 6 for joint control")
        if self.min_tcp_z_mm is not None and not math.isfinite(self.min_tcp_z_mm):
--- a/tests/test_manual_mode.py
+++ b/tests/test_manual_mode.py
@ -4,6 +4,7 @@ from types import SimpleNamespace

 import numpy as np
 import pytest
+import yaml
 from lerobot.datasets.lerobot_dataset import LeRobotDataset

 from lerobot_robot_ufactory.robots.uf_robot.uf_robot_config import UFRobotConfig
@ -28,6 +29,7 @@ class FakeXArm:
        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.gripper_g2_position = 84
        self.calls = []

    def motion_enable(self, **kwargs):
@ -85,6 +87,15 @@ class FakeXArm:
        self.calls.append(("get_gripper_position",))
        return 0, self.gripper_position

+    def set_gripper_g2_position(self, position, **kwargs):
+        self.calls.append(("set_gripper_g2_position", position, kwargs))
+        self.gripper_g2_position = position
+        return 0
+
+    def get_gripper_g2_position(self):
+        self.calls.append(("get_gripper_g2_position",))
+        return 0, self.gripper_g2_position
+
    def getset_tgpio_modbus_data(self, data):
        self.calls.append(("getset_tgpio_modbus_data", data))
        return 0, []
@ -263,6 +274,100 @@ def test_gripper_rs485_commands_are_rate_limited(monkeypatch, tmp_path):
    robot.disconnect()


+def test_xarm_gripper_g2_uses_sdk_units_and_dedicated_api(monkeypatch, tmp_path, caplog):
+    from lerobot_robot_ufactory.robots.uf_robot import uf_robot as uf_robot_module
+
+    arm = FakeXArm("192.168.1.245")
+    monkeypatch.setattr(uf_robot_module, "XArmAPI", lambda robot_ip: arm)
+    monkeypatch.setattr(uf_robot_module.time, "sleep", lambda _: None)
+    config = UFRobotConfig(
+        id="test_gripper_g2",
+        calibration_dir=tmp_path,
+        robot_ip=arm.robot_ip,
+        robot_dof=6,
+        control_space="joint",
+        gripper_type=2,
+        gripper_speed=100,
+        gripper_force=50,
+        gripper_command_interval_s=0.0,
+        gripper_error_log_path=None,
+    )
+    robot = uf_robot_module.UFRobot(config)
+    assert robot._gripper_g2_speed == 100
+    assert robot._gripper_param.speed == int(((100 * 60) / 9.88235 + 140) / 0.4)
+    robot.connect()
+
+    g2_writes = [call for call in arm.calls if call[0] == "set_gripper_g2_position"]
+    assert g2_writes == [
+        (
+            "set_gripper_g2_position",
+            84,
+            {
+                "speed": 100,
+                "force": 50,
+                "wait": True,
+                "check_baud": False,
+            },
+        )
+    ]
+
+    robot._send_gripper_action(0.5)
+    assert [call for call in arm.calls if call[0] == "set_gripper_g2_position"][-1] == (
+        "set_gripper_g2_position",
+        42,
+        {
+            "speed": 100,
+            "force": 50,
+            "wait": False,
+            "wait_motion": False,
+            "check_baud": False,
+            "check_err": False,
+        },
+    )
+    assert not any(call[0] == "getset_tgpio_modbus_data" for call in arm.calls)
+
+    observation = robot.get_observation()
+    assert observation["gripper.pos"] == pytest.approx(0.5)
+
+    arm.get_gripper_g2_position = lambda: (1, None)
+    observation = robot.get_observation()
+    assert observation["gripper.pos"] == pytest.approx(0.5)
+    assert "get_gripper_g2_position" in caplog.text
+    robot.disconnect()
+
+
+def test_xarm_gripper_g2_rejects_legacy_speed_and_invalid_force(tmp_path):
+    with pytest.raises(ValueError, match="15 and 225 mm/s"):
+        UFRobotConfig(
+            id="test_gripper_g2_speed",
+            calibration_dir=tmp_path,
+            robot_dof=6,
+            gripper_type=2,
+            gripper_speed=1500,
+        )
+
+    with pytest.raises(ValueError, match="between 1 and 100"):
+        UFRobotConfig(
+            id="test_gripper_g2_force",
+            calibration_dir=tmp_path,
+            robot_dof=6,
+            gripper_type=2,
+            gripper_force=0,
+        )
+
+
+def test_gello_configs_select_xarm_gripper_g2():
+    config_dir = Path("config/gello")
+    config_paths = sorted(config_dir.glob("*.yaml"))
+    assert config_paths
+
+    for config_path in config_paths:
+        config = yaml.safe_load(config_path.read_text(encoding="utf-8"))
+        assert config["robot"]["gripper_type"] == 2, config_path
+        assert 15 <= config["robot"]["gripper_speed"] <= 225, config_path
+        assert 1 <= config["robot"]["gripper_force"] <= 100, config_path
+
+
 def test_manual_mode_config_rejects_cartesian_control(tmp_path):
    with pytest.raises(ValueError, match="control_space='joint'"):
        UFRobotConfig(
Author	SHA1	Message	Date
lizhuoyuan	e632c3966b	add model inference evaluation	2026-08-19 08:01:38 +00:00
Saberlve	92ef0308db	Use gripper g2 in all configs	2026-08-18 16:12:40 +08:00