Upper body G1: IK, VR và MuJoCo

Vì sao bài này quan trọng?

Ở bài 1, ta đã dựng G1 trong MuJoCo qua low-level DDS. Bài 2 chuyển motion thành MCAP để xem bằng Foxglove. Bài 3 dùng PlotJuggler để nhìn tín hiệu điều khiển. Bài 4 so sánh PD giữ dáng với WBID có tiếp xúc.

Bài 5 chuyển sang một tầng rất thực dụng: điều khiển thân trên G1 bằng VR và IK trong MuJoCo. Nguồn chính là repo GalaxyGeneralRobotics/OpenWBT, đặc biệt các file deploy/run_teleoperation_mujoco.py, deploy/controllers/controller.py, deploy/configs/run_teleoperation.yaml, deploy/helpers/ik_server.py và class G1_29_ArmIK trong deploy/teleop/robot_control/robot_arm_ik.py. README của OpenWBT mô tả hệ thống teleoperation cho Unitree G1/H1 dùng Apple Vision Pro, hỗ trợ robot thật và môi trường mô phỏng; lower body dùng joystick để đi bộ/thay đổi pose, còn upper body lấy pose tay người từ VR rồi chạy inverse kinematics.

Mục tiêu của bài không phải là viết lại OpenWBT từ đầu. Mục tiêu là đọc được pipeline thật: TeleVisionWrapper.get_data() lấy left_wrist/right_wrist, G1_29_ArmIK.solve_ik() sinh sol_q, code clip quanh runner.target_dof_pos[runner.config.action_hl_idx], rồi Runner_handle_mujoco_vision.run_squat() hoặc run_loco() đẩy target xuống MuJoCo với control_dt: 0.02, control_decimation: 4 và model resources/robots/g1_description/g1_29dof_camera.xml.

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

Roadmap series

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

Cấu hình chạy: run_teleoperation.yaml

Điểm bắt đầu của bài này là file:

deploy/configs/run_teleoperation.yaml

Các dòng quan trọng:

squat_config: 'g1_squat.yaml'
loco_config: 'g1_loco.yaml'
exp_name: 'run_teleoperation'
xml_path: "resources/robots/g1_description/g1_29dof_camera.xml"

control_dt: 0.02
control_decimation: 4

num_dof: 29
dof_idx: [0, 1, ..., 28]
action_hl_idx: [15, 16, 17, 18, 19, 20, 21,
                22, 23, 24, 25, 26, 27, 28]

xml_path là model MuJoCo có camera cho G1 29 DOF. Từ tên file g1_29dof_camera.xml, ta biết đây không chỉ là robot body: nó còn có camera left_eye và right_eye để render ảnh gửi ngược về VR. Runner_handle_mujoco_vision sẽ dùng đúng hai camera này.

control_dt: 0.02 nghĩa là tầng điều khiển high-level chạy ở 50 Hz. control_decimation: 4 nghĩa là không phải mỗi bước MuJoCo đều gọi policy/squat/loco update; trong Runner_handle_mujoco.run_squat() và run_loco(), code chỉ update target khi counter % control_decimation == 0. Các bước giữa vẫn dùng target cũ để tính PD torque và gọi mujoco.mj_step(). Nếu simulation_dt của sim là 0.005 giây, decimation 4 sẽ khớp với high-level 0.02 giây. Beginner nên nhớ mối quan hệ này: policy update chậm hơn physics step, còn PD torque vẫn được áp xuống mỗi bước mô phỏng.

action_hl_idx là phần nối quan trọng giữa IK và vector 29 DOF. Index 0-11 là hai chân, 12-14 là waist, 15-21 là tay trái, 22-28 là tay phải. Bài này tập trung vào 14 index cuối: hai vai, khuỷu và cổ tay cho mỗi bên. Tên joint cụ thể được comment trong g1_squat.yaml: shoulder_pitch, shoulder_roll, shoulder_yaw, elbow, wrist_roll, wrist_pitch, wrist_yaw.

Loop chính trong run_teleoperation_mujoco.py

OpenWBT chạy MuJoCo teleoperation bằng:

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

Trong file deploy/run_teleoperation_mujoco.py, chương trình tạo shared memory cho ảnh stereo:

tv_img_shape = (480, 1280, 3)
tv_img_shm = shared_memory.SharedMemory(create=True, size=np.prod(tv_img_shape))
tv_img_array = np.ndarray(tv_img_shape, dtype=np.uint8, buffer=tv_img_shm.buf)

Kích thước 480 x 1280 đến từ hai ảnh 640 x 480 ghép ngang: camera trái và camera phải. Sau đó code tạo:

tv_wrapper = TeleVisionWrapper(True, tv_img_shape, tv_img_shm.name)
arm_ik = G1_29_ArmIK()
runner = Runner_handle_mujoco_vision(config, args=args)
current_mode = "SQUAT"

TeleVisionWrapper(True, ...) bật chế độ binocular. G1_29_ArmIK() là solver IK cho hai tay G1 29 DOF. Runner_handle_mujoco_vision vừa là runner MuJoCo, vừa render ảnh từ camera trái/phải.

Phần quan trọng nhất nằm trong hàm con tv_arms():

head_rmat, left_wrist, right_wrist, left_hand, right_hand = tv_wrapper.get_data()
current_lr_arm_q = runner.qj.copy()[15:29]
current_lr_arm_dq = runner.dqj.copy()[15:29]
sol_q, sol_tauff = arm_ik.solve_ik(
    left_wrist,
    right_wrist,
    current_lr_arm_q,
    current_lr_arm_dq,
)
sol_q = np.clip(
    sol_q,
    runner.target_dof_pos[runner.config.action_hl_idx] - 0.01,
    runner.target_dof_pos[runner.config.action_hl_idx] + 0.01,
)
runner.target_dof_pos[runner.config.action_hl_idx] = sol_q

Đọc chậm đoạn này sẽ thấy toàn bộ upper-body pipeline:

Điểm thực dụng nhất là bước clip. IK có thể nhảy nếu tracking VR nhiễu, frame đổi đột ngột, hoặc bài toán tối ưu rơi vào nghiệm khác. OpenWBT không gán thẳng sol_q xuống robot. Nó chỉ cho target tay dịch từng bước rất nhỏ quanh target hiện tại. 0.01 rad xấp xỉ 0.57 độ. Đây là một bộ giới hạn tốc độ đơn giản nhưng hữu ích trước khi PD torque nhận target.

TeleVisionWrapper.get_data() thực sự trả gì?

File deploy/teleop/open_television/tv_wrapper.py là nơi dễ bị đọc lướt. Nhưng đây là nơi quyết định IK có đúng hệ tọa độ hay không.

Wrapper bắt đầu từ dữ liệu OpenXR/Vuer: head matrix, left hand, right hand và 25 landmarks cho mỗi bàn tay. Trong comment, tác giả ghi rõ convention:

OpenXR: y up, z back, x right
Robot:  z up, y left, x front

Vì vậy bước đầu tiên là đổi basis:

head_mat = T_robot_openxr @ head_vuer_mat @ fast_mat_inv(T_robot_openxr)
left_wrist_mat = T_robot_openxr @ left_wrist_vuer_mat @ fast_mat_inv(T_robot_openxr)
right_wrist_mat = T_robot_openxr @ right_wrist_vuer_mat @ fast_mat_inv(T_robot_openxr)

Sau đó code đổi wrist convention từ Apple Vision Pro/XR sang Unitree URDF. Hai bên trái/phải không giống nhau vì quy ước trục của tay trái và tay phải khác nhau:

unitree_left_wrist = left_wrist_mat @ T_to_unitree_left_wrist
unitree_right_wrist = right_wrist_mat @ T_to_unitree_right_wrist

Tiếp theo là chuyển origin. Wrist pose ban đầu ở world/head frame, nhưng solver IK của OpenWBT lấy origin gần waist joint motor. Vì vậy code trừ translation của head rồi cộng offset:

unitree_left_wrist[0:3, 3] -= head_mat[0:3, 3]
unitree_right_wrist[0:3, 3] -= head_mat[0:3, 3]

unitree_left_wrist[0, 3] += 0.15
unitree_right_wrist[0, 3] += 0.15
unitree_left_wrist[2, 3] += 0.45
unitree_right_wrist[2, 3] += 0.45

Kết quả cuối cùng:

return head_rmat, unitree_left_wrist, unitree_right_wrist, unitree_left_hand, unitree_right_hand

Trong run_teleoperation_mujoco.py, bài toán arm IK chỉ dùng left_wrist/right_wrist. left_hand/right_hand vẫn được trả về, nhưng pipeline MuJoCo vision trong file này không dùng trực tiếp để điều khiển finger joints. Điều đó quan trọng: bài này đang nói về upper arm IK 14 DOF, không phải điều khiển dexterous hand đầy đủ. Các class hand trong controller.py nằm ở nhánh real/dexhand riêng, có topic DDS rt/dex3/left/cmd và rt/dex3/right/cmd.

G1_29_ArmIK: vì sao chỉ giải hai tay?

Class G1_29_ArmIK build robot từ:

deploy/assets/g1/g1_body29_hand14.urdf

Nhưng solver không tối ưu toàn bộ robot. Nó tạo mixed_jointsToLockIDs gồm toàn bộ joint chân, waist và các joint ngón tay. Sau đó gọi buildReducedRobot(). Nói đơn giản: model đầy đủ có body, tay và bàn tay; reduced model khóa những phần không cần giải, chỉ để lại 14 joint của hai tay.

Sau khi có reduced model, code thêm hai operational frame:

L_ee: gắn vào left_wrist_yaw_joint, offset [0.05, 0, 0]
R_ee: gắn vào right_wrist_yaw_joint, offset [0.05, 0, 0]

Hai frame này là end-effector mà IK cố đưa tới left_wrist/right_wrist từ VR. Solver dùng Pinocchio cho kinematics, CasADi để tạo hàm symbolic, và Ipopt để giải tối ưu. Cost có bốn phần:

50 * translational_cost
+ 1 * rotation_cost
+ 0.02 * regularization_cost
+ 0.1 * smooth_cost

Trọng số vị trí là 50, lớn hơn rotation rất nhiều. Đây là lựa chọn hợp lý cho teleoperation beginner-friendly: bàn tay/cổ tay phải đến gần vị trí mong muốn trước, còn orientation vẫn bám nhưng không được phép làm nghiệm joint quá cực đoan hoặc giật mạnh. regularization_cost hạn chế joint đi xa vô lý. smooth_cost phạt chênh lệch giữa nghiệm mới và nghiệm trước. Ngoài ra còn có WeightedMovingFilter([0.4, 0.3, 0.2, 0.1], 14) để lọc nghiệm sol_q.

Hàm solve_ik() nhận:

solve_ik(left_wrist, right_wrist, current_lr_arm_motor_q, current_lr_arm_motor_dq)

Nếu có current_lr_arm_motor_q, nó dùng làm init_data và var_q_last. Đây là một chi tiết rất quan trọng. IK không bắt đầu từ neutral pose mỗi frame; nó bắt đầu từ trạng thái tay hiện tại. Nhờ vậy nghiệm thường gần posture hiện tại hơn, ít nhảy hơn, và phù hợp với loop realtime.

Khi solve thành công, code lấy sol_q, lọc, cập nhật init_data, rồi tính sol_tauff bằng RNEA của Pinocchio:

sol_tauff = pin.rnea(model, data, sol_q, v, zeros)

Trong pipeline MuJoCo của bài này, sol_tauff không được dùng để set torque feedforward. Runner vẫn dùng PD position control ở MuJoCo. Vì vậy bạn nên hiểu sol_q là output chính: đó là joint target cho hai tay.

Nếu Ipopt không hội tụ, solve_ik() in debug info và trả về current_lr_arm_motor_q cùng torque zero. Đây là fallback an toàn: khi IK lỗi, tay không nhảy sang nghiệm rác; nó giữ gần state hiện tại.

Runner_handle_mujoco_vision: target thành torque như thế nào?

Trong controller.py, Runner_handle_mujoco load model:

self.m = mujoco.MjModel.from_xml_path(config.xml_path)
self.d = mujoco.MjData(self.m)
self.m.opt.timestep = self.config.simulation_dt

Mỗi lần run_squat() hoặc run_loco() chạy, runner lấy proprioception từ MuJoCo:

self.qj = self.d.qpos[7:][self.config.dof_idx]
self.dqj = self.d.qvel[6:][self.config.dof_idx]
self.quat = self.d.qpos[3:7]
self.ang_vel = self.d.qvel[3:6]

Một lần nữa, hãy chú ý qpos[7:] và qvel[6:]. Floating base có 7 phần tử position nhưng 6 phần tử velocity.

Trong run_squat(), nếu đến tick high-level:

if self.counter % self.config.control_decimation == 0:
    self.refresh_prop()
    gravity_orientation = self.get_gravity_orientation(self.quat)
    target_dof_pos = self.target_dof_pos.copy()
    cmd_raw[0] = usb_left.lx   # height
    cmd_raw[1] = usb_right.rx
    self.target_dof_pos = self.squat_controller.run(...)
    self.transition_squat()

Sau đó target được biến thành torque:

tau = self.pd_control(self.squat_controller, self.target_dof_pos)
self.d.ctrl[:] = tau
mujoco.mj_step(self.m, self.d)

pd_control() trong MuJoCo rất trực tiếp:

return (target_q - q) * kp + (target_dq - dq) * kd

Vì tv_arms() đã ghi sol_q vào target_dof_pos[action_hl_idx], phần tay của vector target sẽ đi vào cùng hàm PD này. Lower-body/squat/loco policy cập nhật chân và body; IK cập nhật hai tay; PD là tầng cuối biến toàn bộ vector target 29 DOF thành torque mô phỏng.

Runner_handle_mujoco_vision kế thừa runner trên và thêm stereo rendering:

self.renderer = Renderer(self.m, width=640, height=480)
self.left_camera_id = mj_name2id(..., "left_eye")
self.right_camera_id = mj_name2id(..., "right_eye")

Sau super().run_squat() hoặc super().run_loco(), nó render camera trái và phải rồi ghép ngang:

self.render_image = np.concatenate((left_image, right_image), axis=1)

Ở loop ngoài, np.copyto(tv_img_array, np.array(runner.render_image)) đưa ảnh này vào shared memory. TeleVisionWrapper dùng shared memory đó để gửi ảnh camera robot về thiết bị XR. Vì vậy pipeline là hai chiều: VR gửi wrist pose vào IK, MuJoCo render camera gửi ảnh ngược về VR.

SQUAT và LOCOMOTION khác nhau ở đâu?

Trong run_teleoperation_mujoco.py, mode ban đầu là "SQUAT". Ở mode này, loop gọi:

tv_arms()
runner.run_squat(manual=True)
viewer.sync()

Nghĩa là upper-body IK được cập nhật trong SQUAT mode. Người vận hành có thể điều chỉnh body pose bằng joystick, đồng thời đưa hai cổ tay bằng VR. Khi điều kiện chuyển mode thỏa, runner.transfer_to_loco đưa robot sang "LOCOMOTION".

Ở LOCOMOTION mode, loop gọi:

runner.run_loco(manual=True)
viewer.sync()

Trong file này, tv_arms() không được gọi ở nhánh LOCOMOTION. Điều đó không có nghĩa OpenWBT về mặt ý tưởng không thể teleop tay khi đi; nó chỉ nói đúng hành vi của script MuJoCo này: IK cổ tay được cập nhật ở SQUAT branch, còn LOCOMOTION branch tập trung vào locomotion controller. Khi đọc code hoặc sửa pipeline, đây là chi tiết không nên bỏ qua. Nếu bạn kỳ vọng vừa đi vừa vẫy tay trong chính script này, hãy kiểm tra lại nơi gọi tv_arms() trước khi nghi ngờ IK.

Hai mode cũng có policy riêng:

g1_loco.yaml có num_dof: 12, num_actions: 12 cho chân, nhưng gain/default angle vẫn là vector 29 phần tử để PD tầng cuối có đủ target. g1_squat.yaml quan sát 29 DOF nhưng action policy chính là 12 action lower-body. Phần arm target đến từ IK thông qua action_hl_idx.

ik_server.py dùng khi nào?

File deploy/helpers/ik_server.py không nằm trong loop MuJoCo trực tiếp ở trên, nhưng nó đáng đọc vì cho thấy OpenWBT tách IK thành service như thế nào cho Isaac Sim. IKServer mở TCP socket ở localhost:12345, nhận JSON gồm:

left_wrist
right_wrist
current_lr_arm_q
current_lr_arm_dq

Sau đó nó gọi cùng một solver:

sol_q, sol_tauff = self.arm_ik.solve_ik(...)

Nếu thành công, server trả sol_q và success: true. Nếu lỗi, nó trả lại current_lr_arm_q, success: false và error_msg. Cách thiết kế này có một lợi ích: Isaac Sim client không cần nhúng toàn bộ Pinocchio/CasADi solver vào process chính. Nó chỉ gửi wrist target và nhận joint target. Với MuJoCo script trong bài này, solver được gọi in-process; với Isaac Sim, solver có thể chạy như một process riêng bằng start_ik_server.sh.

Checklist debug cho beginner

Nếu bạn muốn tự chạy hoặc sửa pipeline này, hãy kiểm tra theo thứ tự sau:

1. run_teleoperation.yaml có đúng xml_path: resources/robots/g1_description/g1_29dof_camera.xml không. Nếu thiếu camera left_eye/right_eye, Runner_handle_mujoco_vision sẽ không render được stereo image.
2. control_dt và control_decimation có khớp với simulation_dt không. Nếu physics step và policy step lệch quá nhiều, robot có thể rung hoặc phản hồi chậm.
3. TeleVisionWrapper.get_data() có trả wrist matrix 4 x 4 hợp lệ không. Nếu tracking mất, hãy in translation và determinant của rotation matrix trước khi đổ lỗi cho IK.
4. current_lr_arm_q = runner.qj[15:29] có đúng joint order không. Sai một index là vai/khuỷu/cổ tay sẽ đổi vai cho nhau.
5. solve_ik() có hội tụ không. Nếu không, xem target wrist có nằm ngoài workspace robot không, hoặc offset head-to-waist có hợp lý không.
6. Đừng bỏ np.clip(... +/- 0.01) khi mới thử. Gỡ giới hạn này có thể làm tay giật mạnh khi VR tracking nhiễu.
7. Phân biệt sol_q và sol_tauff. Trong script MuJoCo này, sol_q là target chính; torque feedforward từ RNEA chưa được dùng làm lệnh điều khiển cuối.
8. Nhớ rằng trong file hiện tại, tv_arms() chỉ được gọi ở SQUAT mode. Nếu muốn upper body chạy trong LOCOMOTION, cần thay đổi loop có chủ đích và kiểm tra stability.

Một sơ đồ tối giản:

Apple Vision Pro / OpenXR
        |
        v
TeleVisionWrapper.get_data()
  - đổi hệ trục OpenXR -> robot
  - đổi wrist convention -> Unitree URDF
  - chuyển origin head -> waist
        |
        v
left_wrist, right_wrist
        |
        v
G1_29_ArmIK.solve_ik()
  - Pinocchio reduced model
  - CasADi + Ipopt
  - smooth filter
        |
        v
sol_q (14 arm joints)
        |
        v
clip quanh target_dof_pos[action_hl_idx] +/- 0.01
        |
        v
Runner_handle_mujoco_vision.run_squat()
        |
        v
PD torque -> mujoco.mj_step()
        |
        v
left_eye/right_eye render -> shared memory -> VR

Kết luận

OpenWBT đáng học vì nó không chỉ là một demo VR đẹp mắt. Nó cho thấy cách ghép nhiều tầng rất cụ thể: XR tracking, đổi hệ tọa độ, IK hai tay, limit target, policy squat/loco, PD torque, MuJoCo stepping và stereo rendering. Với beginner, bài học lớn nhất là không nhầm lẫn các tầng. VR không điều khiển torque trực tiếp. IK không điều khiển cả 29 DOF. sol_tauff không phải lệnh torque cuối trong script này. Và control_dt: 0.02 không có nghĩa physics chỉ chạy 50 Hz nếu control_decimation đang tách policy step khỏi simulation step.

Bài tiếp theo, Checklist sim-to-real, sẽ dùng chính các chi tiết này để kiểm tra trước khi chuyển từ MuJoCo sang robot thật: joint order, network interface, DDS topic, gain, mode switch, emergency damping và cách giữ thay đổi đủ nhỏ để robot không nhận một target bất ngờ.

Bài viết liên quan

• Từ PD giữ dáng đến WBID có tiếp xúc
• Checklist sim-to-real
• Bắt đầu với MuJoCo