ZMQ + VideoViewer: Debug Sim-to-Real G1 với PlotJuggler

Chúng ta đã đi qua bốn bài với cùng một nguồn dữ liệu: hardware G1 thật, qua ROS2. Bài cuối này mở ra một chiều debug khác — so sánh song song simulation và hardware trong cùng một PlotJuggler session.

Kịch bản: bạn train policy trên MuJoCo, deploy lên G1 thật, và robot bắt đầu ngã ở một pha gait cụ thể. Nguyên nhân là gì? Sim học được behaviour đúng nhưng transfer sai (sim2real gap)? Hay policy chưa bao giờ đúng, chỉ là sim lỏng lẻo nên không ngã?

Để trả lời, bạn cần nhìn thấy sim state và real state trên cùng một timeline. ZMQ Subscriber của PlotJuggler cộng với VideoViewer làm được điều đó.

Roadmap series

Tại sao cần so sánh sim-to-real trực tiếp?

Gap giữa simulation và thực tế là vấn đề số một của mọi robot locomotion project. Nhưng "gap" không đồng nhất — nó có thể đến từ nhiều nguồn:

Loại gap          Biểu hiện                          Cách phát hiện
─────────────────────────────────────────────────────────────────────
Dynamics gap      Sim đổ quá nhanh, real đổ chậm hơn  So sánh angular velocity
Friction gap      Real slip joint ở tốc độ cao          Tracking error diverge
Timing gap        Phase gait lệch → transfer policy sai  Cross-correlate foot contact
Sensor gap        IMU real nhiễu hơn sim nhiều lần      FFT noise floor khác biệt
Actuator gap      Torque real thấp hơn sim model        Power curve lệch

Nhìn vào list này, rõ ràng bạn cần so sánh định lượng, không phải "thử deploy rồi xem có ngã không". PlotJuggler với ZMQ + VideoViewer cho phép bạn nhìn thấy gap từng kênh, từng millisecond.

Kiến trúc tổng thể

