从遥操臂（GELLO）到机械臂（Franka FR3）的运动映射

整个映射架构是 1:1 关节空间直接映射——没有运动学重定向、没有末端位姿映射、也没有缩放。GELLO 遥操作设备和 FR3 机械臂共享相同的 7-DoF 串联运动学结构，所以映射就是"关节角度直接对应"。

端到端数据流

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
Dynamixel 电机（GELLO / 主端）
  │  原始脉冲数
  ▼
DynamixelDriver.get_joints()  →  脉冲 → 弧度
  │
  ▼
GelloHardware.process_arm_joint_positions()   ← 核心映射逻辑
  │  增量追踪 + joint_signs + 裁剪到 FR3 限位
  ▼
GelloPublisher.publish_joint_jog()  →  ROS2 topic: /gello/joint_states
  │
  ▼
JointImpedanceController.jointStateCallback_()   ← 从端接收
  │  deadband + 指数平滑滤波
  ▼
JointImpedanceController.update()  →  q_goal[i] = gello_position_values_[i]
  │
  ▼
PD+ 阻抗控制: τ = Kp*(q_goal - q) + Kd*(-dq_filtered)
  │
  ▼
Franka FR3（从端机械臂）

关键文件与映射逻辑

1. 主端核心：gello_hardware.py

这是映射的核心所在。关键方法：normalize_joint_positions()（第 91-133 行）——将原始电机弧度转换为标准化关节位置：

1
normalized = mod((raw - assembly_offsets) * joint_signs - mid_positions, 2π) - π + mid_positions

步骤：

  1. 减去装配偏移量（assembly_offsets）——补偿 Dynamixel 电机在 GELLO 机构中的物理安装角度
  2. 乘以关节方向符号（joint_signs，+1 或 -1）——处理电机旋转方向
  3. 绕到 [mid - π, mid + π) 范围内——其中 mid 是 FR3 各关节限位的中心点
为什么要先减去mid再加上mid?

因为这里是为了取和mid最近的mod的raw_offset, 而不是单纯的只是取mod, 而先加后减的作用是恢复原来的值, 而不是相对于mid的值

process_arm_joint_positions()（第 229-247 行）——初始化后的增量追踪：

1
2
3
4
delta = (current_raw - prev_raw) * joint_signs
position = prev_position + delta
# 裁剪到 FR3 关节限位
position = clip(position, JOINT_POSITION_LIMITS)
  • 这里由于变换的弧度值实际上很小, 所以可以在这里做这样的增量调整
  • 这里 JOINT_POSITION_LIMITS 是 Franka FR3 的关节限位（来自 Franka 官方文档），不是 GELLO 的限位。这是整个映射中唯一的"约束"——确保发出的目标位置不会超出 FR3 的运动范围。
  • _goal_position_to_pulses()（第 263-281 行）——反向映射，用于写回零力矩目标位置（重力补偿模式）。

2. 主端发布：gello_publisher.py

从 GelloHardware.get_joint_and_gripper_positions() 读取 7 个关节位置，以 FR3 关节名（fr3_joint1 ~ fr3_joint7）发布到 ROS2 topic gello/joint_states。

3. 从端控制器：joint_impedance_controller.cpp）

  • jointStateCallback_()（第 104-125 行）——订阅 gello/joint_states，提取 7 个关节位置传入滤波器。
  • filterGelloPositions_()（第 225-240 行）——对 GELLO 来的位置做两重滤波：
    • Deadband（gello_deadband = 0.003 rad）：忽略小于此阈值的微小变化，抑制手抖
    • 指数平滑（gello_filter_alpha = 0.35）：每次只接受 35% 的增量，平滑主端动作
    1
    2
    if |delta| > deadband:
        filtered += 0.35 * delta
  • update()（第 51-102 行）——主控制循环分三个阶段：
    1. Phase 1：等待有效关节状态消息
    2. Phase 2：用 MotionGenerator（S 曲线速度规划，speed_factor=0.2）平滑地从当前位姿过渡到第一个 GELLO 位置
    3. Phase 3（正常运行）：直接将滤波后的 GELLO 位置复制为目标：
    1
    q_goal(i) = gello_position_values_[i];  // 1:1 直接映射
  • calculateTauDGains_()（第 217-223 行）——PD+ 阻抗控制律：
