Debug MPC/WBID G1 bằng PlotJuggler

Vì sao bài 3 chuyển sang PlotJuggler?

Ở bài 1, ta đã nhìn Unitree G1 ở tầng low-level: MuJoCo, DDS, LowCmd, LowState, domain và interface. Ở bài 2, ta đổi một motion G1 thành MCAP để phát lại kinematics trong Foxglove. Hai bước đó giúp bạn hiểu robot đang có hình dạng và dữ liệu gì, nhưng khi bắt đầu debug controller thì một câu hỏi khác xuất hiện: tín hiệu số đang đi sai ở đâu?

Foxglove rất mạnh cho 3D, TF, camera, point cloud và replay log. PlotJuggler lại cực kỳ hợp với việc nhìn nhiều đường signal theo thời gian: CoM x/y/z, vận tốc, roll/pitch/yaw, contact force, active contact, foot reference và thời gian solve QP. Khi một humanoid ngã, chỉ xem viewer MuJoCo thường không đủ. Bạn cần nhìn xem MPC dự đoán CoM đi đâu, trạng thái SRBD hiện tại lệch bao nhiêu, chân swing có reference hợp lý không, contact nào đang active và WBID có bắt đầu chậm dần không.

Bài này dùng repo ioloizou/g1_locomotion, một locomotion stack cho Unitree G1 kết hợp Single Rigid Body Dynamics (SRBD), Model Predictive Control (MPC) và Whole-Body Inverse Dynamics (WBID) trong kiến trúc cascaded. README của repo hướng dẫn chạy:

roslaunch g1_mujoco_sim mpc_wbid_simulation.launch

Sau đó mở PlotJuggler ở terminal khác:

rosrun plotjuggler plotjuggler

Rồi nạp layout có sẵn:

g1_mujoco_sim/config/MPC_QP_layout.xml

Sau bài này, bạn sẽ biết đọc các message g1_msgs/SRBD_state.msg, g1_msgs/State.msg, g1_msgs/Feet_reference.msg, theo dõi các topic /srbd_current, /mpc_solution, /feet_ref_pos, /wbid_statistics, và dùng wbid_solve_time như một cảnh báo sớm khi QP bắt đầu quá chậm cho vòng mô phỏng.

Roadmap series

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

Nếu bạn chưa quen MuJoCo, đọc thêm Bắt đầu với MuJoCo. Nếu bạn quan tâm sim-to-real trên G1 ở tầng policy, bài GR00T-VisualSim2Real cho G1 cho bức tranh rộng hơn.

Stack này chạy gì bên dưới?

Launch file mpc_wbid_simulation.launch không chỉ mở một viewer. Nó chạy node mô phỏng ros_run_simulation.py, node mpc_to_wbid_node, nạp robot_description từ g1_description/g1_23dof.urdf, bật robot_state_publisher ở 250 Hz và mở RViz với cấu hình SRBD/full-body. Vì vậy có ba luồng dữ liệu chính:

MuJoCo state
  -> SRBD current state publisher
  -> /srbd_current

MPC node
  -> predicted SRBD horizon and contact plan
  -> /mpc_solution

WBID loop
  -> QP inverse dynamics, torque command, solve time
  -> /wbid_statistics/full

MPC ở đây không trực tiếp điều khiển mọi joint. Nó tối ưu ở tầng SRBD: torso orientation, center of mass, velocity, contact point và contact force. WBID nhận trạng thái hiện tại, nghiệm MPC và reference chân, sau đó giải QP whole-body để tạo inverse dynamics torque. Trong source ros_run_simulation.py, đoạn WBID gọi WBID.stack.update(), WBID.setReference(...), WBID.solveQP(), rồi lấy torque bằng WBID.getInverseDynamics(). Ngay sau đó, code cập nhật self.wbid_solve_time và publish qua pal_statistics registry tên /wbid_statistics.

Điểm beginner cần nhớ: nếu robot ngã, chưa chắc lỗi nằm ở MuJoCo. Có thể MPC horizon đã lệch, contact schedule sai, feet reference nhảy, hoặc WBID vẫn đúng về toán nhưng giải QP quá chậm so với timestep. PlotJuggler giúp bạn tách các khả năng này.

Cài và chạy từ repo

README của repo gợi ý dùng Docker image opensot từ branch g1-locomotion của hucebot/opensot_docker. Sau khi vào container, bạn build các ROS package trong workspace g1_locomotion, checkout branch walking-demo nếu đang bám đúng hướng dẫn README, rồi source workspace:

cd g1_locomotion
git checkout walking-demo
cd g1_mpc && git checkout walking-demo
cd .. && make all
cd ../../ && source setup.bash

Nếu bạn đã có workspace riêng, yêu cầu tối thiểu là ROS thấy được package g1_mujoco_sim, g1_msgs, g1_description, g1_mpc, pal_statistics và PlotJuggler. Kiểm tra nhanh:

rospack find g1_mujoco_sim
rospack find g1_msgs
rosmsg show g1_msgs/SRBD_state
which plotjuggler || true

Nếu rosrun plotjuggler plotjuggler không chạy, container của bạn có thể thiếu package PlotJuggler. Với ROS Noetic, thường cần cài ros-noetic-plotjuggler và plugin ROS tương ứng. Trong môi trường Docker khóa sẵn, ưu tiên làm theo Dockerfile của repo trước, vì version ROS, XBot/OpenSoT và MuJoCo binding phải khớp.

Chạy simulation ở terminal 1:

source setup.bash
roslaunch g1_mujoco_sim mpc_wbid_simulation.launch

Đợi RViz và MuJoCo viewer mở. README nói thí nghiệm straight-line walking sẽ chạy vài bước rồi dừng. Ở terminal 2, cũng trong container và cũng đã source workspace:

source setup.bash
rosrun plotjuggler plotjuggler

Trong PlotJuggler, nạp layout:

File -> Load layout
g1_mujoco_sim/config/MPC_QP_layout.xml

Nếu layout hiện panel nhưng chưa có data, mở nguồn streaming ROS topic trong PlotJuggler, chọn các topic mà layout cần, rồi start streaming. Layout XML đã lưu danh sách topic:

/feet_ref_pos
/mpc_solution
/mpc_statistics/full
/srbd_current
/wbid_statistics/full

Bạn cũng có thể kiểm tra topic từ terminal:

rostopic list | grep -E "srbd|mpc|feet|wbid"
rostopic hz /srbd_current
rostopic hz /mpc_solution
rostopic hz /wbid_statistics/full

Nếu rostopic hz đứng im, PlotJuggler không phải lỗi chính. Hãy kiểm tra launch, workspace source, ROS master và node MPC/WBID trước.

Ba message cần hiểu trước khi nhìn đồ thị

Message quan trọng nhất là g1_msgs/SRBD_state.msg:

std_msgs/Header header
State[] states_horizon
ContactPoint[] contacts
geometry_msgs/Point landing_position

Nó chứa một horizon trạng thái SRBD, danh sách contact point và landing position của chân swing. Cùng một type được dùng cho /srbd_current và /mpc_solution, nhưng ý nghĩa khác nhau:

State.msg là phần tử bên trong states_horizon:

int32 trajectory_index
geometry_msgs/Vector3 orientation
geometry_msgs/Vector3 position
geometry_msgs/Vector3 angular_velocity
geometry_msgs/Vector3 linear_velocity
float32 gravity

orientation là roll, pitch, yaw của torso theo radian. position là CoM x/y/z theo mét. angular_velocity là vận tốc góc torso theo rad/s. linear_velocity là vận tốc tuyến tính CoM theo m/s. Khi bạn nhìn tab States, phần lớn curve chính là các field này.

Feet_reference.msg đơn giản hơn:

std_msgs/Header header
ContactPoint[] feet_positions

Mỗi ContactPoint có name, position, force và active. Với /feet_ref_pos, field quan trọng nhất là feet_positions[i]/position/*. Nó cho biết chân swing hoặc contact target đang được planner/WBID đặt ở đâu. Nếu reference chân nhảy đột ngột, robot có thể đá vào không khí hoặc đặt chân sai dù CoM plot nhìn có vẻ ổn.

Layout MPC_QP_layout.xml có gì?

Layout có sẵn không phải chỉ là một file trang trí. Nó encode một workflow debug rất hợp lý. Các tab chính gồm:

Trong tab States, layout dùng states_horizon[1] cho /mpc_solution và states_horizon[0] cho /srbd_current. Đây là một lựa chọn thực dụng: current state chỉ cần phần tử hiện tại, còn MPC solution dùng phần tử kế tiếp trong horizon để so sánh với trạng thái mà WBID sắp đuổi theo. Nếu bạn đổi horizon indexing trong MPC node, hãy sửa layout theo code của bạn.

Một lỗi rất phổ biến là nhìn một curve lẻ rồi kết luận controller sai. Với MPC/WBID, phải đọc theo cặp:

CoM position desired/current
CoM velocity desired/current
Torso orientation desired/current
Torso angular velocity desired/current
Contact active planned/current
Foot reference/current contact position
Solve time vs control timestep

Nếu chỉ CoM x lệch nhưng contact active và foot reference vẫn hợp lý, có thể MPC đang cho phép trôi tạm trong phase chuyển chân. Nếu contact active đổi sai, CoM lệch chỉ là hậu quả. Nếu solve time tăng đều trước khi robot ngã, vấn đề có thể là timing hoặc QP conditioning chứ không phải reference.

Cách đọc tab States

Bắt đầu với States, vì đây là nơi dễ hiểu nhất. Bạn sẽ thấy bốn nhóm lớn: position, linear velocity, orientation và angular velocity. Với humanoid đi thẳng, các kỳ vọng cơ bản là:

Khi mới debug, đừng bật tất cả curve cùng lúc. Hãy chọn một cửa sổ 5 đến 10 giây, zoom vào đoạn robot bắt đầu mất ổn định, rồi so sánh /mpc_solution/states_horizon[1]/position/x với /srbd_current/states_horizon[0]/position/x. Nếu MPC dự đoán một đường mượt nhưng current state không theo, hãy nghi WBID, contact hoặc torque saturation. Nếu MPC solution đã có spike, hãy nghi MPC cost, constraint, state estimate hoặc reference input.

Một mẹo đơn giản: dùng vertical tracker của PlotJuggler để đọc cùng timestamp trên nhiều plot. Khi CoM y bắt đầu trôi, xem ngay contact active tại timestamp đó. Nếu contact active chuyển phase đúng lúc nhưng force z chưa lên, khả năng chân vừa được planner coi là chống nhưng trong mô phỏng chưa thật sự gánh lực.

Cách đọc contact force và contact active

Contact Forces trong layout vẽ /mpc_solution/contacts[0..3]/force/x/y/z. Repo dùng bốn contact point để mô tả hai bàn chân. Trong code mô phỏng, left foot contact task active nếu contact point 0 hoặc 1 active, right foot contact task active nếu contact point 2 hoặc 3 active. Vì vậy đừng đọc từng point như một bàn chân độc lập. Hãy đọc theo cặp:

Left foot  = contacts[0], contacts[1]
Right foot = contacts[2], contacts[3]

Khi double support, bạn thường kỳ vọng lực z được chia giữa hai chân, tùy trạng thái CoM. Khi swing một chân, chân stance phải gánh phần lớn lực. Nếu contact active báo chân phải đang stance nhưng force z trên contact phải gần như 0, có thể contact state, frame hoặc constraint đang không khớp.

Tab Contact Active vẽ cả planned active từ /mpc_solution và current active từ /srbd_current. Đây là tab rất đáng xem trước khi tune gain. Nếu schedule sai, tăng PD hay chỉnh QP weight chỉ che triệu chứng. Ví dụ:

Với beginner, mục tiêu đầu tiên không phải có plot đẹp. Mục tiêu là contact active và force kể cùng một câu chuyện. Nếu active = true nhưng lực bằng 0 quá lâu, hoặc active = false nhưng force z rất lớn, bạn cần debug contact trước khi đụng tới MPC cost.

Cách đọc /feet_ref_pos

/feet_ref_pos publish Feet_reference, trong đó feet_positions là danh sách ContactPoint. Layout mặc định vẽ feet_positions[0]/position/x/y/z cùng với contact position hiện tại. Đây là chỗ bạn phát hiện lỗi swing foot rất nhanh.

Một foot reference khỏe thường có ba đặc điểm:

1. x/y thay đổi mượt theo bước, không nhảy xa vô lý.
2. z nâng lên khi swing và về gần mặt đất khi landing.
3. landing position nhất quán với hướng di chuyển của CoM.

Nếu z reference không nâng nhưng contact active lại tắt, chân có thể kéo lê. Nếu z reference nâng quá cao hoặc hạ quá trễ, WBID phải tạo chuyển động chân khó, QP solve time có thể tăng hoặc torque yêu cầu vượt khả năng. Nếu x/y reference nhảy một bậc lớn, hãy kiểm tra gait procedure, landing position và đơn vị tọa độ. Trong robotics, rất nhiều lỗi "controller bất ổn" thật ra là lỗi reference không liên tục.

Bạn có thể tự thêm curve vào PlotJuggler:

/feet_ref_pos/feet_positions[0]/position/x
/feet_ref_pos/feet_positions[0]/position/y
/feet_ref_pos/feet_positions[0]/position/z
/srbd_current/contacts[0]/position/x
/srbd_current/contacts[0]/position/y
/srbd_current/contacts[0]/position/z

Sau đó đổi sang cùng một plot để nhìn tracking. Nếu index 0 không phải chân bạn đang quan tâm trong phiên bản code của bạn, dùng rostopic echo /feet_ref_pos -n 1 để xem name của contact point trước.

wbid_solve_time: cảnh báo sớm khi QP chậm

Trong source ros_run_simulation.py, WBID đăng ký một statistic:

self.registry = StatisticsRegistry("/wbid_statistics")
self.wbid_solve_time = 0.0
self.registry.registerFunction("wbid_solve_time", (lambda: self.wbid_solve_time))

Sau mỗi vòng sim_step, code gọi WBID.solveQP(), lấy inverse dynamics, cập nhật self.wbid_solve_time, rồi self.registry.publish(). Layout PlotJuggler đọc field đầu tiên:

/wbid_statistics/full/statistics[0]/value

Trong tab Solving Times, nó được vẽ cùng với một statistic từ MPC:

/mpc_statistics/full/statistics[4]/value
/wbid_statistics/full/statistics[0]/value

Bạn không cần biết ngay toàn bộ schema của pal_statistics để dùng được plot này. Hãy bắt đầu bằng cách xác nhận tên statistic:

rostopic echo /wbid_statistics/full -n 1

Tìm entry có name gần wbid_solve_time, rồi xem value. Nếu layout của bạn đã map đúng, đường /wbid_statistics/full/statistics[0]/value chính là thời gian solve WBID QP.

Cách đọc thực dụng:

Đừng chỉ nhìn giá trị trung bình. Với walking controller, spike đúng lúc đổi contact phase nguy hiểm hơn một mean đẹp. Hãy zoom vào đoạn Contact Active đổi trạng thái và xem solve time có spike cùng timestamp không. Nếu có, bạn đã có một hướng debug rõ ràng: contact transition làm QP khó.

Checklist debug khi layout không có data

Nếu PlotJuggler mở nhưng không hiện curve, đi theo checklist này:

# 1. ROS master còn sống?
rostopic list

# 2. Workspace đã source đúng?
rospack find g1_mujoco_sim
rospack find g1_msgs

# 3. Simulation publish topic chưa?
rostopic list | grep -E "srbd_current|mpc_solution|feet_ref_pos|wbid_statistics"

# 4. Message có ra không?
rostopic echo /srbd_current -n 1
rostopic echo /wbid_statistics/full -n 1

# 5. Tần số có hợp lý không?
rostopic hz /srbd_current
rostopic hz /wbid_statistics/full

Nếu /srbd_current có data nhưng PlotJuggler không thấy, lỗi thường nằm ở plugin ROS streaming hoặc layout chưa subscribe topic. Nếu /mpc_solution không có data, kiểm tra node mpc_to_wbid_node. Nếu /wbid_statistics/full không có data, kiểm tra pal_statistics dependency và đoạn registry trong node mô phỏng. Nếu mọi topic đều có data nhưng curve trong layout vẫn rỗng, có thể message path trong layout không khớp version message hiện tại. Khi đó kéo topic từ cây dữ liệu bên trái vào plot thủ công để xem tên field thật.

Một workflow debug 10 phút

Khi robot ngã hoặc bước không ổn, làm theo thứ tự này:

1. Mở States, zoom vào 2 giây trước lúc ngã. So sánh CoM current và MPC.
2. Mở Contact Active tại cùng timestamp. Xem planned và current contact có lệch không.
3. Mở Contact Forces. Xem force z của chân stance có rơi về 0 hoặc spike không.
4. Mở Contact Positions. So sánh foot reference với contact position hiện tại.
5. Mở Solving Times. Xem wbid_solve_time có spike đúng lúc chuyển phase không.
6. Quay lại terminal và echo đúng topic nghi ngờ để xác nhận không phải lỗi layout.

Sau 10 phút, bạn thường sẽ phân loại được lỗi vào một trong bốn nhóm:

Đây là lý do PlotJuggler đáng dùng ngay từ đầu. Nó buộc bạn debug theo bằng chứng, không theo cảm giác từ viewer.

Nguồn kỹ thuật đã dùng

Các điểm kỹ thuật trong bài này được đối chiếu từ README và source của ioloizou/g1_locomotion, đặc biệt là g1_mujoco_sim/launch/mpc_wbid_simulation.launch, g1_mujoco_sim/config/MPC_QP_layout.xml, g1_mujoco_sim/src/ros_run_simulation.py và các message trong g1_msgs/msg/. Với PlotJuggler, bạn nên ưu tiên tài liệu qua ROS Index package plotjuggler và repository chính thức PlotJuggler/plotjuggler, thay vì các domain không rõ nguồn.

Bài viết liên quan

• Dựng G1 MuJoCo qua DDS low-level
• Biến LAFAN1 G1 thành MCAP Foxglove
• WBID và PD cho G1 trong MuJoCo