Visualize 23 khớp G1: MCAP bag replay & Layout XML

Trong bài trước, chúng ta đã cài đặt PlotJuggler và kết nối ROS2 subscriber để stream dữ liệu live từ G1. Tuy nhiên, dữ liệu live chỉ có ích khi robot đang chạy. Khi bạn muốn phân tích lại một lần chạy đã xảy ra — chẳng hạn robot bị ngã lúc 14:03 và bạn muốn biết tại sao — bạn cần file bag đã ghi lại.

Và format bag tốt nhất cho ROS2 ngày nay là .mcap: tự chứa schema, hỗ trợ seek nhanh, và PlotJuggler đọc được trực tiếp qua plugin DataLoadMCAP.

Bài này hướng dẫn toàn bộ pipeline:

1. Thu thập dữ liệu G1 vào file .mcap
2. Mở file với plugin DataLoadMCAP trong PlotJuggler
3. Kéo thả toàn bộ 23 khớp (q, dq, tau) lên grid multi-panel đồng bộ
4. Lưu layout thành file .xml để tái dùng không giới hạn lần
5. Vẽ phase portrait (góc khớp vs vận tốc) bằng chế độ XY Scatter

Tại sao cần visualize cả 23 khớp cùng lúc?

G1 có 23 bậc tự do (DOF). Mỗi khớp phát ra 3 kênh số liệu tại tần số điều khiển 500 Hz:

Tổng cộng: 23 × 3 = 69 kênh mỗi timestep. Nếu bạn phải click từng kênh một để kéo vào plot, bạn cần 69 thao tác. Layout XML giải quyết vấn đề này: sau khi setup một lần, mọi session tiếp theo chỉ cần hai click — File → Load Layout.

Nhưng quan trọng hơn: xem tất cả các khớp đồng thời mới thấy được pattern. Ví dụ, khớp cổ tay trái bắt đầu rung trước 200ms khi robot mất thăng bằng — điều bạn sẽ bỏ lỡ hoàn toàn nếu chỉ nhìn vào chân.

Layout khớp G1 (23 DOF)

Trước khi bắt đầu, hãy nắm chắc bảng ánh xạ chỉ số khớp. Đây là cấu trúc 23-DOF variant của G1 (phiên bản phổ biến nhất — waist giản lược, wrist đơn giản):

Chân phải (Right Leg) — indices 0–5:
  [0] hip_pitch      [1] hip_roll       [2] hip_yaw
  [3] knee_pitch     [4] ankle_pitch    [5] ankle_roll

Chân trái (Left Leg) — indices 6–11:
  [6] hip_pitch      [7] hip_roll       [8] hip_yaw
  [9] knee_pitch    [10] ankle_pitch   [11] ankle_roll

Lưng (Waist) — indices 12–14:
  [12] waist_yaw    [13] waist_roll    [14] waist_pitch

Tay phải (Right Arm) — indices 15–18:
  [15] shoulder_pitch  [16] shoulder_roll
  [17] elbow_pitch     [18] wrist_roll

Tay trái (Left Arm) — indices 19–22:
  [19] shoulder_pitch  [20] shoulder_roll
  [21] elbow_pitch     [22] wrist_roll

Trong PlotJuggler, sau khi load MCAP, 69 kênh xuất hiện dưới cấu trúc cây:

/lowstate/motor_state[0]/q    ... /lowstate/motor_state[22]/q
/lowstate/motor_state[0]/dq   ... /lowstate/motor_state[22]/dq
/lowstate/motor_state[0]/tau  ... /lowstate/motor_state[22]/tau

Phần 1 — Xác nhận plugin DataLoadMCAP đã sẵn sàng

Plugin DataLoadMCAP được bundled sẵn từ PlotJuggler 3.6.0 trở lên. Kiểm tra:

# Kiểm tra phiên bản
ros2 run plotjuggler plotjuggler --version
# Cần >= 3.6.0

# Khởi động PlotJuggler
source /opt/ros/humble/setup.bash
ros2 run plotjuggler plotjuggler &

# Vào App → Preferences → Loaded Plugins
# Tìm "DataLoadMCAP" trong danh sách

Nếu build từ source, plugin nằm tại plotjuggler_plugins/DataLoadMCAP/. Đảm bảo build với MCAP support:

cd ~/ws_plotjuggler
colcon build --cmake-args -DCOMPILE_WITH_ROSBAG2=ON

Nếu cài qua APT hoặc Snap, plugin đã có sẵn — không cần làm thêm gì.

Phần 2 — Ghi dữ liệu G1 vào file .mcap

Bước 2.1 — Cài storage plugin

MCAP không phải format mặc định của ROS2 (mặc định là SQLite .db3). Cần cài thêm:

sudo apt install ros-$ROS_DISTRO-rosbag2-storage-mcap
# Ví dụ: ros-humble-rosbag2-storage-mcap

Verify:

ros2 bag record --help | grep storage
# Sẽ liệt kê các storage backends có sẵn, bao gồm "mcap"

Bước 2.2 — Ghi bag từ G1

Trên máy đã kết nối G1 (DDS domain phải khớp — xem bài 1 để cấu hình CycloneDDS):

# Tạo thư mục lưu trữ bag
mkdir -p ~/g1_bags

# Bắt đầu ghi — tên file có timestamp để dễ phân biệt session
ros2 bag record \
  --storage mcap \
  --output ~/g1_bags/g1_walk_$(date +%Y%m%d_%H%M%S) \
  /lowstate \
  /imu_state \
  /tf \
  /tf_static

# Nhấn Ctrl+C khi muốn dừng ghi

Lưu ý về dung lượng: Ở 500 Hz, /lowstate tạo khoảng 2–3 MB/giây. Một session 5 phút ≈ 900 MB. Đặt --max-bag-size 1073741824 (1 GB) để tự động split file khi quá lớn.

Đặt tên có nghĩa: g1_fall_20260615_143000 rõ hơn test_bag_3. Ba tháng sau bạn vẫn biết đây là session ghi lúc robot ngã lúc 14:30.

Bước 2.3 — Verify file bag

ros2 bag info ~/g1_bags/g1_walk_20260615_143000/

# Output mẫu:
# Files:         g1_walk_20260615_143000_0.mcap
# Duration:      30.512s
# Start:         Jun 15 2026 14:30:00.142 (1750007400.142)
# End:           Jun 15 2026 14:30:30.654 (1750007430.654)
# Messages:      91536
# Topic information:
#   Topic: /lowstate   | Type: unitree_hg/msg/LowState | Count: 15256 | Freq: 500.0
#   Topic: /imu_state  | Type: sensor_msgs/msg/Imu     | Count: 15256 | Freq: 500.0

Nếu Count của /lowstate ≈ Duration × 500 → bag ghi đầy đủ, không bị drop frame.

Phần 3 — Load file .mcap vào PlotJuggler

Bước 3.1 — Mở file

1. Trên toolbar chính, click "Data" → "Load Data File" (hoặc Ctrl+O)

2. File dialog xuất hiện. Navigate đến thư mục bag và chọn file .mcap:

   ~/g1_bags/g1_walk_20260615_143000/g1_walk_20260615_143000_0.mcap
   

3. PlotJuggler tự nhận diện plugin phù hợp dựa trên extension. Nếu có dialog hỏi plugin, chọn "DataLoadMCAP".

Bước 3.2 — Dialog cấu hình DataLoadMCAP

Trước khi parse, plugin hiện dialog cấu hình:

• Time reference (radio button):

  • logTime: Thời điểm message được ghi vào file MCAP. Dùng option này khi phân tích offline.
  • publishTime: Thời điểm message được publish trên ROS2 topic. Có thể bị lệch nếu mạng có latency.
  • Khuyến nghị cho G1: chọn logTime.

• Topic filter: Ô search để lọc topic cần load. Gõ lowstate nếu bạn chỉ cần joint data, không cần IMU hay TF (load nhanh hơn).

• Max array size: Giữ nguyên mặc định — G1 chỉ có 23 phần tử motor_state, jauh dưới bất kỳ giới hạn nào.

Click OK. PlotJuggler parse file và hiện progress bar thực. Với bag 30 giây, quá trình này mất khoảng 2–5 giây.

Bước 3.3 — Kết quả sau khi load

Panel "Timeseries List" bên trái sẽ mở rộng ra 200+ kênh. Cấu trúc cây:

▶ /lowstate
  ▶ motor_state
    ▶ [0]
      ├── q          ← vị trí khớp 0 (hip_pitch phải)
      ├── dq         ← vận tốc khớp 0
      ├── tau        ← moment xoắn khớp 0
      ├── temperature
      └── lost
    ▶ [1]
      ...
    ▶ [22]           ← khớp 22 (wrist_roll trái)

Mẹo lọc nhanh: Dùng ô search phía trên Timeseries List. Gõ /q để lọc ra chỉ 23 kênh position. Gõ motor_state\[0 để xem toàn bộ fields của riêng khớp 0.

Phần 4 — Tạo multi-panel layout cho 23 khớp

Layout multi-panel: nhiều plot chia sẻ cùng timeline và time cursor — nguồn: repo facontidavide/PlotJuggler

Mục tiêu: tạo layout 3 panel lớn (vị trí / vận tốc / moment xoắn) hiển thị đồng thời toàn bộ 23 khớp.

Bước 4.1 — Tạo 3 panel

1. Chuột phải vào vùng plot chính → "Split Horizontally" → Panel chia thành 2 hàng.
2. Chuột phải vào panel dưới → "Split Horizontally" lần nữa → Giờ có 3 hàng.
3. Đặt tên từng panel bằng cách click vào title bar của panel:
   • Panel trên: Position (q) [rad]
   • Panel giữa: Velocity (dq) [rad/s]
   • Panel dưới: Torque (tau) [N·m]

Bước 4.2 — Kéo thả theo nhóm

Điền panel Position:

1. Trong Timeseries List, dùng filter gõ: /lowstate/motor_state.*\]/q
2. Nhấn Ctrl+A để chọn tất cả 23 kênh q được lọc
3. Kéo thả vào Panel trên (Position)

Điền panel Velocity:

1. Xóa filter, gõ lại: /lowstate/motor_state.*\]/dq
2. Ctrl+A → Kéo vào Panel giữa (Velocity)

Điền panel Torque:

1. Filter: /lowstate/motor_state.*\]/tau
2. Ctrl+A → Kéo vào Panel dưới (Torque)

Kết quả: mỗi panel hiển thị 23 đường với màu tự động theo index khớp. PlotJuggler dùng màu HSV cycling nên khớp 0 và khớp 22 sẽ có màu gần giống nhau — nhưng bạn có thể hover để xem tên kênh trong tooltip.

Layout thay thế: Chia theo bộ phận cơ thể

Nếu bạn debug một chi cụ thể, layout theo bộ phận rõ hơn:

Tab "Chân" (3 panel):
  ├── q[0-5]   vs   q[6-11]   (phải vs trái — đối chiếu gait)
  ├── dq[0-5]  vs  dq[6-11]
  └── tau[0-5] vs tau[6-11]

Tab "Thân + Tay" (2 panel):
  ├── q[12-14], q[15-22]
  └── tau[12-14], tau[15-22]

Tạo tab mới: Click nút "+" bên cạnh tên tab ở góc dưới cùng của layout area.

Phần 5 — Time cursor đồng bộ giữa các panel

Đây là tính năng làm cho multi-panel thực sự có giá trị: tất cả panel cập nhật đồng bộ khi bạn di chuyển con trỏ thời gian.

Điều hướng timeline:

• Kéo thanh timeline ở đáy màn hình: tất cả panel scroll đồng bộ
• Scroll chuột trên bất kỳ panel: zoom trục thời gian
• Ctrl + scroll: zoom chỉ trục Y của panel đó

Kết nối zoom giữa panels:

Click nút "Link" (biểu tượng xích mắt) trên toolbar → Khi bạn zoom vào khoảng thời gian trên panel Position, các panel Velocity và Torque cũng zoom về cùng khoảng đó. Cực kỳ hữu ích khi xem xét một sự kiện cụ thể.

Đặt vertical marker:

Chuột phải trên panel → "Add Vertical Line" → Nhập timestamp

Ví dụ: Đặt marker tại t = 14.340 giây — thời điểm robot ngã. Marker sẽ xuất hiện trên tất cả panels. Bạn có thể scroll ngược về t = 13.8 để xem pattern 500ms trước khi ngã.

Ví dụ thực tế từ debugging G1:

Marker đặt tại t = 14.340. Nhìn vào panel Torque: khớp [4] (ankle_pitch phải) bị saturate ở ±18 N·m từ t = 13.85 — tức là 490ms trước khi ngã. Nhìn panel Position cùng lúc: q[4] đang ở góc -0.35 rad (đã gần joint limit). Controller đang "chiến đấu" với mặt đất bằng maximum torque nhưng vẫn không đủ. Nguyên nhân: terrain có độ dốc mà motion planner không tính đến.

Phần 6 — Lưu và tải lại Layout XML

Lưu layout

Sau khi setup xong, lưu ngay trước khi đóng PlotJuggler:

File → Save Layout    (hoặc Ctrl+S)

Chọn tên file mô tả rõ ràng:

g1_23joints_q_dq_tau_v1.xml

Tổ chức theo thư mục:

mkdir -p ~/plotjuggler_layouts/g1/

# Lưu layout chính
~/plotjuggler_layouts/g1/g1_23joints_q_dq_tau_v1.xml

# Lưu layout theo bộ phận
~/plotjuggler_layouts/g1/g1_legs_comparison.xml
~/plotjuggler_layouts/g1/g1_arms_only.xml

File XML chứa gì:

<!-- Ví dụ cấu trúc XML layout PlotJuggler -->
<PlotJuggler version="3.8">
  <CurrentPlots>
    <Tab name="Main">
      <Row>
        <Plot name="Position (q) [rad]">
          <Curve name="/lowstate/motor_state[0]/q" color="#E8544A"/>
          <Curve name="/lowstate/motor_state[1]/q" color="#5B9BD5"/>
          <!-- ... 21 curves nữa ... -->
        </Plot>
      </Row>
    </Tab>
  </CurrentPlots>
</PlotJuggler>

XML lưu đủ thông tin: số panel, kênh nào trong panel nào, màu sắc từng đường, trạng thái link zoom, tab nào đang active.

Load lại layout

Session debug tiếp theo, workflow chỉ còn 4 bước:

1. ros2 run plotjuggler plotjuggler
2. File → Load Data File → chọn .mcap mới
3. File → Load Layout → g1_23joints_q_dq_tau_v1.xml
4. Phân tích ngay

PlotJuggler tự động map tên kênh trong XML vào dữ liệu mới load. Điều kiện duy nhất: tên kênh trong file bag mới phải giống với tên đã lưu trong XML (đảm bảo nếu bạn dùng cùng driver G1 và topic names).

Chia sẻ layout trong team

Layout XML là text thuần, dễ dàng commit vào Git:

cd ~/workspace/g1_project/
mkdir -p plotjuggler_layouts/

cp ~/plotjuggler_layouts/g1/*.xml plotjuggler_layouts/

git add plotjuggler_layouts/
git commit -m "feat: PlotJuggler layouts cho debug 23 khớp G1"
git push

Toàn bộ team share cùng layout — không ai phải setup lại từ đầu. Khi bạn thêm panel mới hoặc phát hiện cách nhìn tốt hơn, commit version mới và mọi người cùng được hưởng.

Phần 7 — XY Scatter: Vẽ Phase Portrait

Phase portrait là đồ thị vị trí khớp (q) trên trục X và vận tốc (dq) trên trục Y. Hình dạng đường cong tiết lộ trạng thái động học của khớp mà time series đơn thuần không thể hiện rõ.

Cách đọc phase portrait của robot

Các bước tạo XY Scatter

1. Trong Timeseries List, click vào /lowstate/motor_state[3]/q (ví dụ: knee phải)

2. Giữ Ctrl + click /lowstate/motor_state[3]/dq

3. Giữ chuột phải và kéo cả hai kênh vào panel trống

4. PlotJuggler hiện dialog → Chọn "XY Scatter"

5. Nhấn Play (▶) để xem quỹ đạo phase portrait animate theo thời gian

Ví dụ: Debug knee joint trong walking gait

Chọn khớp [3] (right knee_pitch) và [9] (left knee_pitch) để so sánh song song:

• Phase portrait phải (khớp 3): Ellipse rộng, đối xứng — healthy
• Phase portrait trái (khớp 9): Ellipse bị bẹp, góc nhỏ hơn — knee trái flex ít hơn phải

Kết luận: Bất đối xứng gait do knee trái có PD gain khác hoặc joint friction cao hơn. Bước tiếp theo: so sánh tau[3] vs tau[9] — nếu torque phải cao hơn để đạt cùng vị trí thì do friction; nếu torque bằng nhau thì do controller parameter không đồng đều.

Tổng kết: Workflow debug hoàn chỉnh

Sau bài này, pipeline của bạn là:

# 1. Ghi bag (trên máy kết nối G1)
ros2 bag record --storage mcap \
  -o ~/g1_bags/g1_$(date +%Y%m%d_%H%M%S) \
  /lowstate /imu_state

# 2. Mở PlotJuggler
ros2 run plotjuggler plotjuggler

# 3. Load bag + layout
# File → Load Data File → chọn .mcap
# File → Load Layout → g1_23joints_q_dq_tau_v1.xml

# 4. Tìm thời điểm bất thường
# Scroll timeline, đặt marker tại sự kiện

# 5. Phân tích phase portrait cho khớp nghi vấn
# Ctrl+click q + dq → kéo chuột phải → XY Scatter → Play

Bạn vừa biến một đống 69 kênh số liệu thành một dashboard debug tái dùng được chỉ với một file XML. Trong bài tiếp theo, chúng ta sẽ phân tích dữ liệu IMU: chuyển đổi quaternion sang Euler angle bằng Lua script và dùng FFT để tìm tần số rung bất thường trong gait G1.

Bài viết liên quan

• Bài 1 — PlotJuggler + G1: Cài đặt ROS2 và kết nối live
• Bài 3 — IMU + Quaternion + FFT: Tìm rung trong gait G1
• MCAP trong ROS2: Thu thập và pipeline dữ liệu humanoid