Skip to content

Latest commit

 

History

History
1172 lines (977 loc) · 50.8 KB

File metadata and controls

1172 lines (977 loc) · 50.8 KB

RoboCOIN

📄READMEs: English | 中文 | LeRobot Readme

🔗Links: Project Website | ArXiv | PDF | Visualize & Download

新闻

  • 🔥[2025.12.09] 我们的数据集在 HuggingFaceModelScope 上累计取得300,000下载量!
  • 🔥[2025.11.24] 我们的技术报告已在 ArXiv 上发布!

目录


概述

作为 RoboCOIN数据集的官方配套工具,本项目基于 LeRobot 仓库构建,在完全兼容其数据格式的基础上,增加对子任务、场景描述与运动描述等元数据的支持,并提供从数据集检索、下载到标准化加载的完整流程,并提供了多款机器人的模型部署功能。

核心功能

  1. 支持便捷的数据集检索、下载及 DataLoader 加载功能,支持子任务、场景描述与运动描述等元数据的读取。
  2. 实现统一机器人控制接口,支持多种机器人平台的接入与控制,如Piper/Realman等基于SDK的控制,以及基于ROS/Moveit的通用控制方式
  3. 实现统一单位转换接口,支持多种机器人平台的单位转换,如角度制与弧度制的转换
  4. 提供可视化功能,支持2D/3D轨迹绘制与相机图像显示
  5. 支持基于LeRobot Policy与OpenPI Policy的模型推理与机器人控制

安装

pip install robocoin

数据集检索、下载与加载

检索和下载数据集

数据集目录请访问:https://flagopen.github.io/RoboCOIN-DataManager/ 我们将持续更新数据集,您可以在上方页面中找到最新的数据集

This demo shows how to discovery, download, and standardized loading RoboCOIN datasets

上面的 GIF 展示了如何发现、下载和使用 RoboCOIN 数据集

# you can copy the bash command from the website and paste it here, such as:
# Required storage:  10.8GB.
robocoin-download --hub huggingface --ds_lists Cobot_Magic_move_the_bread R1_Lite_open_and_close_microwave_oven

# the default download path is ~/.cache/huggingface/lerobot/, which will be used as default dir of LerobotDataset.
# if you want to speicifiy download dir, please add --target-dir YOUR_DOWNLOAD_DIR, such as:
# robocoin-download --hub huggingface --ds_lists Cobot_Magic_move_the_bread R1_Lite_open_and_close_microwave_oven --target-dir /path/to/your/download/dir

# we also provide a download option via ModelScope, such as:
# robocoin-download --hub modelscope --ds_lists Cobot_Magic_move_the_bread R1_Lite_open_and_close_microwave_oven 

加载数据集

import torch
from lerobot.datasets.lerobot_dataset import LeRobotDataset  # Note: module name is 'datasets' (plural)

dataset = LeRobotDataset("RoboCOIN/Cobot_Magic_move_the_bread")

dataloader = torch.utils.data.DataLoader(
    dataset,
    num_workers=8,
    batch_size=32,
)

lerobot-features说明

observation.state / action feature

表示从机械臂(从臂/主臂)采集到的数据。如果数采机器人没有action数据,则使用observation.state数据填充action数据。features典型的命名方式及说明如下:

feature 单位 说明
{dir}_arm_joint_{num}_rad rad 由采集数据转换而成,表示机械臂的关节角(从臂/主臂)。
{dir}_hand_joint_{num}_rad rad 由采集数据转换而成,表示手部关节角。
{dir}_gripper_open_scale - 取值范围为 [0, 1];0 表示完全闭合,1表示完全张开;由采集数据转换而成。
{dir}_eef_pos_{axis} m Robot SDK获取的末端位置(单位为米)。
{dir}_eef_rot_{axis} rad Robot SDK获取的末端姿态(欧拉角,单位为弧度)。

eef_sim_pose_state/eef_sim_pose_action

表示在仿真环境中计算得到的统一坐标系下机器人末端位姿态数据。在observation.state / action中,由于各数采机器人SDK定义的坐标系不一致,所以我们使用仿真方法,获得了各机器人末端在统一坐标系(x前/y左/z上,坐标系原点为机器人底盘或双脚中心)下的位姿数据,并用 eef_sim_pose_state/eef_sim_pose_action feature表示。

注:此处的 {dir} 为统一占位符,代表 leftright


重点预告

  • 版本兼容性:当前 RoboCOIN 支持 LeRobot v2.1 数据格式,v3.0 版本数据格式支持将很快推出。
  • 代码来源:当前本项目基于 LeRobot v0.3.4 开发,未来RoboCOIN将提供LeRobot扩展功能模块,并完全兼容LeRobot官方仓库。

机器人控制逻辑

机器人控制模块为多种机器人平台提供统一接口,支持 SDK 直控(如 Piper、Realman)和通用 ROS/MoveIt 控制,包含标准化单位转换、绝对 / 相对位置控制、轨迹可视化等功能。

graph LR
    subgraph 机器人底层接口
    A1[统一单位转换]
    A2[绝对/相对位置控制]
    A3[相机与轨迹可视化]
    A[机器人底层接口]
    end
    
    %% 机器人服务层
    subgraph 机器人服务
    C[机器人服务]
    C1[SDK 直控]
    C2[ROS 控制]
    C11[Agilex Piper 服务]
    C12[Realman 服务]
    C13[其他机器人服务]
    C21[通用机器人服务]
    end
    
    %% 相机服务层
    subgraph 相机服务
    D[相机服务]
    D1[OpenCV 相机服务]
    D2[RealSense 相机服务]
    end
    
    %% 推理服务层
    subgraph 推理服务
    E[推理服务]
    E1[RPC 通信]
    E11[LeRobot 策略]
    E2[WebSocket 通信]
    E21[OpenPi 策略]
    end
    
    %% 连接关系
    A1 --- A
    A2 --- A
    A3 --- A

    C --- C1
    C --- C2
    C1 --- C11
    C1 --- C12
    C1 --- C13
    C2 --- C21
    
    D --- D1
    D --- D2

    E --- E1
    E1 --- E11
    E --- E2
    E2 --- E21

    A --- C
    A --- D
    A --- E
Loading

机器人目录结构

