Unitree G1 whole-body với cuRobo: floating base, tool frames và retargeting

Whole-body nghĩa là gì trong cuRobo?

Với Unitree G1, whole-body không chỉ là "planning hai tay". Tài liệu cuRobo dùng ví dụ G1 29-DOF cho humanoid motion retargeting: thêm floating base bằng extra_links, định nghĩa nhiều tool_frames như pelvis, torso, shoulder, elbow, wrist, hip, knee, ankle, rồi solve chuỗi pose target bằng MotionRetargeter. Output gồm 35 DOF: 6 virtual base joints và 29 body joints.

Luồng solve thật sự: global IK rồi warm-start

MotionRetargeter không giải từng frame như một bài IK độc lập. Nó dùng hai pha:

1. Frame đầu tiên: chạy global IK với nhiều seed để tìm một posture hợp lý. Đây là lúc solver được phép tìm rộng vì chưa có nghiệm trước đó.
2. Các frame tiếp theo: dùng nghiệm frame trước làm warm start. Có thể chạy IK velocity-limited hoặc MPC (use_mpc=True) để thêm smoothness, acceleration và jerk cost.

Điểm này cực quan trọng cho G1. Nếu bạn batch từng frame rời rạc rồi gửi xuống robot, motion có thể có joint jump dù từng frame riêng lẻ đều "valid". Whole-body retargeting production phải coi trajectory là một chuỗi có memory, không phải danh sách pose độc lập.

1. Floating base bằng extra_links

Humanoid cần pelvis di chuyển tự do. cuRobo hỗ trợ thêm 6 virtual joints giữa base_link và pelvis mà không sửa URDF:

extra_links:
  base_link_x:
    parent_link_name: base_link
    child_link_name: null
    joint_name: base_j_x
    joint_type: X_PRISM
  base_link_ztheta:
    parent_link_name: base_link_ytheta
    child_link_name: pelvis
    joint_name: base_link_ztheta
    joint_type: Z_ROT

Điều quan trọng: virtual base là biến tối ưu trong retargeting/offline planning. Nó không có nghĩa là bạn gửi trực tiếp 6 DOF base command xuống motor.

2. Build G1 config

Tài liệu có ví dụ:

python -m curobo.examples.getting_started.build_robot_model \
  --urdf curobo/content/assets/robot/g1/g1_29dof_rev_1_0.urdf \
  --asset-path curobo/content/assets/robot/g1 \
  --tool-frames \
    pelvis torso_link \
    left_shoulder_roll_link left_elbow_link left_wrist_yaw_link \
    right_shoulder_roll_link right_elbow_link right_wrist_yaw_link \
    left_hip_roll_link left_knee_link left_ankle_roll_link \
    right_hip_roll_link right_knee_link right_ankle_roll_link \
  --output curobo/content/configs/robot/unitree_g1_29dof_retarget.yml \
  --visualize

Pre-built config cũng có trong repo. Nhưng với deploy thật, vẫn nên tự build lại để khớp URDF, mesh và end-effector bạn dùng.

3. Ba mức fidelity

MPC chậm hơn, nên trên Jetson chỉ dùng khi planner rate và horizon phù hợp. Với offline retargeting BVH sang CSV, workstation có thể hợp lý hơn.

4. Tool frame weighting

Không phải link nào cũng nặng như nhau:

• pelvis/feet: position weight cao để giữ dáng và balance proxy;
• wrists: position và orientation cao cho manipulation;
• shoulders: weight thấp hơn vì elbow/wrist đã ràng buộc arm;
• torso: dùng để tránh posture vặn quá mức.

Weight sai làm robot "đuổi" target human không phù hợp tỉ lệ cơ thể G1.

Một cấu hình tư duy thực dụng:

tool_weights = {
    "pelvis": "high position, low orientation",
    "left_ankle_roll_link": "high position+orientation",
    "right_ankle_roll_link": "high position+orientation",
    "left_wrist_yaw_link": "high position+orientation for manipulation",
    "right_wrist_yaw_link": "high position+orientation for manipulation",
    "left_shoulder_roll_link": "low, mostly posture regularization",
    "right_shoulder_roll_link": "low, mostly posture regularization",
    "torso_link": "medium, prevent unnatural twist",
}

Nếu target đến từ BVH/SOMA, hãy log cả target pose sau rescale. Rất nhiều lỗi "cuRobo retarget sai" thực chất là mapping human-to-robot sai: hip width, shoulder offset, foot contact hoặc coordinate frame của mocap.

5. Streaming vs offline

solve_sequence phù hợp offline: BVH, dataset generation, preview motion. solve_frame phù hợp online/teleop nhưng cần warm start và velocity limit rất chặt. Không dùng streaming whole-body trực tiếp trên robot thật nếu chưa có stabilizing controller bên dưới.

Checklist trước khi chuyển từ offline sang streaming:

• CSV/offline trajectory replay được trong simulator với cùng URDF.
• Max joint delta giữa hai frame nằm dưới giới hạn controller.
• Foot target không trượt khi lẽ ra đang contact.
• Virtual base không được biến thành command phần cứng.
• Whole-body output đi qua locomotion/balance policy hoặc WBC, không gửi raw target trực tiếp xuống motor.
• Khi solve_frame timeout, robot giữ target trước đó hoặc chuyển về safe posture, không dùng late result.

Kết luận

cuRobo mở cửa cho G1 whole-body retargeting thực dụng, nhất là khi cần chuyển mocap/human motion sang joint trajectory có self-collision awareness. Nhưng output whole-body vẫn phải đi qua policy/controller giữ thăng bằng. Bài cuối sẽ gom deployment checklist, issue hiện tại và quy trình đưa lên production.