┌─────────────────────────┐      ZMQ PUB      ┌──────────────────────┐
│  MuJoCo Simulation      │ ─────────────────▶ │                      │
│  (Python)               │  JSON @ 200 Hz     │  PlotJuggler         │
│  ─ G1 model (MJCF)      │                    │                      │
│  ─ Policy runner        │                    │  ZMQ Subscriber      │
│  ─ zmq_publisher.py     │                    │  ├── sim/* channels  │
└─────────────────────────┘                    │                      │
                                               │  ROS2 Subscriber     │
┌─────────────────────────┐      ROS2          │  ├── /lowstate/*     │
│  G1 Robot (hardware)    │ ─────────────────▶ │                      │
│  ─ SDK real-time        │  /lowstate @ 50Hz  │  VideoViewer         │
│  ─ ROS2 bridge          │                    │  ├── camera_real.mp4 │
└─────────────────────────┘                    │  └── camera_sim.mp4  │
                                               └──────────────────────┘

Hai nguồn data chạy song song: ROS2 topic từ G1 hardware (đã thiết lập từ bài 1), và ZMQ stream từ MuJoCo Python script. PlotJuggler nhận cả hai, hiển thị trên cùng timeline với timestamp đồng bộ.

Phần 1: ZMQ Publisher trong Python/MuJoCo

Cài thư viện:

pip install pyzmq mujoco

Script zmq_publisher.py — chạy song song với simulation loop:

import zmq
import json
import time
import numpy as np
import mujoco
import mujoco.viewer

# ── ZMQ Setup ────────────────────────────────────────────────────────
context = zmq.Context()
socket  = context.socket(zmq.PUB)
socket.bind("tcp://127.0.0.1:9872")  # PlotJuggler kết nối vào port này

# ── MuJoCo Setup ─────────────────────────────────────────────────────
model = mujoco.MjModel.from_xml_path("unitree_g1/scene.mjcf")
data  = mujoco.MjData(model)

JOINT_NAMES = [
    "left_hip_yaw",   "left_hip_roll",   "left_hip_pitch",
    "left_knee",      "left_ankle_pitch", "left_ankle_roll",
    "right_hip_yaw",  "right_hip_roll",  "right_hip_pitch",
    "right_knee",     "right_ankle_pitch","right_ankle_roll",
    # ... 23 khớp đầy đủ
]

PUBLISH_RATE = 200   # Hz — cao hơn ROS2 bridge (50Hz) để có đủ resolution
dt_publish   = 1.0 / PUBLISH_RATE
t_last_pub   = 0.0

def get_joint_index(name: str) -> int:
    return mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_JOINT, name)

# ── Simulation Loop ───────────────────────────────────────────────────
with mujoco.viewer.launch_passive(model, data) as viewer:
    while viewer.is_running():
        mujoco.mj_step(model, data)

        t_now = data.time
        if t_now - t_last_pub >= dt_publish:
            t_last_pub = t_now

            # Build payload — cấu trúc JSON PlotJuggler hiểu
            # Key format: "timestamp" bắt buộc, còn lại là tên kênh tuỳ ý
            payload = {"timestamp": t_now}

            for i, name in enumerate(JOINT_NAMES):
                jid = get_joint_index(name)
                payload[f"sim/motor_state/{i}/q"]   = float(data.qpos[jid])
                payload[f"sim/motor_state/{i}/dq"]  = float(data.qvel[jid])
                payload[f"sim/motor_state/{i}/tau"]  = float(data.actuator_force[i])

            # IMU từ body "imu_in_pelvis" (check MJCF của bạn)
            imu_body_id = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_BODY, "pelvis")
            payload["sim/imu/quaternion/w"] = float(data.xquat[imu_body_id, 0])
            payload["sim/imu/quaternion/x"] = float(data.xquat[imu_body_id, 1])
            payload["sim/imu/quaternion/y"] = float(data.xquat[imu_body_id, 2])
            payload["sim/imu/quaternion/z"] = float(data.xquat[imu_body_id, 3])

            # Send — PlotJuggler dùng topic prefix (empty string = default topic)
            socket.send_string("" + "\n" + json.dumps(payload))

        viewer.sync()

Điểm quan trọng về format JSON: PlotJuggler ZMQ Subscriber yêu cầu message gồm hai phần cách nhau bởi \n: (1) tên topic (string, có thể rỗng), (2) JSON payload với key "timestamp" bắt buộc. Nếu thiếu "timestamp", PlotJuggler dùng wall clock — gây lệch khi replay.

Phần 2: ZMQ Subscriber trong PlotJuggler

Khởi động ZMQ Publisher trên MuJoCo trước, sau đó:

PlotJuggler → Streaming → Start Streaming → chọn "ZMQ Subscriber"

Cửa sổ cấu hình:

Address:  tcp://127.0.0.1:9872   ← khớp với bind() trong Python
Topic:    (để trống)
Parser:   JSON
Timeout:  500 ms

Nhấn OK. Trong vài giây, data tree bên trái sẽ xuất hiện toàn bộ cây sim/*:

sim/
  motor_state/
    0/q        ← position khớp 0 từ sim
    0/dq       ← velocity khớp 0 từ sim
    0/tau      ← torque khớp 0 từ sim
    1/q
    ...
  imu/
    quaternion/w
    quaternion/x
    ...

Đồng thời, ROS2 stream đã chạy từ bài 1 vẫn tiếp tục cho /lowstate/* từ hardware.

Kéo vào panel để so sánh:

• Left panel: sim/motor_state/10/dq + /lowstate/motor_state[10]/dq
• Right panel: sim/imu/quaternion/w + /lowstate/imu_state/quaternion[0]

Hai đường phải chồng lên nhau nếu sim2real perfect — khoảng cách giữa chúng chính là gap bạn cần debug.

Phần 3: VideoViewer — đồng bộ camera feed

PlotJuggler có plugin VideoViewer (bundled từ 3.6+) cho phép phát video .mp4 đồng bộ với data timeline.

Chuẩn bị video

Camera hardware G1: Dùng ROS2 để record:

ros2 bag record /camera/color/image_raw/compressed -o g1_real_session.bag
# Sau đó export ra .mp4:
ros2 run image_transport republish compressed raw \
  --ros-args --remap in/compressed:=/camera/color/image_raw/compressed \
             --remap out:=/camera_raw
ffmpeg -i <ros_bag_video_topic_extracted>.mp4 camera_real.mp4

Camera từ MuJoCo: Render offscreen và save:

# Trong simulation loop, thêm renderer
renderer = mujoco.Renderer(model, height=480, width=640)

# Mỗi N steps, render và save frame
renderer.update_scene(data, camera="front_camera")
frame = renderer.render()   # numpy array RGB
# Ghi vào video bằng imageio hoặc cv2.VideoWriter

Load vào PlotJuggler

Tools → Video Viewer → Open Video File
→ Chọn camera_real.mp4
→ Chọn "Sync with active cursor" (bật đồng bộ cursor)

Bây giờ khi bạn kéo timeline cursor trong PlotJuggler → video tự nhảy đến frame tương ứng. Nếu video bắt đầu từ thời điểm khác với data, dùng offset:

Video Viewer → Time offset: -2.34 s   ← offset để sync

Cách tính offset: tìm một event đặc biệt trong data (ví dụ: peak IMU spike khi G1 bước bộ đầu tiên), tìm frame tương ứng trong video, tính delta timestamp.

Phần 4: Case study — Phát hiện phase error sim2real

Kịch bản

G1 chạy policy locomotion học từ MuJoCo. Trên sim ổn định, nhưng trên hardware robot bắt đầu dao động từ bước thứ 8–10.

Quy trình debug

Bước 1: Chạy song song — MuJoCo + ZMQ publisher, G1 hardware + ROS2 bridge. Bắt đầu cùng lúc (hoặc với offset đã tính).

Bước 2: Mở 4 panel trong PlotJuggler:

Panel A (top-left):  sim/motor_state/10/dq  vs  /lowstate/motor_state[10]/dq
                     (velocity khớp hông trái: xanh = sim, đỏ = real)

Panel B (top-right): sim/motor_state/11/dq  vs  /lowstate/motor_state[11]/dq
                     (velocity khớp gối trái)

Panel C (bottom-left): sim/imu/quaternion (pitch component via ToolboxQuaternion)
                        vs /lowstate/imu_state/rpy[1]  (pitch real)

Panel D (bottom-right): VideoViewer camera (hardware)

Bước 3: Phân tích kết quả:

Quan sát tại bước thứ 9 (t ≈ 4.5s):

hip_left dq — sim: +1.2 rad/s, real: +0.85 rad/s
  → Real velocity thấp hơn 29% → friction cao hơn sim model

knee_left dq — sim: -2.1 rad/s, real: -1.4 rad/s
  → Phase DELAY: real đạt peak trễ hơn sim 42ms

pitch IMU — sim: 3.2°, real: 4.8°
  → Real lean forward nhiều hơn sim → center of mass model sai

Video (t=4.5s): G1 nghiêng rõ về phía trước — confirm với pitch IMU

Kết luận: Vấn đề kép — friction model thấp hơn thực tế (gây velocity lag), và COM (center of mass) model sai (gây pitch divergence). Cần:

1. System identification friction từ data tracking_error (bài 4)
2. Đo lại khối lượng + inertia G1 thực tế để cập nhật MJCF

Phân tích cross-correlation

Dùng Lua Custom Function từ bài 4 để tính phase lag giữa sim và real. Vì mỗi Custom Function chỉ xuất 1 kênh, tạo 2 functions riêng: một cho correlation tại lag 0, một tại lag 5.

Function xcorr_lag0 (tương quan tại độ trễ 0):

main input: sim/motor_state/10/dq   (từ ZMQ)
additional: /lowstate/motor_state[10]/dq  (từ ROS2)
output: "xcorr_lag0"

Global Code:

WINDOW   = 100   -- 100 samples ≈ 2 giây ở 50Hz
sim_buf  = {}
real_buf = {}
buf_idx  = 0
for k = 1, WINDOW do sim_buf[k] = 0; real_buf[k] = 0 end

Function Code (lag 0):

-- value = sim_vel, v1 = real_vel
buf_idx = (buf_idx % WINDOW) + 1   -- circular buffer 1-indexed trong Lua
sim_buf[buf_idx]  = value
real_buf[buf_idx] = v1

local cc = 0
for k = 1, WINDOW do
  cc = cc + sim_buf[k] * real_buf[k]
end
return cc / WINDOW

Tạo function thứ hai xcorr_lag5 (sao chép Global Code), Function Code dùng offset 5 trong real_buf:

local cc = 0
for k = 1, WINDOW do
  local k5 = ((k + 4) % WINDOW) + 1   -- dịch 5 samples
  cc = cc + sim_buf[k] * real_buf[k5]
end
return cc / WINDOW

Nếu xcorr_lag5 > xcorr_lag0 → real signal trễ hơn sim 5 samples (100ms tại 50Hz) → policy cần compensate.

Checklist debug sim2real với PlotJuggler

Mỗi lần transfer policy từ sim sang hardware, chạy qua checklist này:

□ 1. Kiểm tra dt_sim vs dt_real
     sim/imu/timestamp delta ≈ 5ms (200Hz)?
     /lowstate timestamp delta ≈ 20ms (50Hz)?
     Mismatch → interpolation sai trong policy

□ 2. So sánh q_init (joint position lúc t=0)
     sim/motor_state/*/q[t=0] ≈ /lowstate/motor_state[*]/q[t=0]?
     Nếu không → home position khác nhau → offset cần bù