所有机器人脚本都在src/lerobot/robots下,以Realman机器人平台为例,相应的所有文件位于src/lerobot/robots/realman(单臂)与src/lerobot/robots/bi_realman(双臂)下:

realman # 单臂
├── __init__.py
├── configuration_realman.py # 配置类
├── realman.py               # 关节控制
└── realman_end_effector.py  # 末端控制

bi_realman # 双臂
├── __init__.py
├── bi_realman.py               # 关节控制
├── bi_realman_end_effector.py  # 末端控制
└── configuration_bi_realman.py # 配置类

机器人基础配置类

继承关系

graph LR
    A[RobotConfig] --> B[BaseRobotConfig]
    B --> C[BaseRobotEndEffectorConfig]
    B --> D[BiBaseRobotConfig]
    D --> E[BiBaseRobotEndEffectorConfig]
    C --> E
Loading

机器人平台的基础配置位于src/lerobot/robots/base_robot/configuration_base_robot.py

# 关节控制的基础配置类
@RobotConfig.register_subclass("base_robot")
@dataclass
class BaseRobotConfig(RobotConfig):
    # 相机设置,表示为字典,字典key为相机名,value为相机配置类,如
    # {
    #     head: {type: opencv, index_or_path:0, height: 480, width: 640, fps: 30}, 
    #     wrist: {type: opencv, index_or_path:1, height: 480, width: 640, fps: 30},
    # }
    # 上述示例创建了head和wrist两个相机,分别加载了/dev/video0, /dev/video1
    # 最终发送给模型的将是{"observation.head": shape(480, 640, 3), "observation.wrist": shape(480, 640, 3)}
    cameras: dict[str, CameraConfig] = field(default_factory=dict)
    # 关节名称,包含夹爪
    joint_names: list[str] = field(default_factory=lambda: [
        'joint_1', 'joint_2', 'joint_3', 'joint_4', 'joint_5', 'joint_6', 'joint_7', 'gripper',
    ]) 

    # 初始化模式:none表示不进行初始化,joint/end_effector表示基于关节/末端初始化
    init_type: str = 'none'
    # 根据初始化模式,在开始推理之前要初始化的值
    # 对于joint,单位为radian
    # 对于end_effector,单位为m(前3个值) / radian(3~6个值)
    init_state: list[float] = field(default_factory=lambda: [
        0, 0, 0, 0, 0, 0, 0, 0,
    ])

    # 各关节控制单位,视SDK而定,如Realman SDK共7个关节,接收角度作为参数,则应设为:
    # ['degree', 'degree', 'degree', 'degree', 'degree', 'degree', 'degree', 'm']
    # 最后一维为m,表示夹爪值不用进行单位转换
    joint_units: list[str] = field(default_factory=lambda: [
        'radian', 'radian', 'radian', 'radian', 'radian', 'radian', 'radian', 'm',
    ])
    # 末端控制单位,视SDK而定,如Realman SDK接收米作为xyz和角度作为rpy,则应设为:
    # ['m', 'm', 'm', 'degree', 'degree', 'degree', 'm']
    # 最后一维为m,表示夹爪值不用进行单位转换
    pose_units: list[str] = field(default_factory=lambda: [
        'm', 'm', 'm', 'radian', 'radian', 'radian', 'm',
    ])
    # 模型接收的关节控制单位,视数据集而定,如数据集中保存的单位为弧度,则应设为:
    # ['radian', 'radian', 'radian', 'radian', 'radian', 'radian', 'radian', 'm']
    # 最后一维为m,表示夹爪值不用进行单位转换
    model_joint_units: list[str] = field(default_factory=lambda: [
        'radian', 'radian', 'radian', 'radian', 'radian', 'radian', 'radian', 'm',
    ])
    
    # 相对位置控制模式:none表示绝对位置控制,previous/init表示基于上一状态或初始状态进行相对转换
    # 以关节控制为例:
    # - 若为previous:则得到的state + 上一个state -> 要达到的state
    # - 若为init:则得到的state + 初始state -> 要达到的state
    delta_with: str = 'none'

    # 是否启用可视化
    visualize: bool = True
    # 是否绘制2D轨迹图,包含XY, XZ, YZ平面上的末端轨迹
    draw_2d: bool = True
    # 是否绘制3D轨迹图
    draw_3d: bool = True


# 末端控制的基础配置类
@RobotConfig.register_subclass("base_robot_end_effector")
@dataclass
class BaseRobotEndEffectorConfig(BaseRobotConfig):
    # 相对变换角,适用于跨本体的情况,即不同本体的零姿态具有不同的朝向,则需要通过该参数进行变换
    base_euler: list[float] = field(default_factory=lambda: [0.0, 0.0, 0.0])

    # 模型接收的末端控制单位,视数据集而定,如数据集中保存的单位是米和弧度,则应设为:
    # ['m', 'm', 'm', 'radian', 'radian', 'radian', 'm']
    # 最后一维为m,表示夹爪值不用进行单位转换
    model_pose_units: list[str] = field(default_factory=lambda: [
        'm', 'm', 'm', 'radian', 'radian', 'radian', 'm',
    ])

参数详解:

参数名 类型 默认值 说明
cameras dict[str, CameraConfig] {} 相机配置字典,键为相机名称,值为相机配置
joint_names List[str] ['joint_1', 'joint_2', 'joint_3', 'joint_4', 'joint_5', 'joint_6', 'joint_7', 'gripper'] 关节名称列表,包括夹爪
init_type str 'none' 初始化类型,可选:'none', 'joint', 'end_effector'
init_state List[float] [0, 0, 0, 0, 0, 0, 0, 0] 初始状态:init_type='joint'时为关节状态,init_type='end_effector'时为末端执行器状态
joint_units List[str] ['radian', 'radian', 'radian', 'radian', 'radian', 'radian', 'radian', 'm'] 机器人关节单位,用于SDK控制
pose_units List[str] ['m', 'm', 'm', 'radian', 'radian', 'radian', 'm'] 末端执行器位姿单位,用于SDK控制
model_joint_units List[str] ['radian', 'radian', 'radian', 'radian', 'radian', 'radian', 'radian', 'm'] 模型关节单位,用于模型输入/输出
delta_with str 'none' 增量控制模式:'none'(绝对控制), 'previous'(相对上一状态), 'initial'(相对初始状态)
visualize bool True 是否启用可视化
draw_2d bool True 是否绘制2D轨迹
draw_3d bool True 是否绘制3D轨迹

