Teleop toàn thân: TWIST và HOMIE

Mục tiêu của bài này

Ba bài trước đã đi từ bản đồ pipeline WholeBodyVLA, dữ liệu video cho LAM, đến retarget motion người sang robot. Bài 4 chuyển sang phần đắt hơn nhưng không thể né nếu muốn làm hệ WholeBodyVLA chạy thật: teleoperation toàn thân trên humanoid. Video action-free giúp học latent action. Retargeted motion giúp train hoặc stress-test controller. Nhưng dữ liệu robot thật vẫn cần một trạm thu có thể ghi lại operator command, robot state, output policy và trạng thái mạng khi robot tương tác với đồ vật thật.

Trong bài này ta so sánh hai stack cụ thể:

1. TWIST, một hệ Teleoperated Whole-Body Imitation System cho Unitree G1. Điểm đáng học nhất ở code deployment là cách tách high-level motion server và low-level controller qua Redis: deploy_real/server_high_level_motion_lib.py, server_low_level_g1_sim.py, server_low_level_g1_real.py, và script legacy realtime server_motion_optitrack_v2 (legacy).py.
2. HOMIE, một cockpit teleop isomorphic cho loco-manipulation. Điểm đáng học nhất là phần cứng operator gồm exoskeleton arms, motion-sensing gloves và pedal, cùng đường deploy HomieDeploy/g1_gym_deploy/scripts/deploy_policy.py dùng LCM, ONNX Runtime, StateEstimator, LCMAgent và DeploymentRunner.

Nếu bạn đang đọc theo thứ tự, hãy xem lại Bản đồ pipeline WholeBodyVLA, Video egocentric và LAM, và Retarget AMASS/GMR sang robot. Bài này chuẩn bị trực tiếp cho RL và LMO, vì teleop station tốt không chỉ thu demo cho VLA mà còn cho bạn quan sát controller thấp tầng có đang giữ thăng bằng đúng hay không. Ngoài series, hai bài nền hữu ích là phân tích WholeBodyVLA ICLR 2026 và hướng dẫn WholebodyVLA open-source.

Vì sao teleop toàn thân khác teleop cánh tay?

Teleop cánh tay thường chỉ cần operator điều khiển end-effector, gripper và camera. Robot có thể đứng yên trên bàn hoặc base di chuyển chậm. Humanoid thì khác: thao tác tay làm đổi trọng tâm, bước chân làm đổi góc nhìn, squat làm thay đổi cả reachability lẫn contact, còn robot thật luôn có giới hạn actuator, latency mạng, trạng thái pin, mode an toàn và damping state. Nếu chỉ log ảnh và joint target, bạn sẽ thiếu phần quan trọng nhất: vì sao robot không ngã trong lúc tay đang làm việc.

Một trạm thu dữ liệu WholeBodyVLA nên tách bốn dòng dữ liệu:

Beginner thường muốn viết một script duy nhất: đọc operator input, chạy policy, gửi action, log camera. Làm vậy dễ demo nhanh, nhưng khó debug. Khi robot giật, bạn không biết lỗi nằm ở retarget, policy, state estimator, latency hay Unitree network. TWIST và HOMIE cho ta hai cách tổ chức khác nhau để tránh mớ rối đó.

Thiết kế tối thiểu nên có:

operator process  --->  command bus  --->  policy process  --->  robot interface
                              ^                  |
                              |                  v
                         state logger  <---  robot state

TWIST: tách high-level và low-level bằng Redis

Paper TWIST mô tả mục tiêu là teleoperate humanoid bằng dữ liệu whole-body người realtime và một neural network controller duy nhất. Hệ thống train controller bằng RL kết hợp behavior cloning, dùng motion retargeted và MoCap thật, rồi deploy trên Unitree G1 với 29 DoF để làm locomotion, manipulation toàn thân, legged manipulation và động tác biểu cảm. Ở mức triển khai, điểm dễ học nhất là cách repo tách high-level control khỏi low-level control.

README TWIST hướng dẫn sim2sim như sau: trước hết chạy high-level motion server để warm up Redis, sau đó chạy low-level simulation server server_low_level_g1_sim.py với JIT model. README nói rõ lý do: high-level control, tức teleop hoặc motion target, được tách khỏi low-level control, tức RL policy. Khi low-level server chạy, robot có thể đứng yên; sau đó high-level server bắt đầu stream motion để policy bám theo.