1
τ = Kp * (q_goal - q) + Kd * (-dq_filtered)

其中 dq_filtered 是低通滤波后的速度（k_alpha = 0.99），Kp 和 Kd 是各关节独立的刚度和阻尼增益。

4. 标定：get_offsets.py

计算每个 Dynamixel 电机在 GELLO 机构中的物理安装偏移量——将原始读数与期望的 home 位姿比较，差值四舍五入到最近的 90°（π/2）整数倍。

数据记录流程详解

数据记录的核心在 my_vla_package 中，使用的是 LeRobot 数据集格式。整个系统由两个 ROS2 节点协作完成。

架构总览

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
键盘输入                    传感器数据源
   │                           │
   ▼                           ▼
keyboard_trigger          ┌─────────────┐
  (空格=START/STOP)        │ 双目相机     │→ /camera/camera_base/color/image_raw
  (D=DISCARD)             │             │→ /camera/camera_wrist/color/image_raw
   │                      ├─────────────┤
   │  /record_command     │ FR3 机械臂   │→ /franka/joint_states (7 个关节位置)
   ▼  (String)            ├─────────────┤
vla_data_recorder  ←──────│ FR3 夹爪     │→ /franka_gripper/joint_states (2 个手指位置)
                          └─────────────┘
   │
   │  LeRobot 格式
   ▼
~/vla_datasets/fr3_connector_direct/
   ├── meta/
   ├── data/       (HDF5 存储数值数据)
   └── videos/     (MP4 存储图像)

核心节点 1：vla_data_recorder.py — 数据录制器

订阅的 4 个话题（时间同步）

通过 message_filters.ApproximateTimeSynchronizer 做近似时间同步，slop=0.1s：

话题 类型 内容
/camera/camera_base/color/image_raw Image 固定基座相机 RGB 图像
/camera/camera_wrist/color/image_raw Image 腕部相机 RGB 图像
/franka/joint_states JointState FR3 7 个关节位置
/franka_gripper/joint_states JointState FR3 夹爪 2 个手指位置

记录的 4 个字段

1
2
3
4
5
6
features = {
    "observation.images.cam_high":  (3, 480, 640),  # 视频: 基座相机
    "observation.images.cam_wrist": (3, 480, 640),  # 视频: 腕部相机
    "observation.state":            (8,),            # float32: 7个关节 + 1个夹爪宽度
    "action":                       (8,),            # float32: 同上结构（时间平移得到）
}

8D 状态的构成（vla_data_recorder.py:65-68）：

1
2
3
arm_pos = np.array(msg_fr3_arm.position, dtype=np.float32)   # 7 维
gripper_width = gripper_pos[0] + gripper_pos[1]               # 1 维（两指位置之和）
current_8d_state = np.concatenate([arm_pos, [gripper_width]])  # 8 维

重要：这里记录的是 FR3 从端机械臂的实际关节状态（/franka/joint_states），不是 GELLO 主端的关节状态。这样做的好处是，训练数据反映的是机器人真实执行的动作，而不是遥操作给出的命令，消除了主从之间的跟踪误差。

录制触发与生命周期

从 /record_command 话题接收 String 类型命令：

1
2
空闲 ──START──→ 录制中 ──STOP──→ 保存到磁盘
               录制中 ──DISCARD─→ 丢弃（不写磁盘）
大概流程：
1. 收到 "START" → is_recording=True，清空内存缓冲区
2. sync_callback 每收到一帧同步数据 → 追加到 current_episode_buffer
3. 收到 "STOP"  → is_recording=False，调用 save_episode_to_lerobot()
4. 收到 "DISCARD" → is_recording=False，清空缓冲区，不写盘

最短录制长度：lookahead_frames + 5 = 15 帧（约 0.5 秒），更短的会被自动丢弃。