双臂机器人的基础配置类位于src/lerobot/robots/base_robot/configuration_bi_base_robot.py,继承自单臂基础配置类:

# 双臂机器人配置
@RobotConfig.register_subclass("bi_base_robot")
@dataclass
class BiBaseRobotConfig(BaseRobotConfig):
    # 左臂初始姿态
    init_state_left: List[float] = field(default_factory=lambda: [
        0, 0, 0, 0, 0, 0, 0, 0,
    ])
    # 右臂初始姿态
    init_state_right: List[float] = field(default_factory=lambda: [
        0, 0, 0, 0, 0, 0, 0, 0,
    ])


# 双臂机器人末端配置
@RobotConfig.register_subclass("bi_base_robot_end_effector")
@dataclass
class BiBaseRobotEndEffectorConfig(BiBaseRobotConfig, BaseRobotEndEffectorConfig):
    pass

参数详解:

参数名 类型 默认值 说明
init_state_left List[float] [0, 0, 0, 0, 0, 0, 0, 0] 左臂初始关节状态
init_state_right List[float] [0, 0, 0, 0, 0, 0, 0, 0] 右臂初始关节状态

特定机器人配置类

每个特定机器人都有专门配置,继承机器人基础配置,请根据具体的机器人SDK进行配置

继承关系,以Realman为例:

graph LR
    A[BaseRobotConfig] --> B[RealmanConfig]
    A --> C[RealmanEndEffectorConfig]
    A --> D[BiBaseRobotConfig]
    D --> E[BiRealmanConfig]
    C --> F[BiRealmanEndEffectorConfig]
    D --> F
Loading

以Realman为例,位于src/lerobot/robots/realman/configuration_realman.py

@RobotConfig.register_subclass("realman")
@dataclass
class RealmanConfig(BaseRobotConfig):
    ip: str = "169.254.128.18" # Realman SDK连接ip
    port: int = 8080           # Realman SDK连接端口
    block: bool = False        # 是否阻塞控制
    wait_second: float = 0.1   # 如果非阻塞,每次行动后延迟多久
    velocity: int = 30         # 移动速度

    # Realman共有7个关节 + 夹爪
    joint_names: list[str] = field(default_factory=lambda: [
        'joint_1', 'joint_2', 'joint_3', 'joint_4', 'joint_5', 'joint_6', 'joint_7', 'gripper',
    ])

    # 使用joint控制达到Realman执行任务的初始姿态
    init_type: str = "joint"
    init_state: list[float] = field(default_factory=lambda: [
        -0.84, -2.03,  1.15,  1.15,  2.71,  1.60, -2.99, 888.00,
    ])

    # Realman SDK默认采用米 + 角度
    joint_units: list[str] = field(default_factory=lambda: [
        'degree', 'degree', 'degree', 'degree', 'degree', 'degree', 'degree', 'm',
    ])
    pose_units: list[str] = field(default_factory=lambda: [
        'm', 'm', 'm', 'degree', 'degree', 'degree', 'm',
    ])


@RobotConfig.register_subclass("realman_end_effector")
@dataclass
class RealmanEndEffectorConfig(RealmanConfig, BaseRobotEndEffectorConfig):
    pass

对于双臂Realman,配置类位于src/lerobot/robots/bi_realman/configuration_bi_realman.py

# 双臂Realman配置
@RobotConfig.register_subclass("bi_realman")
@dataclass
class BiRealmanConfig(BiBaseRobotConfig):
    ip_left: str = "169.254.128.18" # Realman左臂SDK连接ip
    port_left: int = 8080 # Realman左臂SDK连接端口
    ip_right: str = "169.254.128.19" # Realman右臂SDK连接ip
    port_right: int = 8080 # Realman右臂SDK连接端口
    block: bool = False # 是否阻塞控制
    wait_second: float = 0.1 # 如果非阻塞,每次行动后延迟多久
    velocity: int = 30 # 移动速度
    
    # Realman共有7个关节 + 夹爪
    joint_names: List[str] = field(default_factory=lambda: [
        'joint_1', 'joint_2', 'joint_3', 'joint_4', 'joint_5', 'joint_6', 'joint_7', 'gripper',
    ])
    
    # 使用joint控制达到Realman执行任务的初始姿态
    init_type: str = "joint"
    init_state_left: List[float] = field(default_factory=lambda: [
        -0.84, -2.03,  1.15,  1.15,  2.71,  1.60, -2.99, 888.00,
    ])
    init_state_right: List[float] = field(default_factory=lambda: [
         1.16,  2.01, -0.79, -0.68, -2.84, -1.61,  2.37, 832.00,
    ])

    # Realman SDK默认采用米 + 角度
    joint_units: List[str] = field(default_factory=lambda: [
        'degree', 'degree', 'degree', 'degree', 'degree', 'degree', 'degree', 'm',
    ])
    pose_units: List[str] = field(default_factory=lambda: [
        'm', 'm', 'm', 'degree', 'degree', 'degree', 'm',
    ])


# 双臂Realman末端执行器配置
@RobotConfig.register_subclass("bi_realman_end_effector")
@dataclass
class BiRealmanEndEffectorConfig(BiRealmanConfig, BiBaseRobotEndEffectorConfig):
    pass

特定功能说明

统一单位转换

该模块位于src/lerobot/robots/base_robot/units_transform.py,提供长度和角度测量的单位转换功能,支持在机器人控制系统中进行统一的单位管理:长度使用米(m),角度使用弧度(rad)

长度单位转换:标准单位为米(m),支持微米、毫米、厘米、米之间的转换

单位 符号 换算关系
微米 001mm 1 um = 1e-6 m
毫米 mm 1 mm = 1e-3 m
厘米 cm 1 cm = 1e-2 m
m 1 m = 1 m

角度单位转换:标准单位为弧度(rad),支持毫度、度和弧度之间的转换

单位 符号 换算关系
毫度 001deg 1(001deg) = π/18000 rad
deg 1 deg = π/180 rad
弧度 rad 1 rad = 1 rad

