宇树 G1 部署（十一）——遥操作脚本升级 teleop_hand_and_arm_update.py

遥操作脚本数据采集属实有点难用，简单升级一版

基本遥操作指令：

python teleop_hand_and_arm_update.py --xr-mode=hand --arm=G1_29 --ee=inspire1 --record

目录

[1 原版控制逻辑](#1 原版控制逻辑)

[2 升级逻辑](#2 升级逻辑)

[2.1 引入事件与标志](#2.1 引入事件与标志)

[2.2 修改按键回调（更明显的日志）](#2.2 修改按键回调（更明显的日志）)

[2.3 主循环：优先处理"开始/请求停录"](#2.3 主循环：优先处理“开始/请求停录”)

[2.4 在 add_item() 后"完成存储再停止"](#2.4 在 add_item() 后“完成存储再停止”)

[2.5 叠加明显的屏幕标识（非 headless）](#2.5 叠加明显的屏幕标识（非 headless）)

[3 整合版](#3 整合版)

---

1 原版控制逻辑

首先其原版控制逻辑：

1. 等待 r：收到后 arm_ctrl.speed_gradual_max()（逐步放开速度限制，避免瞬时大速度）

2. UI 与热键（非 headless）：

• 显示缩放后的 tv_img_array
• q：退出（仿真里同时发布 reset_category=2，可定义为回到初始/停止）
• s：切换录制
• a：仿真时发布 reset_category=2（例如重置场景）

3. 录制切换：

• 开启：recorder.create_episode() 成功则进入录制态
• 关闭：recorder.save_episode() 并在仿真中发布 reset_category=1（如切换到"下一条"）

4. 读取 XR 数据：tele_data = tv_wrapper.get_motion_state_data()

• 当 xr-mode=hand 且末端为手型（dex3/inspire1）：把 tele_data.left/right_hand_pos（75 维）拷进共享数组（已加锁）
• 当 gripper： xr-mode=controller → 取左右触发值； xr-mode=hand → 取左右 pinch 捏合度

5. 底盘（控制器+motion）：

• right_aButton 退出
• 左右摇杆同时按下触发 Damp()（软急停）
• Move(vx, vy, w) 映射自左右摇杆（做了 0.3 限速与方向取负）

6. 读当前关节状态：get_current_dual_arm_q/dq()

7. IK 求解与下发：

• arm_ik.solve_ik( left_arm_pose, right_arm_pose, q, dq ) → (sol_q, sol_tauff)
• arm_ctrl.ctrl_dual_arm(sol_q, sol_tauff)
• debug 打印 IK 计算耗时

8. 录制帧（若在录制态）：

• 复制头/腕图像快照；切片成左右半幅
• states：臂/末端/（可选）body
• actions：臂（目标 sol_q）、末端（来源于 XR 的当前输入编码）、body（当手柄运动时）
• 仿真时追加 sim_state；调用 recorder.add_item(...)

9. 频率控制：按 --frequency 计算 sleep_time = max(0, 1/f - cost) 并 time.sleep；debug 打印本周期 sleep

可以看出：按下 s 并不会立刻读写文件，而是把一个"切换请求"标志位 should_toggle_recording=True 交给主循环；主循环在下一帧处理这个请求，将 is_recording 在 开始录制 与 停止并保存 两个状态之间切换

2 升级逻辑

核心需求：

• 按下 s 时，当前这一帧的数据先完整写入，然后立刻保存并停止

• 同时增加更明显的日志/屏幕标识（print / logger + 画面 OSD）

因此基本思路为：把"停止录制"改成"本帧写完即停"，用一个 pending_stop 标志在本帧 add_item() 之后触发 save_episode()；并引入更醒目的日志与画面叠加

• 按 s 开始：日志会出现 [REC] START requested → STARTED OK；OSD 左上角红点、帧计数递增
• 按 s 停止：日志出现 [REC] STOP requested: will save AFTER this frame，OSD变成橙点"stopping after this frame..."；本帧写完马上日志打印 [REC] STOPPED. frames=...，OSD转为绿点"REC OFF"
• 磁盘慢时 save_episode() 会打印耗时，便于感知"为什么看起来还在忙"

2.1 引入事件与标志

from threading import Event

# 事件/标志：用事件代替 should_toggle_recording 的布尔
toggle_evt = Event()       # 按 s 置位；主循环消耗后清零
pending_stop = False       # 当录制中收到 s，将在"本帧 add_item() 后保存并停"
frame_in_episode = 0       # 当前 episode 已写入的帧数（仅用于日志/OSD）

2.2 修改按键回调（更明显的日志）

def on_press(key):
    global running
    if key == 'r':
        logger_mp.info("[KEY] r → start signal")
        globals()['start_signal'] = True
    elif key == 'q':
        logger_mp.info("[KEY] q → quit")
        stop_listening()
        running = False
    elif key == 's':
        logger_mp.info("[KEY] s → toggle recording requested")
        toggle_evt.set()
    else:
        logger_mp.info(f"[KEY] {key} pressed (no action)")

2.3 主循环：优先处理"开始/请求停录"

# 处理 s 事件：开始或请求"本帧写完即停"
if args.record and toggle_evt.is_set():
    toggle_evt.clear()
    if not is_recording:
        logger_mp.info("[REC] START requested")
        if recorder.create_episode():
            is_recording = True
            frame_in_episode = 0
            logger_mp.info("[REC] STARTED OK")
        else:
            logger_mp.error("[REC] START FAILED: create_episode() returned False")
    else:
        # 录制中 → 标记为"本帧写完即停"
        pending_stop = True
        logger_mp.info("[REC] STOP requested: will save AFTER this frame")

2.4 在 add_item() 后"完成存储再停止"

if is_recording:
    frame_in_episode += 1

    if pending_stop:
        # 本帧数据已写入，立刻保存并停止
        pending_stop = False
        is_recording = False
        try:
            t0 = time.time()
            recorder.save_episode()
            dt = (time.time() - t0) * 1000
            logger_mp.info(f"[REC] STOPPED. frames={frame_in_episode}, save_episode() took {dt:.1f} ms")
        except Exception as e:
            logger_mp.exception("[REC] save_episode() failed")
        frame_in_episode = 0  # 重置计数
        if args.sim:
            publish_reset_category(1, reset_pose_publisher)

2.5 叠加明显的屏幕标识（非 headless）

在生成 tv_resized_image 之后、imshow 之前，叠加一个显眼的录制状态条（红点/帧计数）：

if not args.headless:
    overlay = tv_resized_image
    # 半透明顶栏
    bar_h = 36
    cv2.rectangle(overlay, (0,0), (overlay.shape[1], bar_h), (0,0,0), -1)
    alpha = 0.5
    tv_resized_image[:bar_h] = (alpha*overlay[:bar_h] + (1-alpha)*tv_resized_image[:bar_h]).astype(tv_resized_image.dtype)

    # 文案
    if is_recording:
        # 红点
        cv2.circle(tv_resized_image, (18, 18), 8, (0,0,255), -1)
        status = f"REC  frames={frame_in_episode}"
    elif pending_stop:
        # 橙点：本帧写完即停
        cv2.circle(tv_resized_image, (18, 18), 8, (0,165,255), -1)
        status = "REC  stopping after this frame..."
    else:
        # 绿点：未录制
        cv2.circle(tv_resized_image, (18, 18), 8, (0,255,0), -1)
        status = "REC OFF"

    cv2.putText(tv_resized_image, status, (40, 24),
                cv2.FONT_HERSHEY_SIMPLEX, 0.7, (255,255,255), 2, cv2.LINE_AA)

3 整合版

https://github.com/YemuRiven/unitree-g1-scripts/tree/main/xr

变更要点：

• 新增 toggle_evt / pending_stop / frame_in_episode
• s 键触发通过 toggle_evt 事件传递；录制中收到 s → 设置 pending_stop=True，在本帧 add_item() 之后 save_episode() 并停止
• 非 headless 模式叠加了清晰的 OSD（红=录制、橙=本帧写完即停、绿=未录制），并打印更醒目的日志
• finally 里做了兜底保存，防止异常退出导致未归档

效果不错，但是在 pc2 上使用确实存储太慢了，遥操作 5min 存储 10min......

所以建议上位机部署遥操作