wangshuxun/Xarm-DataCollection
1
0
Fork 1
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Compare commits

base: wangshuxun:b5d46c4edd8d36dd555f6b82782558a54a88181c
Branches Tags
wangshuxun:main
..
compare: wangshuxun:811a7f771705a606af78226748827336080d74a5
Branches Tags
wangshuxun:main

No commits in common. "b5d46c4edd8d36dd555f6b82782558a54a88181c" and "811a7f771705a606af78226748827336080d74a5" have entirely different histories.
b5d46c4edd ... 811a7f7717

31 changed files with 235 additions and 5909 deletions
Show Stats Expand all files Collapse all files

2
.gitignore vendored
View File

@ -88,5 +88,3 @@ models/
*.trt *.trt
*.xvcd *.xvcd
ufactory_usage/ ufactory_usage/
.history/
datasets/

72
README.md
View File

@ -55,9 +55,12 @@ UFACTORY robot arm integration with the LeRobot framework for robot learning, da
git clone https://github.com/xArm-Developer/lerobot_robot_ufactory.git git clone https://github.com/xArm-Developer/lerobot_robot_ufactory.git
cd lerobot_robot_ufactory cd lerobot_robot_ufactory
# Create a uv virtual environment and sync project dependencies # Create conda environment
uv venv --python 3.10 conda create -n uf_lerobot python=3.10 -y
uv sync conda activate uf_lerobot
# Install project
pip install -e .
``` ```
Includes: `lerobot==0.4.3`, `xarm-python-sdk`, `numpy`, `pyyaml`. LeRobot already pulls in torch, opencv, wandb, etc. Includes: `lerobot==0.4.3`, `xarm-python-sdk`, `numpy`, `pyyaml`. LeRobot already pulls in torch, opencv, wandb, etc.
@ -74,7 +77,7 @@ Dynamixel-based leader arm, joint-space control.
```bash ```bash
# 1. Install GELLO module # 1. Install GELLO module
uv sync --extra gello pip install -e ".[gello]"
# 2. Add serial port permissions (re-login required) # 2. Add serial port permissions (re-login required)
sudo usermod -aG dialout $USER sudo usermod -aG dialout $USER
@ -88,7 +91,7 @@ Pika Sense handheld + Vive Tracker, Cartesian-space control.
```bash ```bash
# 1. Install peripheral deps (skip transitive deps) # 1. Install peripheral deps (skip transitive deps)
uv pip install pysurvive agx-pypika --no-deps pip install pysurvive agx-pypika --no-deps
# 2. Install udev rules (re-plug devices afterwards) # 2. Install udev rules (re-plug devices afterwards)
sudo cp src/rules/*.rules /etc/udev/rules.d/ sudo cp src/rules/*.rules /etc/udev/rules.d/
@ -107,7 +110,7 @@ curl -sL https://raw.githubusercontent.com/xArm-Developer/ufactory_resources/mai
sudo apt install -y --fix-broken sudo apt install -y --fix-broken
# 2. Install peripheral deps # 2. Install peripheral deps
uv pip install pysurvive --no-deps pip install pysurvive --no-deps
# 3. Install udev rules (re-plug devices afterwards) # 3. Install udev rules (re-plug devices afterwards)
sudo cp src/rules/*.rules /etc/udev/rules.d/ sudo cp src/rules/*.rules /etc/udev/rules.d/
@ -132,7 +135,7 @@ sudo reboot
```bash ```bash
# 1. Install SpaceMouse module # 1. Install SpaceMouse module
uv sync --extra spacemouse pip install -e ".[spacemouse]"
# 2. Install udev rules (re-plug device afterwards) # 2. Install udev rules (re-plug device afterwards)
sudo cp src/rules/*.rules /etc/udev/rules.d/ sudo cp src/rules/*.rules /etc/udev/rules.d/
@ -148,47 +151,36 @@ Test teleop-to-robot control loop without recording.
```bash ```bash
# Generic usage # Generic usage
uv run uf-robot-teleop --config_path path/to/config.yaml uf-robot-teleop --config_path path/to/config.yaml
uv run uf-robot-teleop --config_path path/to/config.yaml --fps 60 # specify frequency uf-robot-teleop --config_path path/to/config.yaml --fps 60 # specify frequency
# Example: xArm6 + UMI teleop # Example: xArm6 + UMI teleop
uv run uf-robot-teleop --config_path config/umi/xarm6_umi_record_config.yaml uf-robot-teleop --config_path config/umi/xarm6_umi_record_config.yaml
``` ```
### 2. Manual Drag Data Collection ### 2. 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. Record datasets via teleop.
```bash ```bash
# Generic usage # Generic usage
uv run uf-lerobot-record --config_path path/to/record_config.yaml 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 uf-lerobot-record --config_path path/to/config.yaml --resume true # resume recording
# Example: xArm6 + UMI data collection # Example: xArm6 + UMI data collection
uv run uf-lerobot-record --config_path config/umi/xarm6_umi_record_config.yaml uf-lerobot-record --config_path config/umi/xarm6_umi_record_config.yaml
``` ```
### 5. Policy Training ### 3. Policy Training
Train imitation learning policies on collected data. Train imitation learning policies on collected data.
```bash ```bash
# Generic usage # Generic usage
uv run lerobot-train --policy act --dataset your_dataset_name lerobot-train --policy act --dataset your_dataset_name
# Example: train ACT on xArm6 UMI dataset # Example: train ACT on xArm6 UMI dataset
uv run lerobot-train --policy act --dataset ufactory/xarm6_umi_datas lerobot-train --policy act --dataset ufactory/xarm6_umi_datas
``` ```
Important parameters: Important parameters:
@ -197,7 +189,7 @@ Important parameters:
# Note: repo_id is the same as in the record config # Note: repo_id is the same as in the record config
# Policy type: ACT, training steps: 800k # Policy type: ACT, training steps: 800k
# Checkpoints saved every 20k steps, output to lerobot_datas/train (sibling of lerobot directory) # Checkpoints saved every 20k steps, output to lerobot_datas/train (sibling of lerobot directory)
uv run lerobot-train \ lerobot-train \
--dataset.root=../../../../lerobot_datas/record/ufactory/xarm6_umi_datas \ --dataset.root=../../../../lerobot_datas/record/ufactory/xarm6_umi_datas \
--dataset.repo_id=ufactory/xarm6_umi_datas \ --dataset.repo_id=ufactory/xarm6_umi_datas \
--policy.type=act \ --policy.type=act \
@ -210,16 +202,16 @@ uv run lerobot-train \
--save_freq=20000 --save_freq=20000
``` ```
### 6. Inference & Evaluation ### 4. Inference & Evaluation
Run inference with a trained policy. Run inference with a trained policy.
```bash ```bash
# Generic usage # Generic usage
uv run uf-lerobot-eval --config_path path/to/config.yaml --policy.path your_train_path uf-lerobot-eval --config_path path/to/config.yaml --policy.path your_train_path
# Example: run inference with trained ACT policy # Example: run inference with trained ACT policy
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/ 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 ## Tools
@ -229,11 +221,11 @@ uv run uf-lerobot-eval --config_path config/umi/xarm6_umi_record_config.yaml --p
View and stitch multiple camera feeds. View and stitch multiple camera feeds.
```bash ```bash
uv run uf-camera-view -l # list all cameras uf-camera-view -l # list all cameras
uv run uf-camera-view -l -T xvisio # list XVisio cameras only uf-camera-view -l -T xvisio # list XVisio cameras only
uv run uf-camera-view -T xvisio # view XVisio cameras (default 1280x1280 YU12) 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 uf-camera-view -T xvisio -W 640 -H 1920 -F NV12 # specify format
uv run uf-camera-view -T other # view other camera types uf-camera-view -T other # view other camera types
``` ```
### 2. LeRobot Dataset Tools ### 2. LeRobot Dataset Tools
@ -243,7 +235,7 @@ LeRobot provides dataset utilities for inspecting, editing and managing collecte
#### View an episode: #### View an episode:
e.g. view episode index 17: e.g. view episode index 17:
```bash ```bash
uv run lerobot-dataset-viz \ lerobot-dataset-viz \
--root=../../../../lerobot_datas/record/ufactory/xarm7_record_datas \ --root=../../../../lerobot_datas/record/ufactory/xarm7_record_datas \
--repo-id ufactory/xarm7_record_datas \ --repo-id ufactory/xarm7_record_datas \
--display-compressed-images true \ --display-compressed-images true \
@ -253,7 +245,7 @@ uv run lerobot-dataset-viz \
#### Delete specific episodes: #### Delete specific episodes:
e.g. delete episodes 18 and 19: e.g. delete episodes 18 and 19:
```bash ```bash
uv run lerobot-edit-dataset \ lerobot-edit-dataset \
--root=../../../../lerobot_datas/record/ufactory/xarm7_record_datas \ --root=../../../../lerobot_datas/record/ufactory/xarm7_record_datas \
--repo_id ufactory/xarm7_record_datas \ --repo_id ufactory/xarm7_record_datas \
--new_repo_id ../xarm7_record_datas_new \ --new_repo_id ../xarm7_record_datas_new \
@ -263,7 +255,7 @@ uv run lerobot-edit-dataset \
#### Merge datasets: #### Merge datasets:
```bash ```bash
uv run lerobot-edit-dataset \ lerobot-edit-dataset \
--root=../../../../lerobot_datas/record \ --root=../../../../lerobot_datas/record \
--repo_id ufactory/xarm7_record_datas_merge_1_2 \ --repo_id ufactory/xarm7_record_datas_merge_1_2 \
--operation.type merge \ --operation.type merge \

87
README_ZH.md
View File

@ -55,9 +55,12 @@ UFACTORY(深圳市众为创造科技有限公司) 机械臂与 LeRobot 框架集
git clone https://github.com/xArm-Developer/lerobot_robot_ufactory.git git clone https://github.com/xArm-Developer/lerobot_robot_ufactory.git
cd lerobot_robot_ufactory cd lerobot_robot_ufactory
# 创建 uv 虚拟环境并同步项目依赖 # 创建 conda 环境
uv venv --python 3.10 conda create -n uf_lerobot python=3.10 -y
uv sync conda activate uf_lerobot
# 安装项目
pip install -e .
``` ```
包含：`lerobot==0.4.3`、`xarm-python-sdk`、`numpy`、`pyyaml`（lerobot 已自动携带 torch、opencv、wandb 等训练相关依赖）。 包含：`lerobot==0.4.3`、`xarm-python-sdk`、`numpy`、`pyyaml`（lerobot 已自动携带 torch、opencv、wandb 等训练相关依赖）。
@ -74,7 +77,7 @@ uv sync
```bash ```bash
# 1. 安装 GELLO 模块 # 1. 安装 GELLO 模块
uv sync --extra gello pip install -e ".[gello]"
# 2. 添加串口权限（重新登录后生效） # 2. 添加串口权限（重新登录后生效）
sudo usermod -aG dialout $USER sudo usermod -aG dialout $USER
@ -88,7 +91,7 @@ sudo usermod -aG dialout $USER
```bash ```bash
# 1. 安装外设依赖（不需要它们的间接依赖） # 1. 安装外设依赖（不需要它们的间接依赖）
uv pip install pysurvive agx-pypika --no-deps pip install pysurvive agx-pypika --no-deps
# 2. 安装 udev 规则（重新插拔设备后生效） # 2. 安装 udev 规则（重新插拔设备后生效）
sudo cp rules/*.rules /etc/udev/rules.d/ sudo cp rules/*.rules /etc/udev/rules.d/
@ -107,7 +110,7 @@ curl -sL https://raw.githubusercontent.com/xArm-Developer/ufactory_resources/mai
sudo apt install -y --fix-broken sudo apt install -y --fix-broken
# 2. 安装外设依赖 # 2. 安装外设依赖
uv pip install pysurvive --no-deps pip install pysurvive --no-deps
# 3. 安装 udev 规则（重新插拔设备后生效） # 3. 安装 udev 规则（重新插拔设备后生效）
sudo cp rules/*.rules /etc/udev/rules.d/ sudo cp rules/*.rules /etc/udev/rules.d/
@ -132,7 +135,7 @@ sudo reboot
```bash ```bash
# 1. 安装 SpaceMouse 模块 # 1. 安装 SpaceMouse 模块
uv sync --extra spacemouse pip install -e ".[spacemouse]"
# 2. 安装 udev 规则（重新插拔设备后生效） # 2. 安装 udev 规则（重新插拔设备后生效）
sudo cp rules/*.rules /etc/udev/rules.d/ sudo cp rules/*.rules /etc/udev/rules.d/
@ -147,60 +150,33 @@ sudo udevadm control --reload-rules && sudo udevadm trigger
```bash ```bash
# 通用格式 # 通用格式
uv run uf-robot-teleop --config_path path/to/config.yaml uf-robot-teleop --config_path path/to/config.yaml
uv run uf-robot-teleop --config_path path/to/config.yaml --fps 60 # 指定频率 uf-robot-teleop --config_path path/to/config.yaml --fps 60 # 指定频率
# 示例: xArm6 + UMI 遥操作 # 示例: xArm6 + UMI 遥操作
uv run uf-robot-teleop --config_path config/umi/xarm6_umi_record_config.yaml 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 ```bash
# 通用格式 # 通用格式
uv run uf-lerobot-record --config_path path/to/record_config.yaml uf-lerobot-record --config_path path/to/record_config.yaml
uv run uf-lerobot-record --config_path path/to/config.yaml --resume true # 续录 uf-lerobot-record --config_path path/to/config.yaml --resume true # 续录
# 示例: xArm6 + UMI 数据采集 # 示例: xArm6 + UMI 数据采集
uv run uf-lerobot-record --config_path config/umi/xarm6_umi_record_config.yaml uf-lerobot-record --config_path config/umi/xarm6_umi_record_config.yaml
``` ```
### 4. 数据重放 ### 3. Lerobot训练
`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 训练管道进行模仿学习训练。 采集数据后，使用 LeRobot 训练管道进行模仿学习训练。
```bash ```bash
# 通用格式 # 通用格式
uv run lerobot-train --policy act --dataset your_dataset_name lerobot-train --policy act --dataset your_dataset_name
``` ```
参数示例： 参数示例：
@ -209,7 +185,7 @@ uv run lerobot-train --policy act --dataset your_dataset_name
# 注意: repo_id就是采集时配置文件里面的repo_id # 注意: repo_id就是采集时配置文件里面的repo_id
# 这里训练策略policy.type选用act，训练steps为80w次 # 这里训练策略policy.type选用act，训练steps为80w次
# 训练过程每2w次保存一次结果，结果输出到和lerobot同级目录下的lerobot_datas/train里面 # 训练过程每2w次保存一次结果，结果输出到和lerobot同级目录下的lerobot_datas/train里面
uv run lerobot-train \ lerobot-train \
--dataset.root=../../../../lerobot_datas/record/ufactory/xarm6_umi_datas \ --dataset.root=../../../../lerobot_datas/record/ufactory/xarm6_umi_datas \
--dataset.repo_id=ufactory/xarm6_umi_datas \ --dataset.repo_id=ufactory/xarm6_umi_datas \
--policy.type=act \ --policy.type=act \
@ -222,16 +198,16 @@ uv run lerobot-train \
--save_freq=20000 --save_freq=20000
``` ```
### 6. 推理 ### 4. 推理
指定模型进行推理 指定模型进行推理
```bash ```bash
# 通用格式 # 通用格式
uv run uf-lerobot-eval --config_path path/to/config.yaml --policy.path your_train_path uf-lerobot-eval --config_path path/to/config.yaml --policy.path your_train_path
# 示例：使用训练好的 ACT 策略进行推理 # 示例：使用训练好的 ACT 策略进行推理
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/ uf-lerobot-eval --config_path config/umi/xarm6_umi_record_config.yaml --policy.path ../../../../lerobot_datas/train/xarm6_umi_datas/checkpoints/last/pretrained_model/
``` ```
## 工具集 ## 工具集
@ -241,11 +217,11 @@ uv run uf-lerobot-eval --config_path config/umi/xarm6_umi_record_config.yaml --p
查看和拼接多路摄像头画面。 查看和拼接多路摄像头画面。
```bash ```bash
uv run uf-camera-view -l # 列出所有摄像头 uf-camera-view -l # 列出所有摄像头
uv run uf-camera-view -l -T xvisio # 仅列出 XVisio 摄像头 uf-camera-view -l -T xvisio # 仅列出 XVisio 摄像头
uv run uf-camera-view -T xvisio # 查看 XVisio 摄像头（默认 1280x1280 YU12） uf-camera-view -T xvisio # 查看 XVisio 摄像头（默认 1280x1280 YU12）
uv run uf-camera-view -T xvisio -W 640 -H 1920 -F NV12 # 指定格式 uf-camera-view -T xvisio -W 640 -H 1920 -F NV12 # 指定格式
uv run uf-camera-view -T other # 查看其他类型摄像头 uf-camera-view -T other # 查看其他类型摄像头
``` ```
### 2. Lerobot数据集工具 ### 2. Lerobot数据集工具
@ -254,7 +230,7 @@ Lerobot提供一些数据集工具，方便对采集的数据集进行增删查
### 查看某个索引的episode: ### 查看某个索引的episode:
例如查看索引号为17的episode: 例如查看索引号为17的episode:
```bash ```bash
uv run lerobot-dataset-viz \ lerobot-dataset-viz \
--root=../../../../lerobot_datas/record/ufactory/xarm7_record_datas \ --root=../../../../lerobot_datas/record/ufactory/xarm7_record_datas \
--repo-id ufactory/xarm7_record_datas \ --repo-id ufactory/xarm7_record_datas \
--display-compressed-images true \ --display-compressed-images true \
@ -264,7 +240,7 @@ uv run lerobot-dataset-viz \
### 删除某些索引的episodes: ### 删除某些索引的episodes:
例如删除索引号为18和19的episode: 例如删除索引号为18和19的episode:
```bash ```bash
uv run lerobot-edit-dataset \ lerobot-edit-dataset \
--root=../../../../lerobot_datas/record/ufactory/xarm7_record_datas \ --root=../../../../lerobot_datas/record/ufactory/xarm7_record_datas \
--repo_id ufactory/xarm7_record_datas \ --repo_id ufactory/xarm7_record_datas \
--new_repo_id ../xarm7_record_datas_new \ --new_repo_id ../xarm7_record_datas_new \
@ -274,7 +250,7 @@ uv run lerobot-edit-dataset \
### 合并数据集 ### 合并数据集
```bash ```bash
uv run lerobot-edit-dataset \ lerobot-edit-dataset \
--root=../../../../lerobot_datas/record \ --root=../../../../lerobot_datas/record \
--repo_id ufactory/xarm7_record_datas_merge_1_2 \ --repo_id ufactory/xarm7_record_datas_merge_1_2 \
--operation.type merge \ --operation.type merge \
@ -336,3 +312,4 @@ lerobot_robot_ufactory/
## 许可证 ## 许可证
本项目基于 Apache License 2.0 发布，详见 [LICENSE](LICENSE) 文件。 本项目基于 Apache License 2.0 发布，详见 [LICENSE](LICENSE) 文件。

4
config/gello/xarm5_gello_record_config.yaml
View File

@ -3,8 +3,9 @@ robot:
id: "uf_robot" id: "uf_robot"
robot_dof: 5 robot_dof: 5
control_space: "joint" control_space: "joint"
robot_ip: "192.168.1.245" robot_ip: "192.168.1.75"
gripper_type: 1 gripper_type: 1
start_joints: [0, 0, -90, 90, 0]
# make sure to edit with your correct configurations! # make sure to edit with your correct configurations!
teleop: teleop:
@ -14,6 +15,7 @@ teleop:
joint_signs: [1, 1, -1, 1, 1] joint_signs: [1, 1, -1, 1, 1]
start_joints: [0, 0, -90, 90, 0] start_joints: [0, 0, -90, 90, 0]
gripper_id: 8 gripper_id: 8
torque_joint_ids: [3, 5]
dataset: dataset:
# root of local repo: /home/<user_name>/.cache/huggingface/lerobot (default) # root of local repo: /home/<user_name>/.cache/huggingface/lerobot (default)

4
config/gello/xarm6_gello_record_config.yaml
View File

@ -3,8 +3,9 @@ robot:
id: "uf_robot" id: "uf_robot"
robot_dof: 6 robot_dof: 6
control_space: "joint" control_space: "joint"
robot_ip: "192.168.1.245" robot_ip: "192.168.1.68"
gripper_type: 1 gripper_type: 1
start_joints: [0, 0, -90, 0, 90, 0]
# make sure to edit with your correct configurations! # make sure to edit with your correct configurations!
teleop: teleop:
@ -14,6 +15,7 @@ teleop:
joint_signs: [1, 1, -1, 1, 1, 1] joint_signs: [1, 1, -1, 1, 1, 1]
start_joints: [0, 0, -90, 0, 90, 0] start_joints: [0, 0, -90, 0, 90, 0]
gripper_id: 8 gripper_id: 8
torque_joint_ids: [3]
dataset: dataset:
# root of local repo: /home/<user_name>/.cache/huggingface/lerobot (default) # root of local repo: /home/<user_name>/.cache/huggingface/lerobot (default)

8
config/gello/xarm7_gello_record_config.yaml
View File

@ -3,18 +3,16 @@ robot:
id: "uf_robot" id: "uf_robot"
robot_dof: 7 robot_dof: 7
control_space: "joint" control_space: "joint"
robot_ip: "192.168.1.245" robot_ip: "192.168.1.85"
gripper_type: 1 gripper_type: 1
# Redundant args, indicating the initial pose of xarm7. Set by 192.168.1.245:18333 start_joints: [0, 0, 0, 90, 0, 90, 0]
# start_joints: [0, -30, 0, 0, 0, 30, 0]
# make sure to edit with your correct configurations! # make sure to edit with your correct configurations!
teleop: teleop:
type: uf::gello_teleop type: uf::gello_teleop
id: "gello_teleop" id: "gello_teleop"
port: "/dev/serial/by-id/usb-FTDI_USB__-__Serial_Converter_FTAJZYC7-if00-port0" port: "/dev/serial/by-id/usb-FTDI_USB__-__Serial_Converter_FTAJZYC7-if00-port0"
# GELLO calibration reference; matches the xArm SDK initial point. start_joints: [0, 0, 0, 90, 0, 90, 0]
start_joints: [0, -30, 0, 0, 0, 30, 0]
dataset: dataset:
# root of local repo: /home/<user_name>/.cache/huggingface/lerobot (default) # root of local repo: /home/<user_name>/.cache/huggingface/lerobot (default)

41
config/manual_mode/xarm7_manual_record_config.yaml
View File

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

2
config/pika/xarm7_pika_record_config.yaml
View File

@ -6,6 +6,8 @@ robot:
robot_ip: "192.168.1.85" robot_ip: "192.168.1.85"
gripper_type: 10 gripper_type: 10
max_linear_velocity: 200 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: cameras:
fisheye: fisheye:
type: opencv type: opencv

5
config/umi/dual_xarm6_umi_record_config.yaml
View File

@ -13,6 +13,8 @@ robot:
robot_ip: "192.168.1.29" robot_ip: "192.168.1.29"
gripper_type: 2 gripper_type: 2
max_linear_velocity: 200 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: cameras:
fisheye: fisheye:
type: opencv type: opencv
@ -29,6 +31,8 @@ robot:
robot_ip: "192.168.1.83" robot_ip: "192.168.1.83"
gripper_type: 2 gripper_type: 2
max_linear_velocity: 200 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: cameras:
fisheye: fisheye:
type: opencv type: opencv
@ -71,3 +75,4 @@ dataset:
num_episodes: 100 num_episodes: 100
push_to_hub: False push_to_hub: False

4
config/umi/xarm6_umi_record_config.yaml
View File

@ -6,6 +6,8 @@ robot:
robot_ip: "192.168.1.83" robot_ip: "192.168.1.83"
gripper_type: 2 gripper_type: 2
max_linear_velocity: 250 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: cameras:
fisheye: fisheye:
type: opencv type: opencv
@ -33,4 +35,4 @@ dataset:
episode_time_s: 60 # max duration for one episode episode_time_s: 60 # max duration for one episode
reset_time_s: 20 # time for resetting env between episodes reset_time_s: 20 # time for resetting env between episodes
num_episodes: 100 num_episodes: 100
push_to_hub: False push_to_hub: False

5
pyproject.toml
View File

@ -25,9 +25,8 @@ classifiers = [
] ]
dependencies = [ dependencies = [
"numpy>=1.24", "numpy>=1.24",
"pyarrow>=14.0",
"pyyaml", "pyyaml",
"lerobot[intelrealsense]==0.4.3", "lerobot==0.4.3",
"xarm-python-sdk", "xarm-python-sdk",
"opencv-python", "opencv-python",
] ]
@ -36,10 +35,8 @@ dependencies = [
uf-robot-teleop = "lerobot_robot_ufactory.scripts.uf_robot_teleop:main" uf-robot-teleop = "lerobot_robot_ufactory.scripts.uf_robot_teleop:main"
uf-lerobot-record = "lerobot_robot_ufactory.scripts.uf_lerobot_record: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-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-vive-calibrate = "lerobot_robot_ufactory.scripts.vive_calibrate:main"
uf-camera-view = "lerobot_robot_ufactory.scripts.uf_camera_view: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] [project.optional-dependencies]
# GELLO 遥操作 # GELLO 遥操作

BIN
src/lerobot_robot_ufactory/devices/umi/xvlib/libtbb.so.2
View File

Binary file not shown.

31
src/lerobot_robot_ufactory/devices/umi/xvlib/xvlib.py
View File

@ -569,36 +569,6 @@ class XVLib:
return sys_lib return sys_lib
return None 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 @classmethod
def _load_opencv_lib(cls, lib_dir, fname): def _load_opencv_lib(cls, lib_dir, fname):
"""加载 opencv .so: 系统(精确版本) → 本地目录.""" """加载 opencv .so: 系统(精确版本) → 本地目录."""
@ -630,7 +600,6 @@ class XVLib:
cls._check_xvsdk() cls._check_xvsdk()
lib_dir = os.path.dirname(__file__) 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_core.so.4.2')
cls._load_opencv_lib(lib_dir, 'libopencv_imgproc.so.4.2') cls._load_opencv_lib(lib_dir, 'libopencv_imgproc.so.4.2')

4
src/lerobot_robot_ufactory/robots/uf_mock_robot/multiple_uf_mock_robot.py
View File

@ -57,10 +57,6 @@ class MultipleUFMockRobot(Robot):
for robot in self.robots.values(): for robot in self.robots.values():
robot.configure() robot.configure()
def reset_to_initial(self) -> None:
for robot in self.robots.values():
robot.reset_to_initial()
def disconnect(self) -> None: def disconnect(self) -> None:
for robot in self.robots.values(): for robot in self.robots.values():
robot.disconnect() robot.disconnect()

4
src/lerobot_robot_ufactory/robots/uf_mock_robot/uf_mock_robot.py
View File

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

24
src/lerobot_robot_ufactory/robots/uf_robot/multiple_uf_robot.py
View File

@ -101,30 +101,6 @@ class MultipleUFRobot(Robot):
for robot in self.robots.values(): for robot in self.robots.values():
robot.configure() 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: def disconnect(self) -> None:
for robot in self.robots.values(): for robot in self.robots.values():
robot.disconnect() robot.disconnect()

287
src/lerobot_robot_ufactory/robots/uf_robot/uf_robot.py
View File

@ -17,7 +17,6 @@ from xarm.core.utils import convert
## Configurations: ## Configurations:
INIT_SYNC_JOINT_VELOCITY_RAD = 0.2 INIT_SYNC_JOINT_VELOCITY_RAD = 0.2
ROBOT_RESET_SPEED_DEG = 60
CARTESIAN_OBS_KEYS = [ CARTESIAN_OBS_KEYS = [
"pose.x", "pose.y", "pose.z", "pose.rx", "pose.ry", "pose.rz", "pose.x", "pose.y", "pose.z", "pose.rx", "pose.ry", "pose.rz",
@ -76,7 +75,6 @@ class UFRobot(Robot, Thread):
self._control_space = self.config.control_space self._control_space = self.config.control_space
self.real_arm = None self.real_arm = None
self._initial_point = None
cameras_args = self.config.cameras_args or {} cameras_args = self.config.cameras_args or {}
self.camera_width = cameras_args.get('w', 0) self.camera_width = cameras_args.get('w', 0)
self.camera_height = cameras_args.get('h', 0) self.camera_height = cameras_args.get('h', 0)
@ -92,6 +90,15 @@ class UFRobot(Robot, Thread):
self._max_joint_velocity = math.radians(self.config.max_joint_velocity) self._max_joint_velocity = math.radians(self.config.max_joint_velocity)
self._max_linear_velocity = self.config.max_linear_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.report_stop_event = Event()
self._rt_report_normal = False self._rt_report_normal = False
self._update_lock = Lock() self._update_lock = Lock()
@ -186,13 +193,6 @@ class UFRobot(Robot, Thread):
self._is_connected = False self._is_connected = False
raise ConnectionError() 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(): for cam in self.cameras.values():
cam.connect() cam.connect()
self._is_connected = self._is_connected and cam.is_connected self._is_connected = self._is_connected and cam.is_connected
@ -206,10 +206,7 @@ class UFRobot(Robot, Thread):
# print('Could not connect to pika gripper.') # print('Could not connect to pika gripper.')
# raise ConnectionError() # raise ConnectionError()
if self.config.manual_mode: self.configure()
self.configure()
else:
self.reset_to_initial()
if calibrate: if calibrate:
self.calibrate() self.calibrate()
@ -217,33 +214,6 @@ class UFRobot(Robot, Thread):
self._is_connected = True 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: def configure(self) -> None:
self.real_arm.motion_enable() self.real_arm.motion_enable()
self.real_arm.clean_error() self.real_arm.clean_error()
@ -256,32 +226,46 @@ class UFRobot(Robot, Thread):
raise RuntimeError(f"Failed to set correct state to UF robot! Controller Error code: {err_warn[0]} !") raise RuntimeError(f"Failed to set correct state to UF robot! Controller Error code: {err_warn[0]} !")
if self._gripper_type > GripperType.NoGripper: if self._gripper_type > GripperType.NoGripper:
self._configure_gripper(move_to_open=not self.config.manual_mode) self.real_arm._arm._baud_checkset = True
if self._gripper_type == GripperType.xArmGripper:
if self.config.manual_mode: self.real_arm.set_gripper_enable(True)
if self.config.teach_sensitivity is not None: self.real_arm.set_gripper_mode(0)
code = self.real_arm.set_teach_sensitivity(self.config.teach_sensitivity) self.real_arm.set_gripper_speed(self._gripper_param.speed)
if code != 0: self.real_arm.set_gripper_position(self._gripper_param.open_pos)
raise RuntimeError(f"set_teach_sensitivity failed, code={code}") elif self._gripper_type == GripperType.xArmGripperG2:
self.real_arm.set_gripper_enable(True)
code = self.real_arm.set_mode(2) self.real_arm.set_gripper_mode(0)
if code != 0: self.real_arm.set_gripper_g2_position(self._gripper_param.open_pos)
raise RuntimeError(f"set_mode(2) failed, code={code}") elif self._gripper_type == GripperType.BioGripperG2:
code = self.real_arm.set_state(0) _, mode = self.real_arm.get_bio_gripper_control_mode()
if code != 0: if mode != 1:
raise RuntimeError(f"set_state(0) failed, code={code}") 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
_, err_warn = self.real_arm.get_err_warn_code() _, err_warn = self.real_arm.get_err_warn_code()
if err_warn[0] != 0: if err_warn[0] != 0:
raise RuntimeError( raise RuntimeError(f"Failed to set correct state to Gripper! Controller Error code: {err_warn[0]} !")
f"Failed to set manual mode for UF robot! Controller Error code: {err_warn[0]} !"
) if self._start_joints is not None:
return 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
if self._control_space == "joint": if self._control_space == "joint":
code = self.real_arm.set_mode(self.config.joint_command_mode) self.real_arm.set_mode(6)
if code != 0:
raise RuntimeError(f"set_mode({self.config.joint_command_mode}) failed, code={code}")
elif self._control_space == "cartesian": elif self._control_space == "cartesian":
self.real_arm.set_mode(7) self.real_arm.set_mode(7)
else: else:
@ -297,49 +281,6 @@ class UFRobot(Robot, Thread):
self.start() self.start()
time.sleep(0.2) 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: def calibrate(self) -> None:
self._is_calibrated = True self._is_calibrated = True
pass # CHECK! currently No-op pass # CHECK! currently No-op
@ -409,64 +350,9 @@ class UFRobot(Robot, Thread):
return obs_dict 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: def send_action(self, action: dict) -> np.ndarray:
if not self._is_connected: if not self._is_connected:
raise ConnectionError() 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: if self.real_arm.error_code != 0:
return action return action
if self.config.no_action: if self.config.no_action:
@ -482,43 +368,17 @@ class UFRobot(Robot, Thread):
for i in range(self._dof): for i in range(self._dof):
cmd_list[i] = action[f"{self.prefix}J{i+1}.pos"] cmd_list[i] = action[f"{self.prefix}J{i+1}.pos"]
if self.config.joint_command_mode == 1: # TODO: make mode 6 compatible with wait=True
# set_servo_angle_j is an absolute target command. It is the if wait_== False and self.real_arm.mode != 6:
# SDK's high-frequency interface and executes only the latest self.real_arm.set_mode(6)
# target, so it must be used with servo motion mode (1). self.real_arm.set_state(0)
if self.real_arm.mode != 1: time.sleep(0.1)
code = self.real_arm.set_mode(1) elif wait_ and self.real_arm.mode != 0:
self._check_motion_code("set_mode(1)", code) self.real_arm.set_mode(0)
code = self.real_arm.set_state(0) self.real_arm.set_state(0)
self._check_motion_code("set_state(0)", code) time.sleep(0.1)
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)
code = self.real_arm.set_servo_angle( self.real_arm.set_servo_angle(angle=cmd_list[:self._dof], speed=jnt_spd, is_radian=True, wait=wait_)
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? elif self._control_space == "cartesian": # unit: mm?
lin_spd = self._max_linear_velocity lin_spd = self._max_linear_velocity
@ -531,7 +391,40 @@ class UFRobot(Robot, Thread):
if self._cmd_cnt < 99999: if self._cmd_cnt < 99999:
self._cmd_cnt += 1 # CHECK!! possibility of overflow? self._cmd_cnt += 1 # CHECK!! possibility of overflow?
if self._gripper_type > GripperType.NoGripper: if self._gripper_type > GripperType.NoGripper:
self._send_gripper_action(action[f"{self.prefix}gripper.pos"]) 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.logs["write_pos_dt_s"] = time.perf_counter() - before_write_t self.logs["write_pos_dt_s"] = time.perf_counter() - before_write_t
return action return action

18
src/lerobot_robot_ufactory/robots/uf_robot/uf_robot_config.py
View File

@ -1,4 +1,5 @@
from dataclasses import dataclass, field from dataclasses import dataclass, field
from typing import Tuple
from lerobot.cameras import CameraConfig from lerobot.cameras import CameraConfig
from lerobot.robots import RobotConfig from lerobot.robots import RobotConfig
@ -17,12 +18,8 @@ class UFRobotConfig(RobotConfig):
gripper_speed: int = -1 # auto gripper_speed: int = -1 # auto
gripper_force: int = -1 # auto gripper_force: int = -1 # auto
observe_joint_vel: bool = False # only effective in joint control mode observe_joint_vel: bool = False # only effective in joint control mode
manual_mode: bool = False # xArm joint teaching mode; records state and optional gripper actions start_joints: Tuple[float, ...] = (0, 0, 0, 90, 0, 90, 0) # °
manual_gripper_speed: float = 0.5 # normalized gripper position per second in manual mode start_tcp_pose: Tuple[float, ...] = None # [x, y, z, roll(°), pitch(°), yaw(°)]
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_joint_velocity: int = 90 # °/s, only effective in joint control mode
max_linear_velocity: int = 200 # mm/s, only effective in cartesian control mode max_linear_velocity: int = 200 # mm/s, only effective in cartesian control mode
no_action: bool = False # only for debug no_action: bool = False # only for debug
@ -30,12 +27,3 @@ class UFRobotConfig(RobotConfig):
def __post_init__(self): def __post_init__(self):
super().__post_init__() super().__post_init__()
self.id = 'uf_robot' if self.id is None else self.id 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")

5
src/lerobot_robot_ufactory/scripts/uf_lerobot_eval.py
View File

@ -232,10 +232,7 @@ 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(): # with torch.no_grad(), torch.autocast(device_type=device.type) if cfg.policy.use_amp else nullcontext():
while True: while True:
reset = getattr(robot, "reset_to_initial", None) robot.configure()
if reset is None:
reset = robot.configure
reset()
policy.reset() policy.reset()
preprocessor.reset() preprocessor.reset()
postprocessor.reset() postprocessor.reset()

202
src/lerobot_robot_ufactory/scripts/uf_lerobot_record.py
View File

@ -6,29 +6,14 @@ import argparse
import logging import logging
import shutil import shutil
import threading import threading
from dataclasses import dataclass
from pathlib import Path from pathlib import Path
import lerobot_robot_ufactory # patch import lerobot_robot_ufactory # patch
from lerobot.scripts.lerobot_record import * 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.uf_mock_teleop import UFMockTeleop
from lerobot_robot_ufactory.teleoperators.base_teleop import UFBaseTeleop from lerobot_robot_ufactory.teleoperators.base_teleop import UFBaseTeleop
from lerobot_robot_ufactory.utils.utils import init_keyboard_listener 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): def _get_dataset_writer(dataset):
return getattr(dataset, "writer", None) return getattr(dataset, "writer", None)
@ -96,45 +81,6 @@ def _current_episode_index(dataset):
return dataset.num_episodes 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): def _create_empty_episode_buffer(dataset, episode_index, template_episode_buffer):
writer = _get_dataset_writer(dataset) writer = _get_dataset_writer(dataset)
@ -175,7 +121,6 @@ class AsyncEpisodeSaver:
self._total_cnts = 0 self._total_cnts = 0
self._finish_cnts = 0 self._finish_cnts = 0
self._exception = None self._exception = None
self._closed = False
self._thread = threading.Thread(target=self._run, name="uf-async-episode-saver", daemon=True) self._thread = threading.Thread(target=self._run, name="uf-async-episode-saver", daemon=True)
self._thread.start() self._thread.start()
@ -196,13 +141,10 @@ class AsyncEpisodeSaver:
self._raise_if_failed() self._raise_if_failed()
def close(self): def close(self):
if self._closed:
return
self._queue.join() self._queue.join()
self._queue.put(self._STOP) self._queue.put(self._STOP)
self._queue.join() self._queue.join()
self._thread.join() self._thread.join()
self._closed = True
self._raise_if_failed() self._raise_if_failed()
def _run(self): def _run(self):
@ -245,39 +187,6 @@ class AsyncEpisodeSaver:
def _raise_if_failed(self): def _raise_if_failed(self):
if self._exception is not None: if self._exception is not None:
raise RuntimeError("Async episode save failed.") from self._exception 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 @safe_stop_image_writer
@ -304,9 +213,6 @@ def record_loop(
display_data: bool = False, display_data: bool = False,
display_compressed_images: bool = False, display_compressed_images: bool = False,
frame_callback: callable = None, 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: if dataset is not None and dataset.fps != fps:
raise ValueError(f"The dataset fps should be equal to requested fps ({dataset.fps} != {fps}).") raise ValueError(f"The dataset fps should be equal to requested fps ({dataset.fps} != {fps}).")
@ -346,10 +252,6 @@ def record_loop(
# only positional cmd for now: Remove velo from observation for cmd if needed! # 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 } 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 timestamp = 0
start_episode_t = time.perf_counter() start_episode_t = time.perf_counter()
while timestamp < control_time_s: while timestamp < control_time_s:
@ -383,29 +285,6 @@ def record_loop(
act_processed_policy: RobotAction = make_robot_action(action_values, dataset.features) 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): elif policy is None and isinstance(teleop, Teleoperator):
act = teleop.get_action() act = teleop.get_action()
@ -465,35 +344,7 @@ def record_loop(
timestamp = time.perf_counter() - start_episode_t timestamp = time.perf_counter() - start_episode_t
def _prepare_recording_episode(robot, teleop, is_uf_teleop, manual_mode): def record(cfg: RecordConfig, async_save: bool = False) -> LeRobotDataset:
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() init_logging()
logging.info(pformat(asdict(cfg))) logging.info(pformat(asdict(cfg)))
if cfg.display_data: if cfg.display_data:
@ -501,7 +352,6 @@ def record(cfg: UFRecordConfig, async_save: bool = False) -> LeRobotDataset:
robot = make_robot_from_config(cfg.robot) robot = make_robot_from_config(cfg.robot)
teleop = make_teleoperator_from_config(cfg.teleop) if cfg.teleop is not None else None 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() teleop_action_processor, robot_action_processor, robot_observation_processor = make_default_processors()
@ -563,23 +413,14 @@ def record(cfg: UFRecordConfig, async_save: bool = False) -> LeRobotDataset:
}, },
) )
try: robot.connect()
robot.connect() if teleop is not None:
if teleop is not None: teleop.connect()
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_evt = not is_headless()
is_uf_teleop = isinstance(teleop, UFBaseTeleop) is_uf_teleop = isinstance(teleop, UFBaseTeleop)
is_recorded = False is_recorded = False
key_dict = {} key_dict = {}
manual_gripper_keys = {"close": False, "open": False}
listener = None
events = {"exit_early": False, "rerecord_episode": False, "stop_recording": False} events = {"exit_early": False, "rerecord_episode": False, "stop_recording": False}
if is_evt: if is_evt:
@ -591,7 +432,6 @@ def record(cfg: UFRecordConfig, async_save: bool = False) -> LeRobotDataset:
} }
def on_press(key): def on_press(key):
_update_manual_gripper_key_state(key, True, manual_gripper_keys)
try: try:
if key == keyboard.Key.right: if key == keyboard.Key.right:
print("Right arrow key pressed. Exiting loop...") print("Right arrow key pressed. Exiting loop...")
@ -610,10 +450,12 @@ def record(cfg: UFRecordConfig, async_save: bool = False) -> LeRobotDataset:
key_dict[key] = True key_dict[key] = True
def on_release(key): def on_release(key):
_update_manual_gripper_key_state(key, False, manual_gripper_keys)
try: try:
if key == keyboard.Key.enter: if key == keyboard.Key.enter:
_print_record_controls(is_recorded, manual_mode) if not is_recorded:
print('⌨ [ESC] Exit [Space] Start [←] Reset [→] Save')
else:
print('⌨ [ESC] Exit [←] Reset [→] Save')
# is_recorded = True # is_recorded = True
except Exception as e: except Exception as e:
print(f"Error handling key release: {e}") print(f"Error handling key release: {e}")
@ -622,9 +464,11 @@ def record(cfg: UFRecordConfig, async_save: bool = False) -> LeRobotDataset:
listener, events = init_keyboard_listener(events=events, on_press=on_press, on_release=on_release) listener, events = init_keyboard_listener(events=events, on_press=on_press, on_release=on_release)
print("\n********** Episode Record Loop Start **********") print("\n********** Episode Record Loop Start **********")
_print_record_controls(is_recorded, manual_mode) print('⌨ [ESC] Exit [Space] Start [←] Reset [→] Save')
else: else:
input('⌨ Press Enter to start record >>> ') input('⌨ Press Enter to start record >>> ')
if is_uf_teleop:
teleop.set_teleop_enabled(True)
is_recorded = True is_recorded = True
print('\n********** Episode Record Loop Start **********') print('\n********** Episode Record Loop Start **********')
@ -633,7 +477,7 @@ def record(cfg: UFRecordConfig, async_save: bool = False) -> LeRobotDataset:
if async_episode_saver is not None: if async_episode_saver is not None:
print('Async episode saving is enabled.') print('Async episode saving is enabled.')
with _RecordingCleanup(robot, teleop, listener, async_episode_saver), VideoEncodingManager(dataset): with VideoEncodingManager(dataset):
recorded_episodes = 0 recorded_episodes = 0
while recorded_episodes < cfg.dataset.num_episodes and not events["stop_recording"]: while recorded_episodes < cfg.dataset.num_episodes and not events["stop_recording"]:
time.sleep(0.01) time.sleep(0.01)
@ -657,8 +501,10 @@ def record(cfg: UFRecordConfig, async_save: bool = False) -> LeRobotDataset:
if is_recorded: if is_recorded:
events["rerecord_episode"] = False events["rerecord_episode"] = False
events["exit_early"] = False events["exit_early"] = False
if is_uf_teleop or manual_mode: if is_uf_teleop:
_prepare_recording_episode(robot, teleop, is_uf_teleop, manual_mode) robot.configure()
obs = robot.get_observation()
teleop.set_teleop_enabled(True, obs)
log_say(f"Recording episode {_current_episode_index(dataset)}", cfg.play_sounds) log_say(f"Recording episode {_current_episode_index(dataset)}", cfg.play_sounds)
record_loop( record_loop(
robot=robot, robot=robot,
@ -676,9 +522,6 @@ def record(cfg: UFRecordConfig, async_save: bool = False) -> LeRobotDataset:
single_task=cfg.dataset.single_task, single_task=cfg.dataset.single_task,
display_data=cfg.display_data, display_data=cfg.display_data,
frame_callback=frame_callback, 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: else:
continue continue
@ -700,7 +543,7 @@ def record(cfg: UFRecordConfig, async_save: bool = False) -> LeRobotDataset:
_set_episode_buffer(dataset, empty_episode_buffer) _set_episode_buffer(dataset, empty_episode_buffer)
is_recorded = False is_recorded = False
if is_evt: if is_evt:
_print_record_controls(is_recorded, manual_mode) print('⌨ [ESC] Exit [Space] Start [←] Reset [→] Save')
else: else:
input('\n⌨ Press Enter to rerecord this episode >>>>> ') input('\n⌨ Press Enter to rerecord this episode >>>>> ')
is_recorded = True is_recorded = True
@ -722,7 +565,7 @@ def record(cfg: UFRecordConfig, async_save: bool = False) -> LeRobotDataset:
recorded_episodes += 1 recorded_episodes += 1
is_recorded = False is_recorded = False
if is_evt: if is_evt:
_print_record_controls(is_recorded, manual_mode) print('⌨ [ESC] Exit [Space] Start [←] Reset [→] Save')
else: else:
input('⌨ Press Enter to record at the next episode >>>>> ') input('⌨ Press Enter to record at the next episode >>>>> ')
is_recorded = True is_recorded = True
@ -733,6 +576,13 @@ def record(cfg: UFRecordConfig, async_save: bool = False) -> LeRobotDataset:
print("\n********** Episode Record Loop Exit **********") 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: if cfg.dataset.push_to_hub:
dataset.push_to_hub(tags=cfg.dataset.tags, private=cfg.dataset.private) dataset.push_to_hub(tags=cfg.dataset.tags, private=cfg.dataset.private)
@ -740,7 +590,7 @@ def record(cfg: UFRecordConfig, async_save: bool = False) -> LeRobotDataset:
return dataset return dataset
@parser.wrap() @parser.wrap()
def get_cfg(cfg: UFRecordConfig) -> UFRecordConfig: def get_cfg(cfg: RecordConfig) -> RecordConfig:
return cfg return cfg
def main(): def main():

272
src/lerobot_robot_ufactory/scripts/uf_lerobot_replay.py
View File

@ -1,272 +0,0 @@
"""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())

27
src/lerobot_robot_ufactory/scripts/uf_robot_teleop.py
View File

@ -61,20 +61,7 @@ def teleop_loop(cfg: TeleopConfig):
is_evt = not is_headless() is_evt = not is_headless()
is_uf_teleop = isinstance(teleop, UFBaseTeleop) is_uf_teleop = isinstance(teleop, UFBaseTeleop)
def reset_uf_control(): is_reset = False
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 is_paused = True
events = {"exit": False} events = {"exit": False}
listener = None listener = None
@ -118,14 +105,12 @@ def teleop_loop(cfg: TeleopConfig):
listener, events = init_keyboard_listener(events=events, on_press=on_press, on_release=on_release) listener, events = init_keyboard_listener(events=events, on_press=on_press, on_release=on_release)
print("\n********** Teleop Control Loop Start **********") print("\n********** Teleop Control Loop Start **********")
if is_uf_teleop: print('⌨ [ESC] Exit [Space] Start [←] Reset')
print('⌨ [ESC] Exit [Space] Reset / Start [←] Reset')
else:
print('⌨ [ESC] Exit [Space] Start [←] Reset')
else: else:
input('⌨ Press Enter to start teleop >>> ') input('⌨ Press Enter to start teleop >>> ')
if is_uf_teleop: if is_uf_teleop:
reset_uf_control() obs = robot.get_observation()
teleop.set_teleop_enabled(True, obs)
is_paused = False is_paused = False
is_reset = False is_reset = False
print("\n********** Teleop Control Loop Start **********") print("\n********** Teleop Control Loop Start **********")
@ -158,10 +143,10 @@ def teleop_loop(cfg: TeleopConfig):
print('⌨ [ESC] Exit [Space] Start [←] Reset') print('⌨ [ESC] Exit [Space] Start [←] Reset')
else: else:
if is_reset: if is_reset:
reset_uf_control()
is_reset = False is_reset = False
robot.configure()
# print('========== Teleop is start ==========') # print('========== Teleop is start ==========')
elif is_uf_teleop: if is_uf_teleop:
obs = robot.get_observation() obs = robot.get_observation()
teleop.set_teleop_enabled(True, obs) teleop.set_teleop_enabled(True, obs)
print('⌨ [ESC] Exit [Space] Pause [←] Reset') print('⌨ [ESC] Exit [Space] Pause [←] Reset')

4
src/lerobot_robot_ufactory/teleoperators/base_teleop/base_teleop.py
View File

@ -22,7 +22,3 @@ class UFBaseTeleop(Teleoperator):
当enabled为True且obs不为None时, 顺便设置机械臂初始位置映射 当enabled为True且obs不为None时, 顺便设置机械臂初始位置映射
""" """
pass pass
def reset_to_robot_observation(self, obs):
"""Align a physical teleoperator with the robot after robot reset."""
pass

126
src/lerobot_robot_ufactory/teleoperators/gello_teleop/gello_teleop.py
View File

@ -10,11 +10,6 @@ from .gello_teleop_config import GelloTeleopConfig
logger = logging.getLogger(__name__) 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): class GelloTeleop(UFBaseTeleop):
""" """
GELLO for xArm tele-op, ref: https://wuphilipp.github.io/gello_site/ GELLO for xArm tele-op, ref: https://wuphilipp.github.io/gello_site/
@ -27,8 +22,6 @@ class GelloTeleop(UFBaseTeleop):
super().__init__(config) super().__init__(config)
self.config = config self.config = config
self._is_connected = False self._is_connected = False
self._teleop_enabled = False
self._needs_alignment = True
self._is_calibrated = True # CHECK!! self._is_calibrated = True # CHECK!!
from gello.dynamixel.driver import DynamixelDriver from gello.dynamixel.driver import DynamixelDriver
@ -64,6 +57,11 @@ class GelloTeleop(UFBaseTeleop):
print(self._dynamixel_robo_config) print(self._dynamixel_robo_config)
self.dof = len(start_joints) 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 @property
def action_features(self) -> dict: def action_features(self) -> dict:
# Add one more dof for gripper # Add one more dof for gripper
@ -96,22 +94,16 @@ class GelloTeleop(UFBaseTeleop):
raise DeviceAlreadyConnectedError(f"{self} already connected") raise DeviceAlreadyConnectedError(f"{self} already connected")
from gello.agents.gello_agent import GelloAgent from gello.agents.gello_agent import GelloAgent
try: self.gello_agent = GelloAgent(port=self.config.port, dynamixel_config=self._dynamixel_robo_config)
self.gello_agent = GelloAgent(port=self.config.port, dynamixel_config=self._dynamixel_robo_config) if not self._is_calibrated and calibrate:
self.gello_agent._robot.set_torque_mode(False) logger.info(
if not self._is_calibrated and calibrate: "Mismatch between calibration values in the motor and the calibration file or no calibration file found"
logger.info( )
"Mismatch between calibration values in the motor and the calibration file or no calibration file found" self.calibrate()
)
self.calibrate()
self.configure() self.configure()
self._is_connected = True self._is_connected = True
super().connect(calibrate) super().connect(calibrate)
except BaseException:
self._is_connected = False
self._close_gello_driver()
raise
logger.info(f"{self} connected.") logger.info(f"{self} connected.")
@property @property
@ -125,78 +117,7 @@ class GelloTeleop(UFBaseTeleop):
# TODO: Go to sync position slowly? Can not # TODO: Go to sync position slowly? Can not
pass 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]: def get_action(self) -> dict[str, np.ndarray]:
if not self._teleop_enabled:
raise RuntimeError("Gello teleop is disabled")
start = time.perf_counter() start = time.perf_counter()
fake_obs = dict({"joint_state": np.array([0.0]*(self.dof+1))}) # for agent.act() argument, actually no use 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 action_array = self.gello_agent.act(fake_obs) # current gello joint pos as np.ndarray
@ -212,20 +133,9 @@ class GelloTeleop(UFBaseTeleop):
def send_feedback(self, feedback: dict[str, float]) -> None: def send_feedback(self, feedback: dict[str, float]) -> None:
raise NotImplementedError raise NotImplementedError
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()
def disconnect(self) -> None: def disconnect(self) -> None:
try: if not self._is_connected:
self._close_gello_driver() DeviceNotConnectedError(f"{self} is not connected.")
finally:
self._is_connected = False self._is_connected = False
self._teleop_enabled = False
self._needs_alignment = True
logger.info(f"{self} disconnected.") logger.info(f"{self} disconnected.")

3
src/lerobot_robot_ufactory/teleoperators/gello_teleop/gello_teleop_config.py
View File

@ -14,10 +14,9 @@ class GelloTeleopConfig(TeleoperatorConfig):
# Others: Calibration angles, joint directions etc # Others: Calibration angles, joint directions etc
joint_ids: Tuple[int, ...] = (1, 2, 3, 4, 5, 6, 7) 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 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) # ° start_joints: Tuple[float, ...] = (0, 0, 0, 90, 0, 90, 0) # °
gripper_id: int = 8 # -1: no gripper gripper_id: int = 8 # -1: no gripper
torque_joint_ids: Tuple[int, ...] = None # deprecated; reset controls all GELLO joints. torque_joint_ids: Tuple[int, ...] = None # the joints will activate torque mode.
def __post_init__(self): def __post_init__(self):
self.id = 'gello_teleop' if self.id is None else self.id self.id = 'gello_teleop' if self.id is None else self.id

11
src/lerobot_robot_ufactory/teleoperators/pika_teleop/pika_teleop.py
View File

@ -111,11 +111,10 @@ class PikaTeleop(UFBaseTeleop, Thread):
self._teleop_enabled = True self._teleop_enabled = True
print(f'[{self.prefix}PIKA] Teleoperation is start') print(f'[{self.prefix}PIKA] Teleoperation is start')
else: else:
if self._last_action is not None: obs = self._last_action
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"]]
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:
if self.config.use_gripper: self._last_gripper_pos = obs[f"{self.prefix}gripper.pos"]
self._last_gripper_pos = obs[f"{self.prefix}gripper.pos"]
self._teleop_enabled = False self._teleop_enabled = False
self._last_action = None self._last_action = None
print(f'[{self.prefix}PIKA] Teleoperation has paused') print(f'[{self.prefix}PIKA] Teleoperation has paused')
@ -188,4 +187,4 @@ class PikaTeleop(UFBaseTeleop, Thread):
return self._last_action return self._last_action
def send_feedback(self, feedback: dict[str, float]) -> None: def send_feedback(self, feedback: dict[str, float]) -> None:
raise NotImplementedError raise NotImplementedError

50
start_manual_record.sh
View File

@ -1,50 +0,0 @@
#!/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[@]}"

143
tests/test_gello_reset.py
View File

@ -1,143 +0,0 @@
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",
]

273
tests/test_lerobot_replay.py
View File

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

410
tests/test_manual_mode.py
View File

@ -1,410 +0,0 @@
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}]

4016
uv.lock generated
View File

File diff suppressed because it is too large Load Diff