推理过程中,机器人平台的控制单位与模型输入/输出单位可能不同,该模块提供了统一的转换接口,确保在控制过程中单位的一致性与正确性:

  1. 机器人状态到模型输入的转换:机器人特定单位 -> 标准单位 -> 模型特定单位
  2. 模型输出到机器人控制的转换:模型特定单位 -> 标准单位 -> 机器人特定单位
sequenceDiagram
    participant A as 机器人状态(特定单位)
    participant B as 标准单位
    participant C as 模型输入/输出(特定单位)
    A -->> B: 1.转换到标准单位
    B -->> C: 2.转换到模型特定单位
    C -->> B: 3.转换到标准单位
    B -->> A: 4.转换到机器人特定单位
Loading

绝对与相对位置控制

提供绝对与相对(相对上一状态、相对初始状态)位置控制3种模式,适用于关节控制与末端执行器控制:

  1. 绝对位置控制(absolute):直接使用模型输出的位置作为目标位置
  2. 相对上一状态位置控制(relative to previous):将模型输出的位置作为相对于上一个状态的增量,计算目标位置
    • 不使用action chunking: 动作 = 当前状态 + 模型输出
    • 使用action chunking: 动作 = 当前状态 + 模型输出的所有chunk,全部执行结束后再更新当前状态
  3. 相对初始状态位置控制(relative to initial):将模型输出的位置作为相对于初始状态的增量,计算目标位置

基于相对上一状态位置控制时,使用action chunking的控制流程示例:

sequenceDiagram
    participant Model as 模型
    participant Controller as 控制器
    participant Robot as 机器人
    
    Note over Robot: 当前状态: st
    
    Model->>Controller: 输出动作序列: [a(t+1), a(t+2), ..., a(t+n)]
    
    Note over Controller: 动作始终相对于初始状态 st 进行计算

    loop 执行动作序列 i = 1 to n
        Controller->>Robot: 执行动作: st + a(t+i)
        Robot-->>Controller: 达到状态 s(t+i) = st + a(t+i)
    end
    
    Note over Robot: 最终状态: s(t+n)
Loading

使用说明

⚠️ 运行机器人控制脚本前,请仔细阅读所有配置项,并根据机器人平台 SDK 和实际环境修改参数。

轨迹重播

机器人平台的配置选项可以在配置类文件中修改,也可以通过命令行传入,以双臂Realman为例,命令如下:

python src/lerobot/scripts/replay.py \
    --repo_id=<your_lerobot_repo_id> \
    --robot.type=bi_realman \
    --robot.ip_left="169.254.128.18" \
    --robot.port_left=8080 \
    --robot.ip_right="169.254.128.19" \
    --robot.port_right=8080 \
    --robot.block=True \
    --robot.cameras="{ observation.images.cam_high: {type: opencv, index_or_path: 8, width: 640, height: 480, fps: 30}, observation.images.cam_left_wrist: {type: opencv, index_or_path: 20, width: 640, height: 480, fps: 30},observation.images.cam_right_wrist: {type: opencv, index_or_path: 14, width: 640, height: 480, fps: 30}}" \
    --robot.id=black \
    --robot.visualize=True

上述命令指定了Realman左臂与右臂的IP/端口,并加载了头部、左手、右手相机,轨迹重播时将根据<your_lerobot_repo_id>中的数据进行控制

模型推理

基于LeRobot Policy的推理 (基础版)

直接运行推理脚本,以双臂 Realman 为例,命令如下:

python src/lerobot/scripts/inference_naive.py \
    --robot.type=bi_realman \
    --robot.ip_left="169.254.128.18" \
    --robot.port_left=8080 \
    --robot.ip_right="169.254.128.19" \
    --robot.port_right=8080 \
    --robot.cameras="{front: {type: opencv, index_or_path: 8, width: 640, height: 480, fps: 30}, left_wrist: {type: opencv, index_or_path: 14, width: 640, height: 480, fps: 30}, right_wrist: {type: opencv, index_or_path: 20, width: 640, height: 480, fps: 30}}" \
    --robot.block=False \
    --robot.id=black \
    --task="do something" \
    --pretrained_path=path/to/checkpoint \
    --repo_id=realman/bi_realman_demo \
    --frequency=10 \
    --camera_keys="[front]"

上述命令初始化 Realman 姿态,加载头部、左臂腕部、右臂腕部相机,传入任务提示 “do something”,加载本地模型进行推理,并输出动作控制机器人平台。

可在终端按 “q” 随时退出,退出后按 “y/n” 标记任务成功 / 失败,视频将保存至 results/ 目录。

基于LeRobot Policy的推理 (异步RPC版)
  1. 运行LeRobot Server,详见src/lerobot/scripts/server/policy_server.py,命令如下:
python src/lerobot/scripts/server/policy_server.py \
    --host=127.0.0.1 \
    --port=18080 \
    --fps=10 

上述命令将启动一个监听在127.0.0.1:18080的服务

  1. 运行客户端程序,以双臂Realman为例,命令如下:
python src/lerobot/scripts/server/robot_client.py \
    --robot.type=bi_realman \
    --robot.ip_left="169.254.128.18" \
    --robot.port_left=8080 \
    --robot.ip_right="169.254.128.19" \
    --robot.port_right=8080 \
    --robot.cameras="{ front: {type: opencv, index_or_path: 8, width: 640, height: 480, fps: 30}, left_wrist: {type: opencv, index_or_path: 14, width: 640, height: 480, fps: 30},right_wrist: {type: opencv, index_or_path: 20, width: 640, height: 480, fps: 30}}" \
    --robot.block=False \
    --robot.id=black \
    --fps=10 \
    --task="do something" \
    --server_address=127.0.0.1:8080 \
    --policy_type=act \
    --pretrained_name_or_path=path/to/checkpoint \
    --actions_per_chunk=50 \
    --verify_robot_cameras=False

上述命令将初始化realman姿态,加载头部、左手、右手相机,传入"do something"作为prompt,加载ACT模型进行推理,并获取action对机器人平台进行控制

基于OpenPI Policy的推理
  1. 运行OpenPI Server,详见OpenPI官方仓库

  2. 运行客户端程序,以Realman为例,命令如下:

python src/lerobot/scripts/server/robot_client_openpi.py \
  --host="127.0.0.1" \ # 服务端IP
  --port=8000 \ # 服务端端口号
  --task="put peach into basket" \ # 任务指令
  --robot.type=bi_realman \ # Realman的配置项
  --robot.ip_left="169.254.128.18" \ 
  --robot.port_left=8080 \ 
  --robot.ip_right="169.254.128.19" \ 
  --robot.port_right=8080 \ 
  --robot.block=False \ 
  --robot.cameras="{ observation.images.cam_high: {type: opencv, index_or_path: 8, width: 640, height: 480, fps: 30}, observation.images.cam_left_wrist: {type: opencv, index_or_path: 14, width: 640, height: 480, fps: 30},observation.images.cam_right_wrist: {type: opencv, index_or_path: 20, width: 640, height: 480, fps: 30}}" \ # 
  --robot.init_type="joint" \
  --robot.id=black

上述命令将初始化realman姿态,加载头部、左手、右手相机,传入"put peach into basket"作为prompt,并获取action对机器人平台进行控制。

推理时,可以在控制台中按"q"随时退出,之后按"y/n"表示当前任务成功或失败,视频将被存放到results/目录中。

层次化任务描述的推理 (目前仅支持OpenPI)

首先为当前任务编写一个配置类,如src/lerobot/scripts/server/task_configs/towel_basket.py:

@dataclass
class TaskConfig:
    # 场景描述
    scene: str = "a yellow basket and a grey towel are place on a white table, the basket is on the left and the towel is on the right."
    # 任务指令
    task: str = "put the towel into the basket."
    # 子任务指令
    subtasks: List[str] = field(default_factory=lambda: [
        "left gripper catch basket",
        "left gripper move basket to center",
        "right gripper catch towel",
        "right gripper move towel over basket and release",
        "end",
    ])
    # 状态统计算子
    operaters: List[Dict] = field(default_factory=lambda: [
        {
            'type': 'position',
            'name': 'position_left',
            'window_size': 1,
            'state_key': 'observation.state',
            'xyz_range': (0, 3),
        }, {
            'type': 'position',
            'name': 'position_right',
            'window_size': 1,
            'state_key': 'observation.state',
            'xyz_range': (7, 10),
        }, {
            'type': 'position_rotation',
            'name': 'position_aligned_left',
            'window_size': 1,
            'position_key': 'position_left',
            'rotation_euler': (0, 0, 0.5 * math.pi),
        }, {
            'type': 'position_rotation',
            'name': 'position_aligned_right',
            'window_size': 1,
            'position_key': 'position_right',
            'rotation_euler': (0, 0, 0.5 * math.pi),
        }, {
            'type': 'movement',
            'name': 'movement_left',
            'window_size': 3,
            'position_key': 'position_aligned_left',
        }, {
            'type': 'movement',
            'name': 'movement_right',
            'window_size': 3,
            'position_key': 'position_aligned_right',
        },{
            'type': 'movement_summary',
            'name': 'movement_summary_left',
            'movement_key': 'movement_left',
            'threshold': 2e-3,
        }, {
            'type': 'movement_summary',
            'name': 'movement_summary_right',
            'movement_key': 'movement_right',
            'threshold': 2e-3,
        }, 
    ])

之后运行命令:

python src/lerobot/scripts/server/robot_client_openpi_anno.py \
  --host="127.0.0.1" \
  --port=8000 \
  --task_config_path="lerobot/scripts/server/task_configs/towel_basket.py" \
  --robot.type=bi_realman_end_effector \
  --robot.ip_left="169.254.128.18" \
  --robot.port_left=8080 \
  --robot.ip_right="169.254.128.19" \
  --robot.port_right=8080 \
  --robot.block=False \
  --robot.cameras="{ observation.images.cam_high: {type: opencv, index_or_path: 8, width: 640, height: 480, fps: 30}, observation.images.cam_left_wrist: {type: opencv, index_or_path: 14, width: 640, height: 480, fps: 30},observation.images.cam_right_wrist: {type: opencv, index_or_path: 20, width: 640, height: 480, fps: 30}}" \
  --robot.init_type="joint" \
  --robot.id=black

推理时,将从第一个子任务开始,按"s"切换到下一个子任务。 可以在控制台中按"q"随时退出,之后按"y/n"表示当前任务成功或失败,视频将被存放到results/目录中。

自定义功能

新增自定义机器人

  1. src/lerobot/robots/目录下创建一个新的文件夹,命名为你的机器人名称,如my_robot
  2. 在该文件夹下创建以下文件:
    • __init__.py:初始化文件
    • my_robot.py:实现机器人控制逻辑
    • configuration_my_robot.py:定义机器人配置类,继承自RobotConfig
  3. configuration_my_robot.py中定义机器人配置,包括SDK特定配置与所需的基础配置参数
  4. my_robot.py中实现机器人控制逻辑,继承自BaseRobot
  5. 实现所有抽象方法:
    • _check_dependencys(self): 检查机器人所需的依赖项
    • _connect_arm(self): 连接到机器人
    • _disconnect_arm(self): 断开与机器人的连接
    • _set_joint_state(self, joint_state: np.ndarray): 设置机器人的关节状态,输入为关节状态的numpy数组,单位为配置类中定义的joint_units
    • _get_joint_state(self) -> np.ndarray: 获取机器人的当前关节状态,返回值为关节状态的numpy数组,单位为配置类中定义的joint_units
    • _set_ee_state(self, ee_state: np.ndarray): 设置机器人的末端执行器状态,输入为末端执行器状态的numpy数组,单位为配置类中定义的pose_units
    • _get_ee_state(self) -> np.ndarray: 获取机器人的当前末端执行器状态,返回值为末端执行器状态的numpy数组,单位为配置类中定义的pose_units
  6. 参照其他机器人实现类,实现其他控制方式(可选):
    • my_robot_end_effector.py:实现基于末端执行器的控制逻辑,继承自BaseRobotEndEffectormy_robot.py
    • bi_my_robot.py:实现双臂机器人的控制逻辑,继承自BiBaseRobotmy_robot.py
    • bi_my_robot_end_effector.py:实现双臂机器人基于末端执行器的控制逻辑,继承自BiBaseRobotEndEffectormy_robot_end_effector.py
  7. src/lerobot/robots/utils.py中注册你的机器人配置类:
    elif robot_type == "my_robot":
        from .my_robot.configuration_my_robot import MyRobotConfig
        return MyRobotConfig(**config_dict)
    elif robot_type == "my_robot_end_effector":
        from .my_robot.configuration_my_robot import MyRobotEndEffectorConfig
        return MyRobotEndEffectorConfig(**config_dict)
    elif robot_type == "bi_my_robot":
        from .my_robot.configuration_my_robot import BiMyRobotConfig
        return BiMyRobotConfig(**config_dict)
    elif robot_type == "bi_my_robot_end_effector":
        from .my_robot.configuration_my_robot import BiMyRobotEndEffectorConfig
        return BiMyRobotEndEffectorConfig(**config_dict)
  8. 在推理脚本开头导入你的机器人实现类:
    from lerobot.robots.my_robot.my_robot import MyRobot
    from lerobot.robots.my_robot.my_robot_end_effector import MyRobotEndEffector
    from lerobot.robots.my_robot.bi_my_robot import BiMyRobot
    from lerobot.robots.my_robot.bi_my_robot_end_effector import BiMyRobotEndEffector
  9. 现在你可以通过命令行参数--robot.type=my_robot来使用你的自定义机器人了

