数据处理算子说明

逆运动学解算算子说明

IK Solver（逆运动学解算算子）用于将机器人双臂末端执行器的目标位姿转换为对应的关节角度指令。用户只需提供末端位姿轨迹数据（ROS2 rosbag格式），算子即可自动完成逆运动学求解，输出各关节的目标角度。

核心能力：

• 支持四种机器人型号：Galaxea R1、Agibot A2D、K100、青龙
• 支持三种优先级策略：手臂优先、躯干优先、固定躯干
• 支持逐帧独立求解或连续积分求解
• 支持追加写入模式，保留原始rosbag中的其他传感器数据
• 输入输出时间戳一一对应，保证时序一致性

处理流程：

输入rosbag（末端位姿） → IK Solver逆运动学求解 → 输出rosbag（关节角度）

参数配置

选择“逆运动学解算算子”后，需填写以下参数：

机器人型号规格

1. 型号总览

   表5 机器人型号总览

   型号	ROBOT_MODEL	描述	总关节数	输出关节数	优先级策略支持
   Galaxea R1	galaxea_r1	轮式双臂人形机器人	29	16	手臂优先/躯干优先/固定躯干
   Agibot A2D	agibot_a2d	双臂固定基座机器人	16	16	手臂优先/固定躯干
   K100	k100	双臂固定基座机器人	14	14	手臂优先/固定躯干
   青龙	qinglong	轮式双臂人形机器人	31	14	手臂优先/躯干优先/固定躯干

2. 各型号输出关节布局

   输出rosbag中每条消息的data数组包含对应机型的关节角度值，布局如下：

   表6 Galaxea R1（16个关节）

   索引	关节名称	说明
   data[0]	torso_joint1	躯干关节1
   data[1]	torso_joint2	躯干关节2
   data[2]	torso_joint3	躯干关节3
   data[3]	torso_joint4	躯干关节4
   data[4]	left_arm_joint1	左臂关节1
   data[5]	left_arm_joint2	左臂关节2
   data[6]	left_arm_joint3	左臂关节3
   data[7]	left_arm_joint4	左臂关节4
   data[8]	left_arm_joint5	左臂关节5
   data[9]	left_arm_joint6	左臂关节6
   data[10]	right_arm_joint1	右臂关节1
   data[11]	right_arm_joint2	右臂关节2
   data[12]	right_arm_joint3	右臂关节3
   data[13]	right_arm_joint4	右臂关节4
   data[14]	right_arm_joint5	右臂关节5
   data[15]	right_arm_joint6	右臂关节6

   表7 Agibot A2D（16个关节）

   索引	关节名称	说明
   data[0]	joint_lift_body	躯干升降关节
   data[1]	joint_body_pitch	躯干俯仰关节
   data[2]	left_arm_joint1	左臂关节1
   data[3]	left_arm_joint2	左臂关节2
   data[4]	left_arm_joint3	左臂关节3
   data[5]	left_arm_joint4	左臂关节4
   data[6]	left_arm_joint5	左臂关节5
   data[7]	left_arm_joint6	左臂关节6
   data[8]	left_arm_joint7	左臂关节7
   data[9]	right_arm_joint1	右臂关节1
   data[10]	right_arm_joint2	右臂关节2
   data[11]	right_arm_joint3	右臂关节3
   data[12]	right_arm_joint4	右臂关节4
   data[13]	right_arm_joint5	右臂关节5
   data[14]	right_arm_joint6	右臂关节6
   data[15]	right_arm_joint7	右臂关节7

   表8 K100（14个关节）

   索引	关节名称	说明
   data[0]	left_arm_joint1	左臂关节1
   data[1]	left_arm_joint2	左臂关节2
   data[2]	left_arm_joint3	左臂关节3
   data[3]	left_arm_joint4	左臂关节4
   data[4]	left_arm_joint5	左臂关节5
   data[5]	left_arm_joint6	左臂关节6
   data[6]	left_arm_joint7	左臂关节7
   data[7]	right_arm_joint1	右臂关节1
   data[8]	right_arm_joint2	右臂关节2
   data[9]	right_arm_joint3	右臂关节3
   data[10]	right_arm_joint4	右臂关节4
   data[11]	right_arm_joint5	右臂关节5
   data[12]	right_arm_joint6	右臂关节6
   data[13]	right_arm_joint7	右臂关节7

   K100 为固定基座机器人，无躯干关节，因此仅输出 14 个手臂关节角度。

   表9 青龙（14个关节）

   索引	关节名称	说明
   data[0]	left_arm_joint1	左臂关节1
   data[1]	left_arm_joint2	左臂关节2
   data[2]	left_arm_joint3	左臂关节3
   data[3]	left_arm_joint4	左臂关节4
   data[4]	left_arm_joint5	左臂关节5
   data[5]	left_arm_joint6	左臂关节6
   data[6]	left_arm_joint7	左臂关节7
   data[7]	right_arm_joint1	右臂关节1
   data[8]	right_arm_joint2	右臂关节2
   data[9]	right_arm_joint3	右臂关节3
   data[10]	right_arm_joint4	右臂关节4
   data[11]	right_arm_joint5	右臂关节5
   data[12]	right_arm_joint6	右臂关节6
   data[13]	right_arm_joint7	右臂关节7

   青龙虽为轮式人形机器人（含躯干关节），但当前版本仅输出手臂关节角度，躯干关节不包含在输出中。