Time-Shifting：观测与动作的对应关系

这是模仿学习数据格式的关键设计。录制时：

  • 第 t 帧的 observation = 时刻 t 的相机 + 状态
  • 第 t 帧的 action = 时刻 t + lookahead_frames 的状态（未来 10 帧 ≈ 333ms 之后）
1
2
3
4
5
6
7
8
9
10
11
for t in range(num_frames):
    frame = self.current_episode_buffer[t]
    future_t = min(t + self.lookahead_frames, num_frames - 1)
    future_action = self.current_episode_buffer[future_t]["state"]  # 未来状态作为 action
    
    self.dataset.add_frame({
        "observation.images.cam_high": frame["img_base"],
        "observation.images.cam_wrist": frame["img_wrist"],
        "observation.state":          frame["state"],       # 当前
        "action":                     future_action,        # 未来
    })

这种做法的含义是：“看到当前观测，预测 333ms 后的目标关节位置”。这是模仿学习中标准的 action chunking 前置处理——模型学会了从当前状态预测未来状态，推理时再配合 action chunk 做连续执行。

存储位置与格式

1
self.repo_id = os.path.expanduser('~/vla_datasets/fr3_connector_direct')

LeRobot 数据集内部结构：

  • meta/ — 元数据（特征定义、FPS、episode 索引等）
  • data/ — 分块的数值数据（底层使用 HDF5）
  • videos/ — 图像以 MP4 视频格式存储（而非逐帧图片，节省空间）

每调用一次 dataset.save_episode() 产生一个新的 episode。多次 START/STOP 循环会在同一数据集中累积多个 episodes。

一、推理侧的对称设计 — franky_inference_client.py

  • 推理的时候使用的是绝对还是相对位置?
  • 绝对位置。推理输出的是绝对关节角度（弧度），不是增量。
  • 证据链从录制侧到推理侧一以贯之：

录制时的 action 就是绝对位置

vla_data_recorder.py:106-116：

1
2
3
4
5
6
7
8
9
for t in range(num_frames):
    frame = self.current_episode_buffer[t]
    future_t = min(t + self.lookahead_frames, num_frames - 1)
    future_action = self.current_episode_buffer[future_t]["state"]  # ← 绝对 8D 状态
    # ...
    self.dataset.add_frame({
        "observation.state": frame["state"],   # 当前绝对位置
        "action":            future_action,    # 未来绝对位置
    })

future_action 就是 333ms 后 /franka/joint_states 上读到的实际关节角度（msg_fr3_arm.position），这是 FR3 编码器输出的绝对角度值。所以模型训练时学到的映射是：

给定当前观测（图片 + 当前绝对关节角） → 预测 333ms 后的绝对关节角

推理时模型输出也是绝对位置

franky_inference_client.py:88-97：

1
2
3
4
response = self.policy.infer(obs_dict)
actions_chunk = response["actions"]
next_action = actions_chunk[10]         # 一个 8D 向量 = [7个关节绝对角度, 夹爪宽度]
scrambled_target_q = next_action[:7]   # 取前 7 个 = 绝对关节角度

没有任何增量累加、没有对当前关节角做差分、也没有 current_position + delta 的操作。target_q 直接就是模型的原始输出，然后原封不动传给运动接口。

二、拿到 action 后怎么执行？

分三步：乱序恢复 → 构造 JointMotion → 独立线程下发。

第 1 步：乱序恢复

1
2
3
4
5
6
# 模型输出是录制顺序 [J1, J3, J6, J7, J2, J4, J5]
scrambled_target_q = next_action[:7]

# 恢复到 FR3 标准顺序 [J1, J2, J3, J4, J5, J6, J7]
unscramble_indices = [0, 4, 1, 5, 6, 2, 3]
target_q = scrambled_target_q[unscramble_indices].tolist()

第 2 步：构造 JointMotion

1
motion = franky.JointMotion(target_q)

franky.JointMotion 是 franky 库（Franka 机器人 Python SDK）提供的关节空间运动原语。它的语义是："驱动所有 7 个关节以协调的方式运动到目标角度 target_q"。