机器人遥操作

遥操作模块为多种机器人平台提供遥操作功能,支持通过多种设备控制机器人,包括基于代码的遥操作和基于硬件的直接遥操作。

graph LR
    Robot[机器人]
    Record[记录脚本]

    subgraph 遥操作模式
        CodeBased[基于代码的遥操作]
        HardwareBased[基于硬件的遥操作]
    end

    subgraph 遥操作设备类型
        ControllerGroup[多样化控制器] --> LeaderArm[主臂]
        ControllerGroup --> Keyboard[键盘]
        ControllerGroup --> Gamepad[游戏手柄]
        VendorDevice[厂商专属硬件]
        VendorDevice --> LeaderArm2[主臂]
        VendorDevice --> ExoSkeleton[外骨骼]
        VendorDevice --> DataGlove[数据手套]
    end

    Robot --- Record
    Record --- CodeBased
    Record --- HardwareBased
    CodeBased --- ControllerGroup
    HardwareBased --- VendorDevice
Loading

遥操作脚本结构

所有遥操作脚本均位于 src/lerobot/teleoperators 目录下。以 Realman 遥操作器为例,相关文件位于 src/lerobot/teleoperators/realman_leader

realman_leader # 单臂遥操作器
├── __init__.py
├── configuration_realman_leader.py # 配置类
├── realman_leader.py               # 遥操作逻辑
└── realman_leader_end_effector.py  # 末端执行器遥操作逻辑

bi_realman_leader # 双臂遥操作器
├── __init__.py
├── configuration_bi_realman_leader.py # 配置类
├── bi_realman_leader.py               # 遥操作逻辑
└── bi_realman_leader_end_effector.py  # 末端执行器遥操作逻辑

遥操作基础配置类

graph LR
    A[TeleoperatorConfig] 
    B[BaseLeaderConfig]
    C[BaseLeaderEndEffectorConfig]
    D[BiBaseLeaderConfig]
    E[BiBaseLeaderEndEffectorConfig]
    A --> B
    B --> C
    B --> D
    D --> E
    C --> E
Loading

遥操作基础配置位于 src/lerobot/teleoperators/base_teleoperator/config_base_teleoperator.py

"""
BaseLeader 遥操作器配置
"""

from dataclasses import dataclass, field
from typing import List

from ..config import TeleoperatorConfig


@TeleoperatorConfig.register_subclass("base_leader")
@dataclass
class BaseLeaderConfig(TeleoperatorConfig):
    """
    BaseLeader 遥操作器配置类
    参数说明:
    - joint_names: 关节名称列表(含夹爪)
    - joint_units: 机器人关节控制单位(适配 SDK)
    - pose_units: 末端执行器控制单位(适配 SDK)
    - model_joint_units: 模型输入/输出关节单位(适配数据集)
    """

    # 关节名称列表(含夹爪)
    joint_names: List[str] = field(default_factory=lambda: [
        "joint_1", "joint_2", "joint_3", "joint_4", "joint_5", "joint_6", "joint_7", "gripper",
    ])

    # 初始化类型和状态(每轮任务开始时执行)
    # 'none': 不初始化
    # 'joint': 关节角度初始化
    # 'end_effector': 末端执行器位姿初始化
    init_type: str = "none"  # 可选值:'none', 'joint', 'end_effector'
    init_state: List[float] = field(default_factory=lambda: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0])
    init_threshold: float = 0.1  # 初始化完成阈值(状态误差小于该值则认为初始化成功)

    # 机器人关节控制单位(适配 SDK)
    joint_units: List[str] = field(default_factory=lambda: [
        "radian", "radian", "radian", "radian", "radian", "radian", "radian", "m",
    ])
    # 末端执行器控制单位(适配 SDK)
    pose_units: List[str] = field(default_factory=lambda: [
        "m", "m", "m", "radian", "radian", "radian", "m",
    ])
    # 模型输入/输出关节单位(适配数据集)
    model_joint_units: List[str] = field(default_factory=lambda: [
        "radian", "radian", "radian", "radian", "radian", "radian", "radian", "m",
    ])


@TeleoperatorConfig.register_subclass("base_leader_end_effector")
@dataclass
class BaseLeaderEndEffectorConfig(BaseLeaderConfig):
    """
    BaseLeaderEndEffector 遥操作器配置类
    扩展 BaseLeaderConfig,增加末端执行器专属参数
    参数说明:
    - base_euler: 机器人 SDK 控制坐标系相对于模型坐标系的旋转角度(暂未实现)
    - model_pose_units: 模型输入/输出末端执行器单位(适配数据集)
    """

    # 机器人 SDK 控制坐标系相对于模型坐标系的旋转角度
    base_euler: List[float] = field(default_factory=lambda: [0.0, 0.0, 0.0])
    # 模型输入/输出末端执行器单位(适配数据集)
    model_pose_units: List[str] = field(default_factory=lambda: [
        "m", "m", "m", "radian", "radian", "radian", "m",
    ])

参数详情:

参数名称 类型 默认值 描述
joint_names List[str] ["joint_1", "joint_2", ..., "gripper"] 关节名称列表(含夹爪)
init_type str "none" 初始化类型,可选值:"none"(不初始化)、"joint"(关节初始化)、"end_effector"(末端执行器初始化)
init_state List[float] [0.0, 0.0, ..., 0.0](8 个元素) 初始化目标值(关节角度或末端执行器位姿)
init_threshold float 0.1 初始化完成阈值(状态误差小于该值则认为成功)
joint_units List[str] ["radian", ..., "m"](8 个元素) 机器人关节控制单位(适配 SDK)
pose_units List[str] ["m", "m", "m", "radian", "radian", "radian", "m"] 末端执行器控制单位(适配 SDK)
model_joint_units List[str] ["radian", ..., "m"](8 个元素) 模型输入/输出关节单位(适配数据集)
base_euler List[float] [0.0, 0.0, 0.0] 坐标系旋转角度(暂未实现)
model_pose_units List[str] ["m", "m", "m", "radian", "radian", "radian", "m"] 模型输入/输出末端执行器单位(适配数据集)

双臂遥操作基础配置类位于 src/lerobot/teleoperators/bi_base_leader/config_bi_base_leader.py,继承自单臂基础配置:

"""
双臂基础遥操作器配置
"""

from dataclasses import dataclass

from lerobot.teleoperators import TeleoperatorConfig

from ..base_leader import BaseLeaderConfig, BaseLeaderEndEffectorConfig


@TeleoperatorConfig.register_subclass("bi_base_leader")
@dataclass
class BiBaseLeaderConfig(BaseLeaderConfig):
    """
    双臂基础遥操作器配置类(关节控制)
    """

    pass


@TeleoperatorConfig.register_subclass("bi_base_leader_end_effector")
@dataclass
class BiBaseLeaderEndEffectorConfig(BiBaseLeaderConfig, BaseLeaderEndEffectorConfig):
    """
    双臂基础遥操作器配置类(末端执行器控制)
    """
    
    pass

遥操作特定配置类

每个遥操作器都有专属配置类,继承自基础配置类,需根据具体机器人 SDK 进行适配。 以 Realman 遥操作器为例,继承关系:

graph LR;
    A[BaseLeaderConfig] --> B[RealmanLeaderConfig]
    A --> C[RealmanLeaderEndEffectorConfig]
    A --> D[BiBaseLeaderConfig]
    D --> E[BiRealmanLeaderConfig]
    C --> F[BiRealmanLeaderEndEffectorConfig]
    D --> F
Loading

Realman 单臂遥操作配置示例(位于 src/lerobot/teleoperators/realman_leader/config_realman_leader.py):

"""
Realman 主臂遥操作器配置
"""

from dataclasses import dataclass, field
from typing import List

from ..base_leader import BaseLeaderConfig, BaseLeaderEndEffectorConfig
from ..config import TeleoperatorConfig


@TeleoperatorConfig.register_subclass("realman_leader")
@dataclass
class RealmanLeaderConfig(BaseLeaderConfig):
    """
    Realman 主臂配置类
    参数说明:
    - ip: Realman 机器人控制器 IP 地址
    - port: Realman 机器人控制器端口
    - block: 是否启用阻塞式控制(动作完成前阻塞)
    - wait_second: 非阻塞模式下的动作延迟时间
    - velocity: 关节运动默认速度(0-100)
    - joint_names: 关节名称列表(含夹爪)
    - init_type: 初始化类型(none/joint/end_effector)
    - init_state: Realman 主臂初始关节状态
    - joint_units: 机器人关节控制单位(适配 SDK)
    - pose_units: 末端执行器控制单位(适配 SDK)
    """

    ##### Realman SDK 配置 #####
    # Realman 机器人控制器 IP 和端口
    ip: str = "169.254.128.18"
    port: int = 8080

    # Realman 机器人含 7 个关节 + 1 个夹爪
    joint_names: List[str] = field(default_factory=lambda: [
        "joint_1", "joint_2", "joint_3", "joint_4", "joint_5", "joint_6", "joint_7", "gripper",
    ])
    
    # Realman 主臂默认初始关节状态
    init_type: str = "joint"
    init_state: List[float] = field(default_factory=lambda: [
        -0.84, -2.03,  1.15,  1.15,  2.71,  1.60, -2.99, 888.00,
    ])

    # Realman SDK 默认单位:角度(degree)+ 米(m)
    joint_units: List[str] = field(default_factory=lambda: [
        "degree", "degree", "degree", "degree", "degree", "degree", "degree", "m",
    ])
    pose_units: List[str] = field(default_factory=lambda: [
        "m", "m", "m", "degree", "degree", "degree", "m",
    ])


@TeleoperatorConfig.register_subclass("realman_leader_end_effector")
@dataclass
class RealmanLeaderEndEffectorConfig(RealmanLeaderConfig, BaseLeaderEndEffectorConfig):
    """
    Realman 主臂末端执行器遥操作配置类
    """

    pass

Realman 双臂遥操作配置示例(位于 src/lerobot/teleoperators/bi_realman_leader/config_bi_realman_leader.py):

"""
双臂 Realman 主臂遥操作器配置
"""

from dataclasses import dataclass, field
from typing import List

from ..base_leader import BaseLeaderConfig, BaseLeaderEndEffectorConfig
from ..config import TeleoperatorConfig


@TeleoperatorConfig.register_subclass("bi_realman_leader")
@dataclass
class BiRealmanLeaderConfig(BaseLeaderConfig):
    """
    双臂 Realman 主臂配置类
    """
    ##### Realman SDK 配置 #####
    # 左右臂 Realman 机器人控制器 IP 和端口
    ip_left: str = "169.254.128.18"
    port_left: int = 8080
    ip_right: str = "169.254.128.19"
    port_right: int = 8080

    # 双臂 Realman 机器人含 7 个关节 + 1 个夹爪(每臂)
    joint_names: List[str] = field(default_factory=lambda: [
        "joint_1", "joint_2", "joint_3", "joint_4", "joint_5", "joint_6", "joint_7", "gripper",
    ])

    # 双臂 Realman 主臂默认初始关节状态
    init_type: str = "joint"
    init_state_left: List[float] = field(default_factory=lambda: [
        -0.84, -2.03,  1.15,  1.15,  2.71,  1.60, -2.99, 888.00,
    ])
    init_state_right: List[float] = field(default_factory=lambda: [
         1.16,  2.01, -0.79, -0.68, -2.84, -1.61,  2.37, 832.00,
    ])

    # Realman SDK 默认单位:角度(degree)+ 米(m)
    joint_units: List[str] = field(default_factory=lambda: [
        "degree", "degree", "degree", "degree", "degree", "degree", "degree", "m",
    ])
    pose_units: List[str] = field(default_factory=lambda: [
        "m", "m", "m", "degree", "degree", "degree", "m",
    ])