输入数据集说明

1. 数据格式

   输入数据为ROS2 rosbag2格式，每个目录包含.db3文件和metadata.yaml。

   输入rosbag中需包含一个std_msgs/msg/Float64MultiArray类型的topic（默认为/whole_body_ik_solver），每条消息携带14个float64值，表示双臂末端执行器的目标位姿。

2. 输入消息布局

   所有机型输入格式相同，每帧14个float64。

   表10 青龙（14个关节）

   索引	含义	单位/说明
   data[0]	左臂末端位置x	米，相对base_link坐标系，正值向前
   data[1]	左臂末端位置y	米，正值向左
   data[2]	左臂末端位置z	米，正值向上
   data[3]	左臂末端四元数qw	四元数标量部分，无旋转时为1.0
   data[4]	左臂末端四元数qx	四元数标量部分，无旋转时为0.0
   data[5]	左臂末端四元数qy	四元数标量部分，无旋转时为0.0
   data[6]	左臂末端四元数qz	四元数标量部分，无旋转时为0.0
   data[7]	右臂末端位置x	米，相对base_link坐标系，正值向前
   data[8]	右臂末端位置y	米，正值向左
   data[9]	右臂末端位置z	米，正值向上
   data[10]	右臂末端四元数qw	四元数标量部分，无旋转时为1.0
   data[11]	右臂末端四元数qx	四元数标量部分，无旋转时为0.0
   data[12]	右臂末端四元数qy	四元数标量部分，无旋转时为0.0
   data[13]	右臂末端四元数qz	四元数标量部分，无旋转时为0.0

3. 各机型位姿参考范围

   位姿值需在机器人可达工作空间范围内，否则求解可能不收敛。

   表11 各机型位姿参考范围说明

   参数	R1	A2D	K100	青龙	说明
   x（前后）	0.2 ~ 0.8	0.2 ~ 0.6	0.2 ~ 0.6	0.2 ~ 0.8	0为底盘中心，正值向前
   y（左右）	-0.5 ~ 0.5	-0.4 ~ 0.4	-0.4 ~ 0.4	-0.5 ~ 0.5	正值向左
   z（上下）	0.8 ~ 1.8	0.8 ~ 1.6	0.8 ~ 1.6	0.8 ~ 1.8	R1/A2D标准高度1.3m，K100/青龙1.2m
   qw, qx, qy, qz	-1.0 ~ 1.0	-1.0 ~ 1.0	-1.0 ~ 1.0	-1.0 ~ 1.0	四元数标量部分

   

   四元数约束：必须为单位四元数，即qw² + qx² + qy² + qz² = 1。

   表12 常用四元数参考

   姿态	qw	qx	qy	qz
   无旋转	1.0	0.0	0.0	0.0
   绕Z轴旋转 90°	0.7071	0.0	0.0	0.7071
   绕Y轴旋转 90°	0.7071	0.0	0.7071	0.0
   绕X轴旋转 90°	0.7071	0.7071	0.0	0.0