Luồng đó có thể vẽ như sau:

Retargeted motion file / MoCap stream
        |
        v
server_high_level_motion_lib.py
        |
        | Redis keys:
        |   action_mimic_g1
        |   action_hand_g1
        v
server_low_level_g1_sim.py hoặc server_low_level_g1_real.py
        |
        | JIT policy + proprio history
        v
PD target / robot action
        |
        v
MuJoCo sim hoặc Unitree G1 thật

Điều này rất hợp với data collection. Bạn có thể thay high-level source từ motion file sang OptiTrack, GMR, PICO, joystick, hoặc VLA decoder mà không viết lại toàn bộ low-level controller. Ngược lại, bạn có thể chạy cùng high-level command qua simulation trước, rồi chuyển sang robot thật khi command sạch.

server_high_level_motion_lib.py: motion thành mimic_obs

server_high_level_motion_lib.py đọc một motion file .pkl qua MotionLib, lấy các frame tương lai theo tar_obs_steps, và build mimic_obs. Trong code, mimic_obs gồm các thành phần như root height, roll/pitch/yaw, root velocity trong frame gốc, yaw angular velocity và joint positions. Với G1, script còn xử lý wrist IDs để thêm vị trí wrist roll vào vector đúng thứ tự.

Vòng lặp chính chạy với control_dt = 0.02, tức khoảng 50 Hz. Mỗi bước, server set hai Redis key:

redis_client.set(f"action_mimic_{args.robot}", json.dumps(mimic_obs_list))
redis_client.set(f"action_hand_{args.robot}", json.dumps(DEFAULT_ACTION_HAND[args.robot].tolist()))

Nếu bật --vis, script mở MuJoCo viewer, đặt qpos theo root pose và joint position từ motion, rồi sync camera theo pelvis. Khi dừng bằng keyboard interrupt hoặc khi thoát, server nội suy từ last_mimic_obs về DEFAULT_MIMIC_OBS trong khoảng 2 giây. Đây là chi tiết nhỏ nhưng rất đáng giữ trong station thật: khi operator mất input hoặc stop session, command nên quay về pose an toàn thay vì đóng băng ở một tư thế cuối bất kỳ.

Với data logger, high-level server là nơi nên ghi:

server_low_level_g1_sim.py: policy inference trong MuJoCo

Low-level simulation server là bản dễ debug nhất trước khi đụng robot thật. Script load TorchScript policy bằng torch.jit.load(policy_path), mở MuJoCo XML g1_sim2sim_with_wrist_roll.xml, đặt sim_dt = 0.001 và sim_decimation = 20. Như vậy physics có thể step ở 1 kHz, còn policy inference diễn ra mỗi 20 step, tương đương 50 Hz. Đây là pattern quen thuộc trong whole-body control: physics/control loop nhanh, policy loop thấp hơn nhưng vẫn đủ nhanh cho locomotion.

Ở mỗi lần inference, server:

1. Đọc qpos, qvel, IMU orientation và angular velocity từ MuJoCo.
2. Tạo proprioception gồm angular velocity scale, roll/pitch, sai lệch joint position so với default, joint velocity scale và last_action.
3. Publish state_body_g1 và state_hand_g1 lên Redis.
4. Đọc action_mimic_g1 từ Redis.
5. Ghép action_mimic, proprioception hiện tại và history buffer 10 frame.
6. Chạy JIT policy, clip action, scale action và cộng default pose để ra PD target.
7. Tính torque theo stiffness/damping, clip bởi torque limits, rồi gán vào data.ctrl.

Pseudocode dễ nhớ:

while sim_running:
    robot_state = read_mujoco_state()
    proprio = build_proprio(robot_state, last_action)
    redis.set("state_body_g1", proprio)

    mimic = redis.get("action_mimic_g1")
    obs = concat(mimic, proprio, history)
    raw_action = policy(obs)
    pd_target = default_dof_pos + action_scale * clip(raw_action)
    torque = kp * (pd_target - q) - kd * qdot
    mujoco.step(torque)