□ 3. IMU noise floor (FFT — dùng ToolboxFFT từ bài 3)
     Real gyro noise floor thường cao hơn sim 5–10x
     Nếu policy nhạy cảm với gyro high-freq → thêm lowpass filter

□ 4. Tracking error divergence (dùng script từ bài 4)
     tracking_error tăng > 0.3 tại khớp nào?
     → Friction/inertia mismatch cần system ID

□ 5. Phase portrait shape
     Overlay phase portrait (q vs dq) sim vs real (bài 2)
     Shape khác nhau → dynamics model không match

□ 6. Video alignment
     Bước chân thứ nhất trong video ≈ foot_force peak trong data?
     Nếu lệch > 50ms → sync offset sai

Tổng kết series

Qua 5 bài, chúng ta đã xây dựng một debugging stack hoàn chỉnh cho G1:

Workflow này scale được: dù bạn đang debug G1 đi thẳng, leo cầu thang, hay thực hiện whole-body manipulation — PlotJuggler + ZMQ + VideoViewer cho phép bạn nhìn thấy chính xác điều gì đang xảy ra và ở layer nào.

Bước tiếp theo từ đây: đưa gap đo được vào system identification (SysID) để cập nhật simulation model, và tạo feedback loop tự động cập nhật sim model từ hardware data. Đó là con đường tới sim2real mà không cần domain randomization quy mô lớn.

Bài viết liên quan

• Lua Transforms: Tính power per joint và tracking error cho G1 — Reactive Script tính công suất cơ học, velocity error từ /lowstate
• Debug IMU G1: Quaternion → Euler + FFT phân tích rung — Phân tích tín hiệu IMU, FFT phát hiện resonance, Moving Average foot contact
• PlotJuggler + G1: Cài đặt ROS2 và kết nối live — Setup ban đầu: CycloneDDS, ROS2 bridge, stream /lowstate realtime