4. 输入目录结构

   输入目录下可包含一个或多个.db3文件（标准录制的db3数据包会对应1个同名yaml配置文件，在当前算子中yaml为可选文件），算子会自动递归扫描所有子目录并逐个处理：

   ├── bag_0.db3
   ├── bag_0_metadata.yaml
   ├── task_pick/
   │   ├── session_01/
   │   │   └── pick_rosbag_0.db3
   │   └── session_02/
   │       ├── pick_rosbag_0.db3
   │       └── pick_rosbag_1.db3
   └── task_place/
       ├── place_rosbag_0.db3
       └── place_rosbag_1.db3

   

   算子会递归扫描输入目录及其所有子目录下的.db3文件，支持任意深度的目录嵌套。输出目录保持与输入相同的相对路径结构。

5. 输入数据注意事项

   表13 输入数据注意事项说明

   事项	说明
   Topic匹配	输入rosbag中的topic名称必须与SUB_TOPIC参数值一致，否则会报错
   消息类型	输入末端位姿消息类型必须为std_msgs/msg/Float64MultiArray
   数据长度	每条消息的data数组必须包含14个float64值（双臂机器人）
   时间戳	多帧消息的时间戳必须递增
   空数据	输入目录为空或db3文件中无消息时，不会崩溃，但无输出
   位姿可达性	位姿超出工作空间范围时，求解可能不收敛，输出关节角可能不正确

输出数据集说明

1. 输出格式

   输出数据为ROS2 rosbag2格式，输出目录结构与输入目录对应，每个输入db3文件生成一个输出rosbag子目录：

   输出目录/
   ├── bag_0/
   │   ├── bag_0.db3
   │   └── metadata.yaml
   ├── bag_1/
   │   ├── bag_1.db3
   │   └── metadata.yaml
   └── ...

2. 输出消息布局

   输出rosbag中包含一个std_msgs/msg/Float64MultiArray类型的topic（默认为/joint_plan_servo_controller），每条消息携带对应机型数量的float64值，表示各关节的目标角度（单位：弧度）。

   各机型的输出关节数和布局详见各型号输出关节布局。

3. 输出数据特征

   表14 输出数据特征说明

   特征	说明
   时间戳保持	输出消息的时间戳与输入一一对应，完全一致
   关节角范围	R1/K100/青龙：[-2π, 2π]； A2D：部分关节有更严格的限位（见表15 A2D机型的部分关节有较严格的限位范围）
   帧数对应	输出帧数与输入帧数一致（若某帧求解失败则跳过该帧）

4. A2D关节限位

   表15 A2D机型的部分关节有较严格的限位范围

   关节	最小值	最大值
   joint_lift_body（躯干升降）	0.0	0.5
   joint_body_pitch（躯干俯仰）	-π	π
   left_arm_joint2	-102°	102°
   left_arm_joint4	-105°	85°
   left_arm_joint6	-120°	120°
   right_arm_joint2	-102°	102°
   right_arm_joint4	-85°	105°
   right_arm_joint6	-120°	100°
   其余关节	-π	π

常见问题

LeRobot数据集V2.1转V3算子说明

输入数据集说明

LeRobotv2.1转LeRobotv3支持以下目录格式

1. 没有repo_id目录

   ├── data
   ├── images
   ├── meta
   └── videos

2. 有单层目录

   └── pick_up_balls
       ├── data
       ├── images
       ├── meta
       └── videos

3. 有多个目录

   ├── pick_up_balls
   │   ├── data
   │   ├── images
   │   ├── meta
   │   └── videos
   └── wash_dishes
       ├── data
       ├── images
       ├── meta
       └── videos

