Biến LAFAN1 G1 thành MCAP Foxglove

Vì sao bài 2 đi qua MCAP trước khi nói PlotJuggler và controller?

Ở bài 1, ta đã dựng nền tảng Unitree G1 trong MuJoCo qua SDK2 DDS: chọn đúng scene, domain, interface, topic low-level và mental model LowCmd/LowState. Nhưng nếu chỉ nhìn robot chạy trong viewer, bạn rất khó debug một chuyển động dài. Bạn cần một file log có timestamp, có frame transform, mở lại được sau khi tắt simulator, chia sẻ được cho người khác, và quan trọng nhất là xem được trong một 3D panel có trục tọa độ rõ ràng. Đó là lý do bài này chuyển sang MCAP + Foxglove.

Bài này mổ xẻ script chính trong repo Foxglove tutorials: datasets/lafan1/LAFAN1_MCAP_WORKING.py. Script này đọc joints_labeled.csv, load URDF G1, dùng URDF.link_fk() để tính forward kinematics, đóng gói toàn bộ link pose vào message FrameTransforms, rồi ghi vào MCAP trên topic /tf bằng mcap_writer.write_message(topic="/tf"). Sau khi chạy xong, bạn có pose_data.mcap để kéo thả vào Foxglove và phát lại motion G1 như một log robotics bình thường.

Nếu bạn mới với MuJoCo và URDF, đọc thêm Bắt đầu với MuJoCo. Nếu bạn quan tâm đường dài từ motion reference sang policy sim-to-real, bài Chạy GR00T-VisualSim2Real cho G1 cho bối cảnh rộng hơn.

Roadmap series

Series G1 MuJoCo: điều khiển, Foxglove và PlotJuggler có 6 bài:

Điểm quan trọng: bài này không viết controller. Ta chỉ dựng đường ống dữ liệu kinematics. Khi sang PlotJuggler, bạn sẽ dùng tư duy tương tự để vẽ signal theo thời gian. Khi sang bài 4, cùng một khái niệm timestamp và frame sẽ giúp bạn kiểm tra controller có bám đúng reference hay không.

Dataset LAFAN1 G1 trong bài này gồm những gì?

Foxglove tutorial mô tả LAFAN1 Retargeting Dataset là motion capture đã được retarget sang Unitree G1. Thay vì chứa ảnh camera hay contact force, file demo chính là một CSV pose theo frame. Mỗi dòng biểu diễn một thời điểm của chuyển động. Về mặt logic, dòng đó gồm:

Một điểm cần nói thẳng vì beginner rất dễ vấp: mô tả dataset thường nói "7 cột floating pelvis pose và 30 cột joint angle". Bản joints_labeled.csv đang nằm trong repo Foxglove tutorials mà tôi kiểm tra có header:

Frame,X,Y,Z,QX,QY,QZ,QW,left_hip_pitch_joint,...,right_wrist_yaw_joint

Tổng cộng có 37 cột: 1 cột Frame, 7 cột pose, và 29 cột joint đúng với model G1 29DoF. Script dùng columns = list(df.columns)[8:37], tức lấy các cột từ index 8 đến 36. Nếu file bạn tải từ nguồn khác thật sự có 30 joint columns, đừng sửa bằng cảm tính. Hãy in len(columns) và tên cột trước:

df = pd.read_csv(CSV_FILE, nrows=0)
columns = list(df.columns)[8:]
print(len(columns))
print(columns)

Nguyên tắc an toàn là: mapping theo header CSV, không mapping theo trí nhớ. Đây cũng là lý do script có JOINT_NAME_TO_CSV_INDEX thay vì hardcode left_hip_pitch_joint = row[8], left_hip_roll_joint = row[9] trong từng dòng.

Các biến cấu hình đầu file

Phần đầu LAFAN1_MCAP_WORKING.py đặt những hằng số quyết định toàn bộ pipeline:

BASE_TIME = datetime.datetime(2024, 1, 1, 0, 0, 0)
MCAP_FILE = "pose_data.mcap"
CSV_FILE = "LAFAN1_Retargeting_Dataset/joints_labeled.csv"
URDF_FILE = "LAFAN1_Retargeting_Dataset/robot_description/g1/g1_29dof_rev_1_0.urdf"
RATE_HZ = 30.0

CSV_FILE là dữ liệu chuyển động. URDF_FILE là mô hình kinematic của robot: link nào nối với link nào, joint nào quay quanh trục nào, origin của từng joint nằm ở đâu. MCAP_FILE là output bạn mở trong Foxglove. RATE_HZ = 30.0 nghĩa là dataset có 30 frame mỗi giây, nên frame 0 ở BASE_TIME, frame 1 ở BASE_TIME + 1/30 giây, frame 30 ở BASE_TIME + 1 giây.

BASE_TIME không cần trùng thời gian thật. MCAP cần timestamp dạng Unix nanoseconds để player biết message nào đến trước, message nào đến sau. Với offline kinematics, bạn chỉ cần một mốc giả ổn định. Quan trọng là mỗi frame cách nhau đúng 1 / RATE_HZ, không phải timestamp hiện tại của máy khi script chạy. Nếu bạn dùng datetime.now() trong vòng lặp, playback sẽ phụ thuộc tốc độ convert, không còn là motion 30 Hz.

Bạn nên đặt cấu trúc thư mục như sau:

lafan1_work/
  LAFAN1_MCAP_WORKING.py
  LAFAN1_Retargeting_Dataset/
    joints_labeled.csv
    robot_description/
      g1/
        g1_29dof_rev_1_0.urdf

Sau đó chạy:

python -m venv .venv
source .venv/bin/activate
pip install numpy pandas urdfpy mcap-protobuf-support foxglove-schemas-protobuf
python LAFAN1_MCAP_WORKING.py

Nếu package name của schema thay đổi theo môi trường Python, ưu tiên kiểm tra README của Foxglove tutorial tại thời điểm bạn chạy. Ý tưởng không đổi: cần writer MCAP protobuf, cần message FrameTransforms, cần parser URDF, và cần numpy cho ma trận 4x4.

JOINT_NAME_TO_CSV_INDEX: đừng đoán index bằng mắt

Script đọc header bằng pandas:

df = pd.read_csv(CSV_FILE, nrows=0)
columns = list(df.columns)[8:37]
JOINT_NAME_TO_CSV_INDEX = {joint: i for i, joint in enumerate(columns, start=8)}
print(JOINT_NAME_TO_CSV_INDEX)

Mục tiêu là tạo dictionary kiểu:

{
  "left_hip_pitch_joint": 8,
  "left_hip_roll_joint": 9,
  "left_hip_yaw_joint": 10,
  ...
}

Tại mỗi frame, code sẽ duyệt dictionary này để tạo joint_positions:

joint_positions = {}
for joint_name, col_index in JOINT_NAME_TO_CSV_INDEX.items():
    if col_index < len(row):
        joint_positions[joint_name] = float(row[col_index])
    else:
        joint_positions[joint_name] = 0.0

URDF.link_fk() của urdfpy nhận cfg là map từ joint name sang giá trị joint. Vì vậy tên cột CSV phải khớp tên joint trong URDF. Nếu CSV có left_hip_pitch_joint nhưng URDF dùng left_hip_pitch, joint đó sẽ không được set như bạn nghĩ. Nếu motion trong Foxglove nhìn như "gãy" ở một vùng cơ thể, hãy kiểm tra ba thứ: tên joint trong CSV, tên joint trong URDF, và len(JOINT_NAME_TO_CSV_INDEX).

Workflow debug nhanh:

robot = URDF.load(URDF_FILE)
urdf_joint_names = {j.name for j in robot.joints}
csv_joint_names = set(JOINT_NAME_TO_CSV_INDEX.keys())

print("CSV not in URDF:", sorted(csv_joint_names - urdf_joint_names))
print("URDF not in CSV:", sorted(urdf_joint_names - csv_joint_names))

Không phải mọi joint trong URDF đều cần có trong CSV. URDF có thể có fixed joints, mimic joints hoặc links phụ cho mesh. Nhưng mọi revolute joint bạn muốn animate phải có giá trị hợp lệ trong joint_positions.

FLOATING_ROOT_FROM_CSV: pelvis đang trôi trong world

URDF thường mô tả robot như một cây kinematic bắt đầu từ root link, ví dụ pelvis. Nó nói "từ pelvis đến left hip là transform nào", "từ left hip đến knee là transform nào". Nhưng motion capture của humanoid không chỉ là góc khớp. Pelvis cũng di chuyển trong world: tiến về phía trước, nâng lên hạ xuống, xoay quanh trục yaw, nghiêng khi bước.

Vì vậy CSV có 7 cột floating root:

x, y, z = map(float, row[1:4])
qx, qy, qz, qw = map(float, row[4:8])
base_pose_4x4 = build_base_pose(x, y, z, qx, qy, qz, qw)

build_base_pose() tạo ma trận đồng nhất 4x4:

[ R00 R01 R02 x ]
[ R10 R11 R12 y ]
[ R20 R21 R22 z ]
[  0   0   0  1 ]

Trong đó R đến từ quaternion pelvis. Khi FLOATING_ROOT_FROM_CSV = True, script nhân:

global_tf = base_pose_4x4 @ local_tf

Nghĩa là local_tf do URDF tính trong hệ pelvis sẽ được đưa ra world bằng pose pelvis từ CSV. Nếu bỏ bước này, G1 vẫn có thể vung tay/chân đúng tương đối, nhưng robot sẽ bị "đóng đinh" tại gốc tọa độ. Bạn sẽ mất chuyển động toàn thân qua không gian, và Foxglove chỉ hiển thị một skeleton cử động tại chỗ.

Khi nào đặt FLOATING_ROOT_FROM_CSV = False? Chỉ khi URDF hoặc loader của bạn đã có floating joint thật cho root và link_fk() đã trả pose world đầy đủ. Với script này, cách beginner nên giữ là True.

convert_csv_to_mcap(): vòng lặp chính

Hàm chính có cấu trúc rất dễ đọc:

def convert_csv_to_mcap(csv_path, urdf_path, mcap_path):
    robot = URDF.load(urdf_path)
    with open(csv_path, "r") as csv_file, open(mcap_path, "wb") as out_file, Writer(out_file) as mcap_writer:
        csv_reader = csv.reader(csv_file)
        headers = next(csv_reader, None)
        for row in csv_reader:
            ...

Bước 1 là load URDF một lần. Đừng load URDF trong từng frame, vì URDF không đổi. Bước 2 là mở CSV và output MCAP. Writer(out_file) quản lý schema/channel bên trong MCAP, nên bạn chỉ cần gọi write_message() cho từng frame.

Trong vòng lặp, script tạo timestamp:

frame_idx = int(row[0])
timestamp = BASE_TIME + datetime.timedelta(seconds=(frame_idx / RATE_HZ))
timestamp_ns = int(timestamp.timestamp() * 1e9)
proto_timestamp = Timestamp()
proto_timestamp.FromDatetime(timestamp)

Ở đây có hai dạng thời gian. timestamp_ns là integer nanoseconds cho metadata MCAP: log_time và publish_time. proto_timestamp là field bên trong từng FrameTransform, để Foxglove hiểu transform đó thuộc thời điểm nào. Beginner hay quên một trong hai. Nếu MCAP metadata đúng nhưng message timestamp sai, panel có thể hiển thị khó đoán. Nếu message timestamp đúng nhưng log_time loạn, playback timeline vẫn không mượt.

URDF.link_fk(): biến joint angle thành link pose

Sau khi lấy pelvis pose và joint angle, script gọi:

fk_poses = robot.link_fk(cfg=joint_positions)

Theo tài liệu urdfpy, cfg là map từ joint hoặc joint name sang configuration value. Kết quả là map từ Link object sang ma trận pose 4x4 của link đó. Đây là đoạn làm việc nặng nhất về kinematics. Bạn không tự viết forward kinematics cho 29 khớp; bạn để URDF parser đi qua cây link-joint và nhân transform theo thứ tự đúng.

Hãy hiểu link_fk() theo ví dụ chân trái:

pelvis
  -> left_hip_pitch_joint
  -> left_hip_roll_joint
  -> left_hip_yaw_joint
  -> left_knee_joint
  -> left_ankle_pitch_joint
  -> left_ankle_roll_joint
  -> left_foot

Mỗi joint có origin cố định trong URDF và một biến động từ CSV. Forward kinematics nhân toàn bộ chain đó để ra pose cuối cùng của từng link. Nếu joint angle tính bằng radian mà bạn lỡ coi là degree, motion sẽ sai rất mạnh. Nếu quaternion pelvis không chuẩn hóa, rotation matrix có thể trôi. Trong pipeline production, nên thêm check:

norm = math.sqrt(qx*qx + qy*qy + qz*qz + qw*qw)
if abs(norm - 1.0) > 1e-3:
    qx, qy, qz, qw = qx/norm, qy/norm, qz/norm, qw/norm

Tạo FrameTransforms cho topic /tf

Foxglove schema FrameTransform biểu diễn pose của một child frame trong parent frame: translation là vị trí gốc child so với parent, rotation là hướng child so với parent. FrameTransforms chỉ là mảng nhiều FrameTransform. Script tạo một message cho mỗi frame CSV:

frame_transforms = FrameTransforms()
for link_obj, mat in link_poses.items():
    child_frame_id = link_obj.name
    parent_frame_id = WORLD_FRAME_ID

    trans = mat[:3, 3]
    rot_mat = mat[:3, :3]
    q = rot_matrix_to_quat(rot_mat)

    ftransform = FrameTransform()
    ftransform.timestamp.CopyFrom(proto_timestamp)
    ftransform.child_frame_id = child_frame_id
    ftransform.parent_frame_id = parent_frame_id
    ftransform.translation.CopyFrom(Vector3(x=float(trans[0]), y=float(trans[1]), z=float(trans[2])))
    ftransform.rotation.CopyFrom(Quaternion(x=float(q[0]), y=float(q[1]), z=float(q[2]), w=float(q[3])))
    frame_transforms.transforms.append(ftransform)

Script chọn cách đơn giản: mọi link đều có parent là world. Cách này không phải TF tree đẹp nhất, nhưng rất dễ visualize vì mỗi transform đã là pose global sau bước base_pose_4x4 @ local_tf. Nếu bạn muốn TF tree đúng topology, bạn phải dựng map child link sang parent link từ robot.joints, rồi ghi transform tương đối giữa parent-child. Đổi lại, bạn cần cẩn thận hơn với floating pelvis. Với bài beginner này, global pose theo world là đủ để kiểm tra motion.

mcap_writer.write_message(topic="/tf")

Cuối mỗi frame, script ghi message:

mcap_writer.write_message(
    topic="/tf",
    message=frame_transforms,
    log_time=timestamp_ns,
    publish_time=timestamp_ns,
)

topic="/tf" là lựa chọn quen thuộc với người làm ROS/Foxglove: đây là nơi 3D panel tìm transform frame. message=frame_transforms là protobuf message chứa tất cả link pose của frame hiện tại. log_time và publish_time đều dùng timestamp_ns vì đây là offline conversion, không có độ trễ network cần mô hình hóa. Nếu bạn đang ghi live telemetry từ DDS hoặc ROS 2, publish_time có thể là thời điểm sensor/controller tạo message, còn log_time là thời điểm recorder nhận và ghi.

Sau khi chạy xong, script in:

MCAP file created: pose_data.mcap

Mở Foxglove Desktop hoặc app web, kéo pose_data.mcap vào, thêm 3D panel, bật frame visibility. Nếu không thấy robot, hãy kiểm tra:

Sơ đồ pipeline

Bạn có thể hình dung toàn bộ script như một pipeline 5 bước:

joints_labeled.csv
  Frame + pelvis XYZQ + joint angles
        |
        v
JOINT_NAME_TO_CSV_INDEX
  header -> joint name -> column index
        |
        v
URDF.link_fk(cfg=joint_positions)
  joint angles + URDF tree -> local link poses
        |
        v
FLOATING_ROOT_FROM_CSV
  world_T_pelvis @ pelvis_T_link -> world_T_link
        |
        v
MCAP /tf
  FrameTransforms -> pose_data.mcap -> Foxglove 3D panel

Điểm hay của MCAP là nó tách dữ liệu khỏi môi trường chạy. Bạn không cần mở MuJoCo, không cần DDS domain, không cần controller. Chỉ cần file pose_data.mcap, Foxglove có thể phát lại motion. Đây là cách rất tốt để review reference motion trước khi biến nó thành target cho PD/WBID hoặc policy tracking.

Bài tập thực hành

Để tự tin rằng bạn hiểu pipeline, hãy làm 5 bài nhỏ:

1. In len(JOINT_NAME_TO_CSV_INDEX) và 5 joint đầu/cuối. Xác nhận file của bạn có 29 hay 30 joint columns.
2. Chỉ convert 100 frame đầu bằng if frame_idx > 100: break, mở MCAP và kiểm tra playback.
3. Đặt FLOATING_ROOT_FROM_CSV = False, convert lại, so sánh robot đứng tại chỗ hay di chuyển qua world.
4. Đổi RATE_HZ từ 30.0 sang 15.0, mở lại Foxglove để cảm nhận timeline chậm đi gấp đôi.
5. Thêm check quaternion norm và cảnh báo nếu norm lệch quá 1e-3.

Sau khi làm xong, bạn đã có kỹ năng quan trọng hơn bản thân file LAFAN1: đọc một nguồn motion dạng bảng, map nó vào URDF, tạo transform có timestamp, và đóng gói thành MCAP để debug. Đó là nền cho bài 3 về PlotJuggler và cho bài 4 khi ta so reference motion với state thật từ MuJoCo DDS.

Nguồn tham khảo

• Foxglove tutorial: Converting the LAFAN1 Retargeting dataset to MCAP
• Source code LAFAN1_MCAP_WORKING.py
• Foxglove FrameTransform schema
• Foxglove FrameTransforms schema
• MCAP Python protobuf writer guide
• urdfpy URDF.link_fk() documentation

Bài viết liên quan

• Dựng G1 MuJoCo qua DDS low-level
• PlotJuggler cho tín hiệu G1
• WBID và PD cho G1 trong MuJoCo