这个 JointMotion 内部做的事情是：

  • 接收目标绝对关节角度（7 个 float，单位弧度）
  • 内部生成一条从当前位置到目标位置的平滑轨迹（通常是 quintic 多项式或 trapezoidal 速度曲线）
  • 由 Franka 底层控制器（libfranka）以 1kHz 的频率执行实时插补和关节级 PD+ 控制
  • 运动的实际速度和加速度受 relative_dynamics_factor = 0.1 约束（最快只有额定的 10%）

第 3 步：独立线程下发（非阻塞）

1
threading.Thread(target=self.robot.move, args=(motion,)).start()

这是整个执行设计中最重要的细节——在独立线程中执行，不阻塞推理循环。

为什么？因为 robot.move(motion) 是一个阻塞调用——它会等到 JointMotion 真正完成（所有关节到达目标位置）才返回。如果在主线程（即 5Hz 的 timer callback）里直接调用：

1
5Hz loop:  [推理 80ms] → [robot.move() 阻塞等待运动完成...] → 下一轮远超 200ms

那每次推理循环的实际周期将完全被运动时间吃掉（可能长达 500ms-1s），就不是 5Hz 了。

使用独立线程的效果：

1
2
3
4
Timer tick 0:    [推理 → 拿到 target_q_0] → 启动 thread_0 执行 move(target_q_0)
Timer tick 200ms: [推理 → 拿到 target_q_1] → 启动 thread_1 执行 move(target_q_1)
Timer tick 400ms: [推理 → 拿到 target_q_2] → 启动 thread_2 执行 move(target_q_2)
                    ↑ 此时 thread_0 可能还没跑完，但被新的 move() 覆盖/中断了

franky.Robot.move() 在被再次调用时，会取消当前正在执行的运动，开始新的运动。所以实际上，每个 200ms 到来时，上一次的 JointMotion 很可能尚未完成就被新的目标覆盖了。这意味着：

  • 机械臂跟踪的是最新推理结果，而不是完整地执行完上一个 motion
  • 实际效果上，每隔 200ms 运动目标就更新一次，机械臂平滑地"追逐"不断更新的目标
  • 配合 relative_dynamics_factor = 0.1，即使目标跳动较大，运动也是柔和的

夹爪执行也在独立线程

1
2
3
4
5
6
7
# 夹爪二值化滞后控制
if target_width < 0.04 and not self.is_gripping:
    threading.Thread(target=self.gripper.grasp, args=(0.0, 0.1, 20.0)).start()
    self.is_gripping = True
elif target_width >= 0.04 and self.is_gripping:
    threading.Thread(target=self.gripper.move, args=(0.08, 0.1)).start()
    self.is_gripping = False

同样是独立线程，不阻塞推理。is_gripping 状态变量防止重复下发相同的开/关指令。

三、完整执行时序图

1
2
3
4
5
6
7
8
9
10
11
12
13
14
Timer callback (每 200ms)
│
├─ 1. 读取最新图像（480x640 rgb8，可能与上次相同帧）
├─ 2. 通过 Franky SDK 读取当前关节角 robot.state.q（绝对角度）
├─ 3. 关节序乱序: [J1~J7 标准] → [J1,J3,J6,J7,J2,J4,J5] 录制序
├─ 4. 拼接 8D state = [7关节, 夹爪宽度]
├─ 5. 构造 obs_dict（图片 + state + prompt）
├─ 6. WebSocket 发送 → VLA 模型推理 → 接收 action chunk
├─ 7. 取 chunk[10]（第 10 个 action，绝对 8D 向量）
├─ 8. 关节序恢复: [J1,J3,J6,J7,J2,J4,J5] → [J1~J7 标准]
├─ 9. 构造 JointMotion(target_q) + 启动独立线程执行
└─ 10. 夹爪二值化判断 + 启动独立线程执行

下一次 timer callback 到来时，上一次的 JointMotion 仍在运行但会被新目标中断