rosbag转Lerobot数据集算子说明

该数据转换算子可将符合通用ROS标准的具身机器人采集数据（如BAG或DB3格式）转换为LeRobot V21格式数据集，输出包含observation（观测）和action（动作）张量。输入为标准ROS格式数据集，可包含多个DB3文件。

输入数据集说明

rosbag数据集目录下应有config.yaml以及db3相关文件。数据格式如下:

data/
    └── config.yaml
    └── prompt.json （可选）
    └── rosbag1/    
        ├── bag1.db3
        └── target_pose.json （可选）  
    └── rosbag2/    
        ├── bag2.db3
        └── target_pose.json （可选）
    └── rosbag3/       
        ├── bag3.db3 
        └── target_pose.json （可选）

• 在数据包含多个rosbag包时，可以为每个rosbag分别设置prompt，prompt顺序与rosbag包名的字母序(升序)保持一致。如果所有rosbag的prompt均相同，则prompt.json中仅设置一条prompt即可。
• 在路径下不包含prompt.json文件时，任务转换过程中会尝试查找config.yaml中是否包含description。description为具体的动作描述性文本，在包含多个rosbag时，所有rosbag的动作描述文本均为description。
• 在description字段也不存在时，默认通过target_pose.json添加任务描述。考虑到有时在采集rosbag数据时会直接同步生成任务描述文本，可以将生成结果保存在target_pose.json文件中方便数据格式转换时添加描述。

  target_pose.json的具体内容示例如下：

  {     ...  "example_prompt": "Action description" }

• 如果数据目录下没有任何关于prompt描述的文件（prompt.json或config.yaml或target_pose.json），则会报错。

  如果存在target_pose.json但不包含"example_prompt"参数，prompt为空字符串。

config.yaml配置文件说明

config.yaml是数据转换流程的核心配置文件，用于定义ROS Bag数据到LeRobot数据集的转换规则。

配置文件主要控制以下环节：

• 数据提取：指定Processor处理器类，从ROS Bag中提取图像、关节状态、控制命令等数据。
• 时间同步：利用initial初始值对缺失数据进行填充，对齐多源时间戳。
• 数据集构建：通过features定义LeRobot数据集的特征结构。（观测状态、动作、图像等）

字段总览

字段详细说明

initial

• 类型：字典。
• 必需：是。
• 可修改：是。
• 说明：定义各组件的初始状态值。在数据同步阶段，当某时间点缺少对应数据时，系统使用此初始值进行填充。具体填充策略：
  • left_arm/right_arm：用于arm_state（最近邻填充）和arm_action（线性插值边界填充）。
  • gripper：用于gripper_state（最近邻填充）和gripper_action（前向填充边界值）。
• 约束：
  • left_arm：长度必须为6，对应左臂6个关节的初始角度。
  • right_arm：长度必须为6，对应右臂6个关节的初始角度。
  • gripper：长度必须为1，对应夹爪的初始开合宽度。

initial:
left_arm: [0.0, 2.0, -0.873, -1.57, -1.117, 1.57]
right_arm: [0.0, 2.0, -0.873, 1.57, 1.117, -1.57]
gripper: [80]

初始值应与机器人实际静止姿态一致，否则同步数据会产生偏差。

features:
observation.state:
dtype: float32
shape: [14]
names:
- left_arm_1
- left_arm_2
- left_arm_3
- left_arm_4
- left_arm_5
- left_arm_6
- left_gripper
- right_arm_1
- right_arm_2
- right_arm_3
- right_arm_4
- right_arm_5
- right_arm_6
- right_gripper
action:
dtype: float32
shape: [14]
names:
- left_arm_exp_1
- left_arm_exp_2
- left_arm_exp_3
- left_arm_exp_4
- left_arm_exp_5
- left_arm_exp_6
- left_gripper_exp
- right_arm_exp_1
- right_arm_exp_2
- right_arm_exp_3
- right_arm_exp_4
- right_arm_exp_5
- right_arm_exp_6
- right_gripper_exp
observation.images.front:
dtype: video
shape: [3, 360, 640]
names:
- channel
- height
- width
observation.images.wrist_left:
dtype: video
shape: [3, 360, 640]
names:
- channel
- height
- width
observation.images.wrist_right:
dtype: video
shape: [3, 360, 640]
names:
- channel
- height
- width