Vì low-level sim publish state ngược lại Redis, bạn có thể viết logger độc lập chỉ subscribe/poll Redis mà không cần sửa controller. Với beginner, đây là điểm thiết kế quan trọng nhất: logger không nên nằm sâu trong policy loop đến mức làm policy chậm. Nếu cần ghi dữ liệu lớn như ảnh camera, hãy ghi ở process riêng và đồng bộ bằng timestamp.

server_low_level_g1_real.py: cùng logic, thêm robot thật

server_low_level_g1_real.py giữ cùng ý tưởng như sim nhưng thay MuJoCo bằng G1RealWorldEnv. Script load config robot_control/configs/g1.yaml, nhận tham số --net cho network interface, load TorchScript policy, rồi chạy sequence reset robot. Trong code có các trạng thái như zero torque, move to default position, default position state, và nút Select trên remote để thoát vòng lặp.

Loop chính đọc state thật:

dof_pos, dof_vel, quat, ang_vel = env.get_robot_state()

Sau đó tạo obs_proprio, publish state_body_g1, đọc action_mimic_g1, ghép history, chạy policy, tính target_dof_pos, rồi gọi:

env.send_robot_action(
    target_dof_pos,
    kp_scale,
    kd_scale,
    left_wrist_roll=wrist_dof_pos[0],
    right_wrist_roll=wrist_dof_pos[1],
)

README TWIST mô tả thêm đường sim2real cho G1: nối robot và laptop bằng Ethernet, đặt IP laptop 192.168.123.222 với netmask 255.255.255.0, ping robot 192.168.123.164, dùng remote control vào dev mode bằng L2+R2, rồi chạy server_low_level_g1_real.py --policy_path ... --net YOUR_NET_INTERFACE_TO_UNITREE_ROBOT. Thứ tự cũng giống sim: chạy low-level controller trước, sau đó dùng high-level motion server để điều khiển.

Ở station thật, hãy log cả network interface và robot mode:

robot:
  model: unitree_g1
  net_interface: eno1
  laptop_ip: 192.168.123.222
  robot_ip: 192.168.123.164
  mode_entry: L2+R2 dev mode
  low_level_script: server_low_level_g1_real.py
policy:
  format: torchscript
  path: assets/twist_general_motion_tracker.pt
  frequency_hz: 50

Nếu một trajectory đẹp trong sim nhưng hỏng trên robot, log này giúp bạn phân biệt lỗi policy với lỗi deployment.

Script legacy OptiTrack: realtime retarget trước khi có GMR mới

TWIST README nói repo cung cấp script legacy dùng tháng 5/2025: deploy_real/server_motion_optitrack_v2 (legacy).py. README cũng ghi nhóm đã nâng cấp sang GMR cho realtime teleop, nên script cũ chủ yếu để tham khảo. Dù vậy, nó rất đáng đọc vì cho thấy high-level server realtime cần làm gì.

Script legacy kết nối OptiTrack, dùng MinkRetarget, giữ buffer Vicon/OptiTrack, retarget từng frame sang robot qpos, dùng inverse dynamics helper để lấy qdot, rồi _get_mimic_obs(qpos, qdot) chuyển qpos/qdot thành vector command. Nó cũng có JoyConController, Speaker, smoothing qdot bằng deque, seed buffer vài frame để tránh dữ liệu đầu vào chưa ổn định, và Redis để publish command.

Điểm rút ra không phải là "hãy dùng script legacy trong production". Điểm rút ra là realtime teleop high-level server nên có các lớp sau:

Nếu dùng GMR thay script legacy, kiến trúc vẫn tương tự. Chỉ thay retargeter và input adapter.

HOMIE: cockpit isomorphic thay vì MoCap retarget thuần túy

HOMIE giải quyết cùng bài toán teleop toàn thân nhưng chọn giao diện operator khác. Paper HOMIE mô tả một cockpit gồm policy loco-manipulation và phần cứng exoskeleton-based. Phần cứng có isomorphic exoskeleton arms, motion-sensing gloves và pedal. Ý tưởng là operator không cần đi theo robot để ra lệnh locomotion; pedal lấy command di chuyển, trong khi phần thân trên của operator điều khiển pose tay/thân trên. Paper nhấn mạnh policy có thể đi và squat tới các độ cao cụ thể trong khi chấp nhận upper-body poses thay đổi liên tục.

So với TWIST, HOMIE ít giống "MoCap toàn thân rồi retarget từng frame", mà giống một cockpit điều khiển chia vai:

exoskeleton arms  ---> upper-body pose command
motion gloves     ---> dexterous hand command
pedal             ---> locomotion / height command
RL policy         ---> lower-body balance, walking, squatting
Unitree SDK2      ---> robot actuation

Vì phần cứng isomorphic gần với robot hơn, HOMIE muốn giảm lỗi retarget so với hệ vision/IK thuần túy. Paper cũng ghi dữ liệu thu bằng cockpit có thể dùng cho imitation learning. Đây là điểm rất phù hợp với WholeBodyVLA: bạn không chỉ remote-control robot để làm demo, mà còn tạo dataset có command/state/action đủ sạch để train bước tự động hóa tiếp theo.

deploy_policy.py: đường inference ONNX trong HOMIE

Trong HomieDeploy/g1_gym_deploy/scripts/deploy_policy.py, script tạo LCM bus:

lc = lcm.LCM("udpm://239.255.76.67:7667?ttl=255")

Sau đó load_and_run_policy() load checkpoint ONNX tại:

ckpt_path = "/home/unitree/deploy/deploy.onnx"

Các thành phần chính là:

Mẫu inference trong file:

def load_onnx_policy(path):
    model = ort.InferenceSession(path)

    def run_inference(input_tensor):
        ort_inputs = {model.get_inputs()[0].name: input_tensor.cpu().numpy()}
        ort_outs = model.run(None, ort_inputs)
        return torch.tensor(ort_outs[0], device="cuda:0")

    return run_inference

Về mặt data station, deploy_policy.py là low-level policy runner của HOMIE. Operator cockpit và robot control binaries tạo ra command/state ở các process khác. Policy runner không nên ôm luôn logging nặng; nó nên expose hoặc mirror observation/action ra logger riêng.

HOMIE deployment: PC, G1, Dex-3 và SDK2

README HomieDeploy nói deployment cần Unitree G1 với Dex-3 hands và một PC; communication giữa robot và PC đi qua Wi-Fi. Trên robot, họ khuyến nghị gắn màn hình, bàn phím và chuột vào board của G1, cài PyTorch cho Nvidia Jetson Orin, cài requirements, build Unitree SDK2 C++ binaries cho g1_control.cpp và hand_control.cpp, rồi pip install -e trong g1_gym_deploy.

Thứ tự chạy deployment trong README:

# robot terminal 3
cd unitree_sdk2/build/bin && ./hand_control

# robot terminal 4
cd unitree_sdk2/build/bin && ./g1_control eth0  # hoặc eth1

# robot terminal 5
python g1_gym_deploy/scripts/deploy_policy.py

Trước khi deploy, README yêu cầu đóng initial control process của G1 bằng tổ hợp L1+A, L2+R2, L2+A, L2+B; sau đó đặt robot xuống đất và dùng nút R2 để cho robot đứng, rồi nhấn R2 lại. Đây là loại chi tiết vận hành phải ghi vào runbook của station. Nếu không ghi, hai session dùng cùng policy nhưng khác robot mode có thể cho kết quả khác nhau.

Một runbook đơn giản:

1. Xác nhận vùng thao tác trống, dây/cáp không vướng chân robot.
2. Xác nhận PC và robot thấy nhau qua Wi-Fi hoặc interface đúng.
3. Tắt initial control process theo đúng tổ hợp remote.
4. Chạy hand_control.
5. Chạy g1_control với interface đúng.
6. Chạy deploy_policy.py.
7. Đưa robot xuống đất, dùng R2 để stand.
8. Bắt đầu cockpit session và logger.
9. Nếu lỗi: dừng operator command trước, sau đó đưa robot về zero torque/default theo runbook.

So sánh TWIST và HOMIE cho trạm thu dữ liệu

Nếu mục tiêu là nghiên cứu controller và retargeting, TWIST là mẫu rất rõ vì high-level/low-level được tách bằng Redis và có sim server cụ thể để kiểm thử. Nếu mục tiêu là thu task demonstration dài, có thao tác vật thể và cần operator làm việc lâu, HOMIE gợi ý giao diện ergonomic hơn: pedal giải phóng upper body, exoskeleton arms giảm ambiguity của IK, gloves xử lý hand command.