@TeleoperatorConfig.register_subclass("bi_realman_leader_end_effector")
@dataclass
class BiRealmanLeaderEndEffectorConfig(BiRealmanLeaderConfig, BaseLeaderEndEffectorConfig):
    """
    双臂 Realman 主臂末端执行器遥操作配置类
    """

    pass

使用说明

⚠️ 运行机器人遥操作脚本前,请仔细阅读所有配置项,并根据机器人平台 SDK 和实际环境修改参数。

主从臂遥操作 (代码逻辑)

  1. 将主从臂硬件设备连接至计算机;
  2. 运行遥操作录制脚本,以 Piper 机器人为例,命令如下:
lerobot-record \
    --robot.type=piper \
    --robot.can=can0 \
    --robot.cameras="{observation.images.cam_front: {type: opencv, index_or_path: 0, width: 640, height: 480, fps: 30}}" \
    --robot.id=piper_follower \
    --teleop.type=piper_leader \
    --teleop.can=can1 \
    --teleop.id=piper_leader \
    --dataset.repo_id=<your_lerobot_repo_id> \
    --dataset.num_episodes=3 \
    --dataset.single_task="执行具体任务" \
    --dataset.push_to_hub=False

上述命令通过 CAN 总线 can0 连接 Piper 从臂,加载前置相机,通过 CAN 总线 can1 连接 Piper 主臂,录制 3 轮 “执行具体任务” 的遥操作数据,并保存至 <your_lerobot_repo_id>。

操作说明:按右方向键(->)开始下一轮任务,按左方向键(<-)重复上一轮任务,按 “esc” 键随时退出遥操作。

硬件设备直接遥操作 (绕过代码逻辑)

上述脚本通过遥操作代码逻辑实现控制,但许多机器人支持基于硬件的直连控制(如 Piper 可通过 CAN 总线直接关联主从臂),实时性和稳定性更优。

此时可绕过代码逻辑,直接通过机器人 SDK 基于主臂硬件输入控制从臂:

lerobot-record \
    --robot.type=piper \
    --robot.can=can0 \
    --robot.cameras="{observation.images.cam_front: {type: opencv, index_or_path: 0, width: 640, height: 480, fps: 30}}" \
    --robot.id=piper_follower \
    --robot.use_hardware_teleop=True \ # 启用硬件直连遥操作(绕过代码逻辑)
    --teleop.can=can0 \
    --teleop.type=piper_leader \
    --teleop.id=piper_leader \
    --dataset.repo_id=<your_lerobot_repo_id> \
    --dataset.num_episodes=3 \
    --dataset.single_task="do something" \
    --dataset.push_to_hub=False

上述命令通过 CAN 总线 can0 同时连接 Piper 主臂和从臂,加载前置相机,录制 3 轮遥操作数据并保存至 <your_lerobot_repo_id>。

添加自定义遥操作器

  1. 在 src/lerobot/teleoperators/ 目录下创建自定义遥操作器文件夹,命名为 my_robot_leader;
  2. 在该文件夹下创建以下文件:
    • init.py:初始化文件;
    • my_robot_leader.py:实现遥操作控制逻辑;
    • configuration_my_robot_leader.py:定义配置类(继承自 TeleoperatorConfig);
    • my_robot_leader_end_effector.py(可选):末端执行器遥操作逻辑;
  3. 在 configuration_my_robot_leader.py 中定义配置,包括 SDK 专属参数和基础配置;
  4. 在 my_robot_leader.py 中实现控制逻辑(继承自 BaseLeader);
  5. 实现所有抽象方法:
    • _check_dependencys(self):检查遥操作器依赖环境;
    • _connect_leader(self):连接主臂硬件设备;
    • _disconnect_leader(self):断开主臂硬件连接;
    • _get_joint_state(self) -> np.ndarray:获取当前主臂关节状态(输出为 joint_units 定义的单位);
    • _get_ee_state(self) -> np.ndarray:获取当前主臂末端执行器状态(输出为 pose_units 定义的单位);
  6. (可选)参考其他遥操作器实现,扩展控制模式:
    • my_robot_leader_end_effector.py:末端执行器遥操作(继承自 BaseLeaderEndEffector 和 my_robot_leader.py);
    • bi_my_robot_leader.py:双臂遥操作(继承自 BiBaseLeader 和 my_robot_leader.py);
    • bi_my_robot_leader_end_effector.py:双臂末端执行器遥操作(继承自 BiBaseLeaderEndEffector 和 my_robot_leader_end_effector.py);
  7. 在 src/lerobot/teleoperators/utils.py 中注册配置类:
    elif teleop_type == "my_robot_leader":
        from .my_robot_leader.configuration_my_robot_leader import MyRobotLeaderConfig
        return MyRobotLeaderConfig(**config_dict)
    elif teleop_type == "my_robot_leader_end_effector":
        from .my_robot_leader.configuration_my_robot_leader import MyRobotLeaderEndEffectorConfig
        return MyRobotLeaderEndEffectorConfig(**config_dict)
    elif teleop_type == "bi_my_robot_leader":
        from .my_robot_leader.configuration_my_robot_leader import BiMyRobotLeaderConfig
        return BiMyRobotLeaderConfig(**config_dict)
    elif teleop_type == "bi_my_robot_leader_end_effector":
        from .my_robot_leader.configuration_my_robot_leader import BiMyRobotLeaderEndEffectorConfig
        return BiMyRobotLeaderEndEffectorConfig(**config_dict)

致谢

感谢以下开源项目对RoboCOIN的支持与帮助: