Checklist sim2real cho controller G1

Vì sao bài cuối là checklist?

Sau năm bài đầu, ta đã có đủ từng mảnh của pipeline G1: dựng mô phỏng low-level DDS trong bài 1, đóng gói log thành MCAP để xem bằng Foxglove trong bài 2, soi tín hiệu bằng PlotJuggler trong bài 3, hiểu vì sao controller chân cần contact-aware control trong bài 4, rồi nối upper body IK và VR trong bài 5. Bài cuối không thêm thuật toán mới. Bài này biến toàn bộ series thành một checklist triển khai để bạn biết lúc nào còn ở MuJoCo, lúc nào đang chạm vào robot thật, dữ liệu nào cần log, và ngưỡng nào phải qua trước khi cho controller chạy trên hardware.

Nguồn kỹ thuật chính của checklist là repo unitreerobotics/unitree_mujoco và repo GalaxyGeneralRobotics/OpenWBT. unitree_mujoco mô tả rõ cách dùng cùng một ví dụ stand_go2 qua ba interface example/cpp, example/python, example/ros2 để đi từ sim sang real. OpenWBT là ví dụ thực dụng hơn cho G1: config run_teleoperation.yaml dùng msg_type: "hg", lowcmd_topic: "rt/lowcmd", lowstate_topic: "rt/lowstate", hàm create_damping_cmd() để đưa robot về damping, logging save_rawdata() ra .pkl, và guard abs(tau_record).max() > 100 trước khi publish lệnh.

Nguyên tắc xuyên suốt rất đơn giản: không có log thì chưa có sim2real. Nếu chỉ thấy robot mô phỏng đứng lên đẹp, nhưng chưa so sánh được qj, dqj, gravity_orientation, target_dof_pos và tau, bạn chưa sẵn sàng. Nếu đã log được nhưng torque guard còn chạm ngưỡng, bạn cũng chưa sẵn sàng. Hardware là bước cuối của pipeline, không phải nơi để debug lần đầu.

Roadmap series

Series G1 MuJoCo: điều khiển, Foxglove và PlotJuggler gồm sáu bài:

Nếu bạn mới với MuJoCo nói chung, đọc thêm Bắt đầu với MuJoCo. Nếu bạn muốn đặt G1 trong bức tranh software stack rộng hơn, xem Humanoid robot software stack: ROS 2, Isaac và LeRobot.

Checklist 1: biết rõ mình đang dùng interface nào

Trong unitree_mujoco, thư mục example có ba đường triển khai:

unitree_mujoco/
  example/
    cpp/      # unitree_sdk2
    python/   # unitree_sdk2_python
    ros2/     # unitree_ros2

Ví dụ trong repo là stand_go2: cho robot đứng lên rồi nằm xuống. Dù tên là Go2, bài học sim-to-real của nó rất quan trọng cho G1 vì nó chỉ ra cách tổ chức interface. Với C++ và Python, cùng một binary hoặc script có hai chế độ: không truyền network interface thì nói chuyện với mô phỏng trên loopback; truyền tên card mạng thì nói chuyện với robot thật. Với ROS 2, khác biệt nằm ở file setup network và ROS_DOMAIN_ID.

Checklist beginner nên viết ra giấy:

Điểm hay của thiết kế này là controller không cần đổi logic điều khiển ở giữa. Cái đổi là transport: domain DDS, interface mạng và topic. Nếu bạn đang debug thuật toán, hãy giữ thuật toán yên và chỉ thay một lớp transport mỗi lần. Nếu vừa đổi gain, vừa đổi domain, vừa đổi robot model, vừa đổi network interface, bạn sẽ không biết lỗi đến từ đâu.

Checklist 2: domain DDS và network interface

Trong mô phỏng Unitree MuJoCo, README khuyến nghị tách DDS domain của sim khỏi robot thật. Với C++:

if (argc < 2)
{
    ChannelFactory::Instance()->Init(1, "lo");
}
else
{
    ChannelFactory::Instance()->Init(0, argv[1]);
}

Với Python:

if len(sys.argv) < 2:
    ChannelFactoryInitialize(1, "lo")
else:
    ChannelFactoryInitialize(0, sys.argv[1])

Đọc đoạn này theo ngôn ngữ vận hành:

Không nên xem 1 và 0 là con số ngẫu nhiên. Robot thật dùng domain mặc định 0. Mô phỏng dùng domain 1 để tránh trường hợp một controller thử nghiệm vô tình publish LowCmd vào robot thật đang ở cùng mạng. "lo" là loopback local; nó làm cho traffic DDS của sim ở trong máy. Khi chuyển sang hardware, bạn phải truyền đúng NIC, ví dụ eno1, enp3s0 hoặc tên interface Ethernet thật trên máy của bạn.

Với ROS 2, checklist tương đương:

# Simulation
source ~/unitree_ros2/setup_local.sh
export ROS_DOMAIN_ID=1
./install/stand_go2/bin/stand_go2

# Real robot
source ~/unitree_ros2/setup.sh
export ROS_DOMAIN_ID=0
./install/stand_go2/bin/stand_go2

Sai lầm thường gặp là chạy setup.sh real nhưng vẫn để ROS_DOMAIN_ID=1, hoặc chạy setup_local.sh rồi tự hỏi vì sao robot thật không nhận lệnh. Một lỗi khác nguy hiểm hơn là để môi trường shell cũ còn ROS_DOMAIN_ID=0 trong lúc bạn nghĩ mình đang test mô phỏng. Vì vậy trước mỗi phiên chạy, in ra ba thứ: tên script đang chạy, ROS_DOMAIN_ID, và network interface. Nếu dùng Python hoặc C++, hãy log rõ branch ChannelFactoryInitialize(1, "lo") hay ChannelFactoryInitialize(0, args.net).

Checklist 3: OpenWBT phải dùng đúng topic và message type cho G1

OpenWBT không chỉ là demo VR. Nó là một checklist mẫu cho cách tổ chức real deployment. Trong deploy/configs/run_teleoperation.yaml, các dòng quan trọng là:

control_dt: 0.02
control_decimation: 4

msg_type: "hg"
imu_type: "pelvis"

lowcmd_topic: "rt/lowcmd"
lowstate_topic: "rt/lowstate"

Với G1, msg_type: "hg" là điểm rất đáng nhớ. Unitree MuJoCo cũng ghi rõ G1 và H1-2 dùng unitree_hg IDL cho low-level communication, trong khi các robot như Go2, B2, H1 cũ dùng unitree_go. Nếu bạn copy ví dụ Go2 sang G1 mà quên đổi message type, subscriber/publisher có thể vẫn khởi tạo được ở một số tầng, nhưng dữ liệu motor không đúng schema sẽ làm phần còn lại vô nghĩa.

Hai topic rt/lowcmd và rt/lowstate là cặp vào/ra của low-level controller:

Trong controller.py, OpenWBT tạo ChannelPublisher(self.config.lowcmd_topic, LowCmdHG) và ChannelSubscriber(self.config.lowstate_topic, LowStateHG) khi msg_type == "hg". Sau đó wait_for_low_state() chờ low_state.tick khác 0 trước khi báo đã kết nối. Đây là một guard rất thực tế: chưa có LowState thì chưa gửi command.

Checklist trước khi chạy OpenWBT real:

1. Robot đã ở damping mode bằng remote Unitree.
2. Máy tính nối đúng Ethernet với robot.
3. --net là tên NIC thật, ví dụ --net eno1.
4. Config đang dùng msg_type: "hg" cho G1.
5. Topic là rt/lowcmd và rt/lowstate, không đổi tùy tiện.
6. Chạy --debug trong lần đầu nếu muốn logic vẫn chạy nhưng command bị thay bằng damping.

Lệnh tương ứng:

source /opt/ros/foxy/setup.sh
source ~/unitree_ros2/setup.sh
python -m deploy.run_teleoperation_real --config run_teleoperation.yaml --net eno1 --debug

Khi vẫn ở MuJoCo:

source /opt/ros/foxy/setup.sh
source ~/unitree_ros2/setup.sh
python -m deploy.run_teleoperation_mujoco --config run_teleoperation.yaml --save_data

Checklist 4: damping không phải nút trang trí

Trong OpenWBT, create_damping_cmd() đặt toàn bộ motor command về:

cmd.motor_cmd[i].q = 0
cmd.motor_cmd[i].dq = 0
cmd.motor_cmd[i].kp = 0
cmd.motor_cmd[i].kd = 8
cmd.motor_cmd[i].tau = 0

Nghĩa là controller không còn cố kéo joint tới một target position cụ thể. Nó chỉ tạo damping theo velocity. Đây là trạng thái an toàn hơn khi robot cần dừng, khi operator chưa start, hoặc khi torque guard phát hiện lệnh bất thường. Trong damping_state(), OpenWBT liên tục publish damping command cho đến khi joystick bên phải phát tín hiệu start. Trong run_loco() và run_squat(), nếu debug=True, code cũng gọi create_damping_cmd(self.low_cmd) trước khi gửi. Vì vậy --debug không phải là print verbose; nó là chế độ không execute policy torque lên robot.

Beginner nên tách ba trạng thái:

Một controller sim2real tốt không nhảy thẳng từ boot vào policy. Nó đi qua damping, rồi default pose, rồi mới policy. Đây là lý do deploy_real() trong OpenWBT gọi runner.damping_state() trước, sau đó runner.move_to_default_pos(), rồi mới vào vòng SQUAT/LOCOMOTION.

Checklist 5: log .pkl bằng save_rawdata()

OpenWBT có hàm save_rawdata() trong cả runner real và runner MuJoCo. Khi bật --save_data, nó tạo thư mục theo args.save_data_dir/config.exp_name và ghi từng frame:

rawdata = {
    "proprioception": {
        "qj": self.qj,
        "dqj": self.dqj,
        "gravity_orientation": get_gravity_orientation(self.quat),
        "ang_vel": self.ang_vel,
    },
    "squat_cmd": self.squat_controller.squat_cmd,
    "loco_cmd": self.loco_controller.loco_cmd,
    "target_dof_pos": self.target_dof_pos,
    "tau": self.tau_record,
}
pickle.dump(rawdata, open(filepath, "wb"))
time.sleep(0.02)

Tên file có dạng 00000001.pkl, 00000002.pkl, ... theo counter. Với time.sleep(0.02), nhịp log xấp xỉ 50 Hz, khớp control_dt: 0.02. Trong MuJoCo, run_squat() và run_loco() chỉ update policy theo control_decimation; physics có thể chạy nhanh hơn, nhưng log điều khiển vẫn nên được hiểu theo nhịp controller.

Các tín hiệu tối thiểu cần giữ lại cho PlotJuggler:

Nếu bạn muốn đưa .pkl vào PlotJuggler nhanh, cách đơn giản là convert sang CSV. Ví dụ:

import glob
import pickle
import pandas as pd

rows = []
for path in sorted(glob.glob("save_mujoco_data/run_teleoperation/*.pkl")):
    with open(path, "rb") as f:
        d = pickle.load(f)
    row = {}
    for i, v in enumerate(d["proprioception"]["qj"]):
        row[f"qj/{i}"] = float(v)
    for i, v in enumerate(d["proprioception"]["dqj"]):
        row[f"dqj/{i}"] = float(v)
    for i, v in enumerate(d["proprioception"]["gravity_orientation"]):
        row[f"gravity_orientation/{i}"] = float(v)
    for i, v in enumerate(d["target_dof_pos"]):
        row[f"target_dof_pos/{i}"] = float(v)
    for i, v in enumerate(d["tau"]):
        row[f"tau/{i}"] = float(v)
    rows.append(row)

pd.DataFrame(rows).to_csv("g1_openwbt_log.csv", index_label="sample")

Trong PlotJuggler, hãy đặt qj/<joint> và target_dof_pos/<joint> cùng một panel. qj nên bám target có độ trễ hợp lý, không có bước nhảy vô lý. Đặt dqj/<joint> panel riêng để thấy rung. Đặt tau/<joint> panel riêng, vì torque spike thường là dấu hiệu đầu tiên của gain sai, target nhảy, joint index lệch, hoặc frame IK bị đổi sai.

Checklist 6: so sánh sim và real theo cùng layout PlotJuggler

Đừng mở log sim một kiểu, log real một kiểu. Hãy tạo một layout PlotJuggler cố định:

Với G1 29DoF, nhóm joint thường được đọc như sau: 0-11 là chân, 12-14 là waist, 15-21 là tay trái, 22-28 là tay phải. OpenWBT dùng action_hl_idx cho hai tay:

action_hl_idx:
  [15, 16, 17, 18, 19, 20, 21,
   22, 23, 24, 25, 26, 27, 28]

Trong bài 5, ta đã thấy IK VR chỉ gán vào nhóm này sau khi clip sol_q quanh target hiện tại ±0.01 rad. Ở bài này, bạn cần kiểm tra hậu quả của clip đó trong log. Nếu target_dof_pos/15..28 mượt nhưng qj/15..28 rung mạnh, vấn đề có thể ở gain hoặc dynamics. Nếu target_dof_pos đã giật, vấn đề ở IK, VR tracking, transform frame hoặc rate limiter.

Khi so sánh sim và real, đừng kỳ vọng đường real giống sim tuyệt đối. Điều bạn cần là cùng xu hướng và không có bất thường nguy hiểm: không đổi dấu ngoài ý muốn, không spike torque lặp lại, không joint bám ngược target, không base orientation trôi vô lý khi robot đứng yên. Nếu một joint có torque lớn trong sim, trên real nó thường không tự nhiên biến mất. Vì vậy sim phải sạch trước.

Checklist 7: torque guard là cổng cuối

Trong runner real, OpenWBT tính tau_record bằng PD:

self.tau_record[motor_idx] = (
    (target_pos[j] - self.low_state.motor_state[j].q) * controller.kps[j]
    - self.low_state.motor_state[j].dq * controller.kds[j]
)

Trước khi publish command, send_cmd() kiểm tra:

if abs(self.tau_record).max() > 100:
    print("large tau: ", np.arange(29)[abs(self.tau_record) > 100])
    create_damping_cmd(self.low_cmd)
    cmd = self.low_cmd

Đây là checkpoint quan trọng nhất của bài. Nếu abs(tau_record).max() > 100 bị kích hoạt, controller không được xem là pass. Nó đang yêu cầu ít nhất một motor tạo torque quá lớn theo công thức PD hiện tại. Nguyên nhân có thể là target quá xa current position, joint index sai, default pose sai, gain quá cao, velocity quá lớn, hoặc state feedback bị trễ. Dù nguyên nhân nào, phản ứng đúng không phải là tăng threshold. Phản ứng đúng là quay lại log, tìm joint bị in ra trong "large tau", rồi xem qj, dqj, target_dof_pos, tau của joint đó.

Một quy trình pass/fail thực dụng:

1. Chạy MuJoCo với --save_data, không robot thật.
2. Convert .pkl sang CSV hoặc format PlotJuggler đọc được.
3. Mở layout cố định và kiểm tra tracking.
4. Tính max(abs(tau)) theo từng file và toàn bộ phiên chạy.
5. Nếu có mẫu vượt 100, sửa trong sim trước.
6. Khi sim pass, chạy real với --debug để xác nhận network, topic và state.
7. Chỉ bỏ --debug khi damping, default pose, log và torque đều ổn.

Đoạn kiểm tra nhanh:

import glob
import pickle
import numpy as np

max_tau = 0.0
worst = None
for path in sorted(glob.glob("save_mujoco_data/run_teleoperation/*.pkl")):
    with open(path, "rb") as f:
        d = pickle.load(f)
    tau = np.asarray(d["tau"])
    value = float(np.abs(tau).max())
    if value > max_tau:
        max_tau = value
        worst = (path, np.where(np.abs(tau) > 100)[0].tolist())

print("max abs(tau):", max_tau)
print("samples over 100:", worst)

Nếu worst có joint index, quay lại bảng joint. Với G1, index 15-28 là arm/upper body, nên lỗi ở đây thường liên quan IK hoặc target tay. Index 0-11 là chân, thường liên quan squat/loco policy, command joystick, default pose hoặc contact trong mô phỏng.

Checklist triển khai một phiên hoàn chỉnh

Đây là phiên làm việc mà tôi khuyên dùng cho beginner:

# 1. Chạy MuJoCo, domain 1, loopback
cd unitree_mujoco/simulate
./build/unitree_mujoco -r go2 -s scene_terrain.xml

# 2. Chạy controller ví dụ qua SDK2/Python hoặc ROS 2 ở domain 1
cd ../example/python
python3 ./stand_go2.py

# 3. Chạy OpenWBT MuJoCo và log raw data
cd /path/to/OpenWBT
source /opt/ros/foxy/setup.sh
source ~/unitree_ros2/setup.sh
python -m deploy.run_teleoperation_mujoco --config run_teleoperation.yaml --save_data

# 4. Convert pkl sang CSV, mở PlotJuggler, kiểm tra qj/dqj/gravity/target/tau
# 5. Chỉ sau khi pass, mới kiểm tra robot thật ở debug mode
python -m deploy.run_teleoperation_real --config run_teleoperation.yaml --net eno1 --debug --save_data

Khi mọi thứ pass ở debug, kiểm tra lần cuối:

Đây là lúc mới bỏ --debug. Ngay cả khi đó, phiên đầu tiên nên rất ngắn: đứng yên, squat nhẹ, dừng, xem log. Sau đó mới tăng biên độ command. Sim2real không phải là một cú nhảy; nó là một chuỗi phép đo nhỏ.

Kết luận

Bài cuối của series gom mọi thứ thành một tiêu chuẩn vận hành: dùng đúng transport (ChannelFactoryInitialize(1, "lo") cho sim, ChannelFactoryInitialize(0, argv[1]) cho real), đúng ROS domain (ROS_DOMAIN_ID=1 cho sim, 0 cho robot thật), đúng message type G1 (hg), đúng topic (rt/lowcmd, rt/lowstate), luôn có damping command, luôn log .pkl, luôn so sánh qj/dqj/gravity_orientation/target_dof_pos/tau trong PlotJuggler, và chỉ chuyển sang hardware khi torque guard không còn bị kích hoạt.

Nếu phải nhớ một câu: controller chưa pass PlotJuggler thì chưa được gặp robot thật. MuJoCo giúp bạn mắc lỗi rẻ hơn. PlotJuggler giúp bạn nhìn thấy lỗi. Torque guard giúp bạn không biến lỗi thành hành vi nguy hiểm trên hardware.

Bài viết liên quan

• Debug MPC/WBID G1 bằng PlotJuggler
• Từ PD giữ dáng đến WBID có tiếp xúc
• Upper body G1: IK, VR và MuJoCo