description

• 类型：字符串。
• 必需：否。
• 可修改：是。
• 说明：任务描述文本。若配置此字段，则所有episode使用统一的description作为prompt，此时不应再提供prompt.json文件。若不配置此字段，系统将按以下优先级获取prompt：
  • prompt.json文件中的多样化描述（每轮循环使用）。
  • ROS Bag同级目录下target_pose.json中的example_prompt字段。

description: "Pick up the red block and place it in the box"

description与prompt.json不应同时存在。若配置了description字段，prompt.json将被忽略。

完整配置示例

• 示例 1：双臂机器人 + 三相机（完整配置）

  processor: galaxea_o3de_processor.GalaxeaO3DEProcessor
  fps: 10
  repo_id: pick_red_test
  description: "Pick up the red block and place it in the box"
  initial:
    left_arm: [0.0, 2.0, -0.873, -1.57, -1.117, 1.57]
    right_arm: [0.0, 2.0, -0.873, 1.57, 1.117, -1.57]
    gripper: [80]
  topic_handlers:
    camera:
      - /camera_right/camera_image_color
      - /camera_left/camera_image_color
      - /camera_head_left/camera_image_color
    arm_state:
      - /joint_states
    arm_action:
      - /r1_arm_controller/follow_joint_trajectory/_action/feedback
    gripper_action:
      - /left_gripper_position_controller/commands
      - /right_gripper_position_controller/commands
  features:
    observation.state:
      dtype: float32
      shape: [14]
      names:
        - left_arm_1
        - left_arm_2
        - left_arm_3
        - left_arm_4
        - left_arm_5
        - left_arm_6
        - left_gripper
        - right_arm_1
        - right_arm_2
        - right_arm_3
        - right_arm_4
        - right_arm_5
        - right_arm_6
        - right_gripper
    action:
      dtype: float32
      shape: [14]
      names:
        - left_arm_exp_1
        - left_arm_exp_2
        - left_arm_exp_3
        - left_arm_exp_4
        - left_arm_exp_5
        - left_arm_exp_6
        - left_gripper_exp
        - right_arm_exp_1
        - right_arm_exp_2
        - right_arm_exp_3
        - right_arm_exp_4
        - right_arm_exp_5
        - right_arm_exp_6
        - right_gripper_exp
    observation.images.front:
      dtype: video
      shape: [3, 360, 640]
      names:
        - channel
        - height
        - width
    observation.images.wrist_left:
      dtype: video
      shape: [3, 360, 640]
      names:
        - channel
        - height
        - width
    observation.images.wrist_right:
      dtype: video
      shape: [3, 360, 640]
      names:
        - channel
        - height
        - width

• 示例 2：双臂机器人 + 单相机（最小配置）
  仅配置一个头部相机，图像分辨率为480x640，fps和repo_id通过函数传参指定：

  processor: galaxea_o3de_processor.GalaxeaO3DEProcessor
  initial:
    left_arm: [0.0, 2.0, -0.873, -1.57, -1.117, 1.57]
    right_arm: [0.0, 2.0, -0.873, 1.57, 1.117, -1.57]
    gripper: [80]
  topic_handlers:
    camera:
      - /camera_head_left/camera_image_color
    arm_state:
      - /joint_states
    arm_action:
      - /r1_arm_controller/follow_joint_trajectory/_action/feedback
    gripper_action:
      - /left_gripper_position_controller/commands
      - /right_gripper_position_controller/commands
  features:
    observation.state:
      dtype: float32
      shape: [14]
      names:
        - left_arm_1
        - left_arm_2
        - left_arm_3
        - left_arm_4
        - left_arm_5
        - left_arm_6
        - left_gripper
        - right_arm_1
        - right_arm_2
        - right_arm_3
        - right_arm_4
        - right_arm_5
        - right_arm_6
        - right_gripper
    action:
      dtype: float32
      shape: [14]
      names:
        - left_arm_exp_1
        - left_arm_exp_2
        - left_arm_exp_3
        - left_arm_exp_4
        - left_arm_exp_5
        - left_arm_exp_6
        - left_gripper_exp
        - right_arm_exp_1
        - right_arm_exp_2
        - right_arm_exp_3
        - right_arm_exp_4
        - right_arm_exp_5
        - right_arm_exp_6
        - right_gripper_exp
    observation.images.front:
      dtype: video
      shape: [3, 480, 640]
      names:
        - channel
        - height
        - width