Trong thực tế, bạn có thể lai hai ý tưởng. Ví dụ: dùng cockpit/operator UI kiểu HOMIE, nhưng publish command vào bus giống TWIST; hoặc dùng TWIST low-level policy runner, nhưng thay high-level motion lib bằng exoskeleton adapter. Điều quan trọng là giữ ranh giới process rõ.

Blueprint: một trạm thu dữ liệu WholeBodyVLA tối thiểu

Một station beginner có thể bắt đầu như sau:

Machine A: operator PC
  - MoCap/GMR hoặc exoskeleton/glove adapter
  - preview viewer
  - command publisher

Machine B: robot/control laptop hoặc G1 onboard
  - low-level policy runner
  - Unitree SDK2/G1 interface
  - robot safety monitor

Machine C: logger/NAS
  - camera recorder
  - Redis/LCM mirror recorder
  - metadata writer

Mỗi episode nên có một manifest:

episode_id: 2026-06-10-g1-cabinet-0007
task: open_cabinet_and_pick_box
operator:
  interface: twist_gmr_mocap
  operator_id: op_02
robot:
  model: unitree_g1
  hands: dex3
  network: ethernet_eno1
policy:
  stack: twist
  low_level: server_low_level_g1_real.py
  checkpoint: twist_general_motion_tracker.pt
streams:
  command: action_mimic_g1
  robot_state: state_body_g1
  hand_state: state_hand_g1
  cameras:
    - head_rgb
    - wrist_left_rgb
    - wrist_right_rgb
timing:
  policy_hz: 50
  camera_hz: 30
quality_flags:
  success: true
  falls: false
  operator_reset: false

Bạn không cần build mọi thứ trong tuần đầu. Nhưng nếu không định nghĩa manifest từ đầu, dataset sẽ trở nên khó dùng sau 100 episode. Với WholeBodyVLA, mục tiêu không chỉ là replay video. Bạn sẽ cần join ảnh, language instruction, operator command, robot proprioception, action output và outcome để train decoder hoặc đánh giá failure.

Checklist an toàn và chất lượng dữ liệu

Trước khi thu dữ liệu:

[ ] Robot có vùng ngã an toàn, người đứng ngoài vùng quét tay/chân.
[ ] Có nút dừng hoặc remote stop được test thật.
[ ] Low-level controller chạy được trong sim với cùng command.
[ ] Logger không làm chậm policy loop.
[ ] Đồng hồ các máy được đồng bộ hoặc có timestamp monotonic riêng.
[ ] Episode manifest được ghi trước khi robot bắt đầu di chuyển.

Trong lúc thu:

[ ] Command bus có dữ liệu đều, không mất frame dài.
[ ] Robot state publish đều ở tần số mong muốn.
[ ] Camera không drop quá nhiều frame.
[ ] Operator reset/default events được đánh dấu.
[ ] Mọi lỗi network/interface được ghi vào metadata.

Sau khi thu:

[ ] Replay được command và state theo timeline.
[ ] Tách được episode success/failure/reset.
[ ] So sánh được operator command với policy output.
[ ] Có thể chuyển dataset sang format training mà không đoán tên field.

Kết luận

TWIST và HOMIE không chỉ là hai demo teleop đẹp. Chúng là hai mẫu kiến trúc cho data collection humanoid. TWIST dạy ta cách tách motion/high-level command khỏi low-level RL policy bằng Redis, kiểm thử trước trong MuJoCo, rồi đưa cùng logic sang Unitree G1 thật. HOMIE dạy ta cách nghĩ về cockpit operator: exoskeleton arms, gloves, pedal, policy runner ONNX, LCM và Unitree SDK2 phối hợp thành một hệ teleop loco-manipulation thực dụng.

Nếu bạn đang xây dữ liệu cho WholeBodyVLA, bài học chính là: đừng trộn operator command, robot state, policy inference và deployment network vào một khối mờ. Tách chúng ra, log từng dòng có timestamp, có runbook an toàn, và luôn có đường sim hoặc preview trước khi robot thật nhận command. Khi đó dữ liệu teleop không chỉ giúp robot làm được một demo, mà còn trở thành nền để train decoder, fine-tune VLA, đánh giá LMO và phân tích lỗi sim-to-real ở các bài tiếp theo.

Bài viết liên quan

• Bản đồ pipeline WholeBodyVLA
• Retarget AMASS/GMR sang robot
• RL và LMO