• 示例 3：双臂机器人 + 双相机

  配置头部相机 + 右腕相机，使用description字段提供统一任务描述：

  processor: galaxea_o3de_processor.GalaxeaO3DEProcessor
  fps: 15
  repo_id: pour_water_task
  description: "Pour water from the bottle into the cup"
  initial:
    left_arm: [0.0, 1.5, -0.5, -1.2, -0.8, 1.2]
    right_arm: [0.0, 1.5, -0.5, 1.2, 0.8, -1.2]
    gripper: [60]
  topic_handlers:
    camera:
      - /camera_head_left/camera_image_color
      - /camera_right/camera_image_color
    arm_state:
      - /joint_states
    arm_action:
      - /r1_arm_controller/follow_joint_trajectory/_action/feedback
    gripper_action:
      - /left_gripper_position_controller/commands
      - /right_gripper_position_controller/commands
  features:
    observation.state:
      dtype: float32
      shape: [14]
      names:
        - left_arm_1
        - left_arm_2
        - left_arm_3
        - left_arm_4
        - left_arm_5
        - left_arm_6
        - left_gripper
        - right_arm_1
        - right_arm_2
        - right_arm_3
        - right_arm_4
        - right_arm_5
        - right_arm_6
        - right_gripper
    action:
      dtype: float32
      shape: [14]
      names:
        - left_arm_exp_1
        - left_arm_exp_2
        - left_arm_exp_3
        - left_arm_exp_4
        - left_arm_exp_5
        - left_arm_exp_6
        - left_gripper_exp
        - right_arm_exp_1
        - right_arm_exp_2
        - right_arm_exp_3
        - right_arm_exp_4
        - right_arm_exp_5
        - right_arm_exp_6
        - right_gripper_ex
    observation.images.front:
      dtype: video
      shape: [3, 360, 640]
      names:
        - channel
        - height
        - width
    observation.images.wrist_right:
      dtype: video
      shape: [3, 360, 640]
      names:
        - channel
        - height
        - width

校验规则

系统在加载config.yaml时会执行以下校验：

1. 顶层结构：文件必须为YAML字典格式。
2. 必需字段：processor、initial、topic_handlers、features必须存在。
3. initial校验：
   • left_arm必须为长度6的列表；
   • right_arm必须为长度6的列表；
   • gripper必须为长度1的列表。
4. topic_handlers校验：
   • camera必须存在，列表长度1~3；
   • arm_state列表长度不超过1；
   • arm_action列表长度不超过1；
   • gripper_action列表长度不超过2。
5. features与camera一致性校验：
   • 话题名含head或front→features中必须包含observation.images.front；
   • 话题名含left→features中必须包含observation.images.wrist_left；
   • 话题名含right→features中必须包含observation.images.wrist_right。
6. 话题消息类型校验（运行时）：
   • camera话题消息类型必须为sensor_msgs/msg/Image；
   • arm_state话题消息类型必须为sensor_msgs/msg/JointState；
   • arm_action话题消息类型必须为control_msgs/action/FollowJointTrajectory_FeedbackMessage；
   • gripper_action话题消息类型必须为std_msgs/msg/Float64MultiArray。