IMU Debug G1: Quaternion → Euler + FFT phân tích rung

Hai bài trước chúng ta đã thiết lập được pipeline hoàn chỉnh: PlotJuggler stream live từ G1 (bài 1), và tổ chức layout 23 khớp với ghi MCAP (bài 2). Bây giờ đến phần thú vị hơn — phân tích dữ liệu IMU thực sự.

Robot Unitree G1 có IMU gắn tại pelvis (xương chậu), publish ở 500 Hz. Từ sensor này chúng ta có thể trả lời ba câu hỏi quan trọng: Robot đang nghiêng bao nhiêu độ theo từng trục? Có tần số rung bất thường nào trong khớp không? Chân nào đang chịu lực và chân nào đang swing?

Ba câu hỏi này cần ba công cụ khác nhau trong PlotJuggler: ToolboxQuaternion, ToolboxFFT, và Moving Average transform. Bài này hướng dẫn cả ba.

Roadmap series

#	Bài	Nội dung
1	Cài đặt & kết nối live	Cài PlotJuggler, CycloneDDS, subscribe `/lowstate`
2	Layout 23 khớp + ghi MCAP	Multi-panel layout, file `.mcap`, phase portrait
3	Debug IMU + FFT (bài này)	Quaternion → Euler, FFT phân tích rung, Moving Average foot_force
4	Lua Transforms	Script Lua tính power per joint, derived signal
5	ZMQ + video sim2real	Cầu nối ZMQ từ MuJoCo, overlay camera feed

Cấu trúc IMU của G1 — nhắc lại nhanh

Kênh IMU trong /lowstate gồm:

/lowstate/imu_state/
  ├── quaternion[0]    ← w (phần thực — scalar)
  ├── quaternion[1]    ← x (phần ảo — pitch axis)
  ├── quaternion[2]    ← y (phần ảo — roll axis)
  ├── quaternion[3]    ← z (phần ảo — yaw axis)
  ├── rpy[0]           ← roll  (rad) — từ firmware
  ├── rpy[1]           ← pitch (rad) — từ firmware
  ├── rpy[2]           ← yaw   (rad) — từ firmware
  ├── accelerometer[0] ← ax (m/s²) — trục X body frame
  ├── accelerometer[1] ← ay (m/s²) — trục Y body frame
  ├── accelerometer[2] ← az (m/s²) — trục Z body frame (≈9.81 khi đứng thẳng)
  ├── gyroscope[0]     ← ωx (rad/s) — vận tốc góc quanh trục X
  ├── gyroscope[1]     ← ωy (rad/s) — vận tốc góc quanh trục Y
  └── gyroscope[2]     ← ωz (rad/s) — vận tốc góc quanh trục Z

Câu hỏi tự nhiên: G1 đã có sẵn /lowstate/imu_state/rpy[0..2] — tại sao cần ToolboxQuaternion?

Câu trả lời: rpy từ firmware được tính bởi onboard filter của G1, có thể bị drift hoặc áp dụng convention Euler khác với những gì bạn cần. Quaternion là dạng dữ liệu gốc từ cảm biến — chính xác hơn và cho phép bạn tự chọn convention khi convert sang Euler angles. Khi debug một vấn đề cụ thể (như robot tưởng mình thẳng đứng nhưng thực tế đang nghiêng), so sánh rpy firmware với Euler angles từ quaternion raw là bước đầu tiên để xác định vấn đề nằm ở đâu.

Phần 1: ToolboxQuaternion — Quaternion sang Euler Angles

Tại sao quaternion phức tạp hơn Euler?

Tưởng tượng bạn đang mô tả hướng của một chiếc điện thoại: "Nghiêng 30 độ theo trục dọc, rồi xoay 15 độ theo trục ngang." Đó là cách Euler angles hoạt động — ba góc tuần tự. Trực quan, dễ đọc.

Nhưng máy tính không thích Euler angles. Nếu bạn xoay quá 90 độ theo một trục, thứ tự xoay bị đảo lộn và bạn gặp Gimbal Lock — robot báo 180 độ pitch nhưng roll đột ngột nhảy ±90 độ. Quaternion tránh hoàn toàn vấn đề này bằng cách mô tả hướng như một điểm trên mặt cầu 4 chiều: không có kỳ dị, không có wrap-around bất ngờ.

Khi debug, bạn cần cả hai: quaternion để đảm bảo tính toán đúng, Euler angles để nhìn vào biểu đồ và hiểu ngay robot đang làm gì.

Mở ToolboxQuaternion

Plugin ToolboxQuaternion được bundled sẵn trong plotjuggler_plugins/ToolboxQuaternion/ từ PlotJuggler 3.7 trở lên. Không cần cài thêm gì nếu bạn dùng ros-humble-plotjuggler-ros.

Truy cập từ menu:

Menu PlotJuggler → Tools → Toolboxes → Quaternion To RPY

Hoặc nhìn vào toolbar phía trên cùng — biểu tượng hình hộp công cụ (🧰). Nếu không thấy, kiểm tra:

# Confirm plugin được load
ros2 run plotjuggler plotjuggler --version
# Vào App → Preferences → Loaded Plugins
# Tìm "ToolboxQuaternion" trong danh sách

Mapping kênh quaternion G1 vào Toolbox

Khi cửa sổ Quaternion To RPY Toolbox mở ra, bạn thấy 4 dropdown để gán input:

┌─────────────────────────────────────────────────────┐
│  Quaternion To RPY                                  │
│                                                     │
│  W (scalar):  [/lowstate/imu_state/quaternion[0] ▼] │
│  X:           [/lowstate/imu_state/quaternion[1] ▼] │
│  Y:           [/lowstate/imu_state/quaternion[2] ▼] │
│  Z:           [/lowstate/imu_state/quaternion[3] ▼] │
│                                                     │
│  Suffix: [_imu]                                     │
│                                                     │
│  [Compute]                    [Close]               │
└─────────────────────────────────────────────────────┘

Gán input:

W (scalar): /lowstate/imu_state/quaternion[0] — đây là phần thực (w) trong quaternion G1
X: /lowstate/imu_state/quaternion[1]
Y: /lowstate/imu_state/quaternion[2]
Z: /lowstate/imu_state/quaternion[3]
Suffix: Nhập tên hậu tố cho output, ví dụ _imu_pj để tránh trùng với kênh rpy gốc

Nhấn Compute. PlotJuggler tính toán và thêm 3 kênh mới vào Signal List:

/lowstate/imu_state/quaternion_imu_pj/roll    ← giá trị rad, range [-π, +π]
/lowstate/imu_state/quaternion_imu_pj/pitch   ← range [-π/2, +π/2]
/lowstate/imu_state/quaternion_imu_pj/yaw     ← range [-π, +π]

Công thức chuyển đổi bên trong Toolbox

Toolbox dùng convention ZYX (Tait-Bryan angles) — phổ biến nhất trong robotics và aerospace:

roll  = atan2( 2(w·x + y·z),   1 - 2(x² + y²) )
pitch = arcsin( 2(w·y - z·x) )
yaw   = atan2( 2(w·z + x·y),   1 - 2(y² + z²) )

Với G1 đứng thẳng, chờ đợi:

roll ≈ 0 (không nghiêng sang bên)
pitch ≈ 0 (không nghiêng trước/sau)
yaw thay đổi theo hướng mặt của robot (không quan trọng khi đứng tại chỗ)

Vẽ so sánh: Quaternion Euler vs Firmware RPY

Sau khi compute xong, kéo 6 kênh vào hai panel song song để so sánh:

Panel trên (Quaternion → Euler):
  /lowstate/imu_state/quaternion_imu_pj/roll
  /lowstate/imu_state/quaternion_imu_pj/pitch

Panel dưới (Firmware RPY):
  /lowstate/imu_state/rpy[0]   ← firmware roll
  /lowstate/imu_state/rpy[1]   ← firmware pitch

Trong điều kiện bình thường, hai cặp phải gần như trùng khớp. Nếu chúng lệch nhau:

Triệu chứng	Nguyên nhân có thể
Offset cố định (ví dụ: Euler = firmware + 0.05 rad)	G1's firmware áp dụng calibration offset khác
Lệch chỉ khi robot di chuyển nhanh	Latency trong filter firmware so với raw quaternion
Lệch đột ngột khi quay nhanh	Gimbal lock trong firmware Euler — quaternion không bị ảnh hưởng
Lệch khi robot đứng yên + drift tăng dần	IMU drift chưa được compensate — cần kiểm tra magnetometer hoặc tích hợp EKF

Bật Toolbox cho streaming live

Mặc định, ToolboxQuaternion chỉ tính trên dữ liệu đã load. Để bật chế độ streaming real-time (khi dùng ROS2 Topic Subscriber thay vì file MCAP):

Trong cửa sổ Toolbox → Check "Apply to streaming data" → Compute

Sau đó, khi PlotJuggler đang streaming live từ G1, ba kênh roll/pitch/yaw mới sẽ cập nhật real-time cùng lúc với quaternion gốc.

Phần 2: ToolboxFFT — Phân tích Tần số Rung Động

Analogy để hiểu FFT trong 30 giây

Bạn đang nghe một bản nhạc — âm thanh là sóng dao động phức tạp theo thời gian. FFT (Fast Fourier Transform) là cách "tách" bản nhạc ra thành từng nốt đơn lẻ theo tần số: có bao nhiêu bass (20-200 Hz), bao nhiêu treble (2000-20000 Hz).

Với G1, thay vì âm thanh, chúng ta có tín hiệu gyroscope (rad/s) theo thời gian. FFT tách tín hiệu này thành:

Tần số gait (~1-2 Hz): robot đang bước đi, thân người đung đưa nhẹ
Tần số balance controller (~10-30 Hz): controller đang "giật" liên tục để giữ thẳng
Mechanical resonance của khớp (~20-80 Hz): nếu có peak bất thường ở đây → vấn đề cơ học

Mở ToolboxFFT

Menu PlotJuggler → Tools → Toolboxes → FFT

Cửa sổ FFT Toolbox hiện ra:

┌─────────────────────────────────────────────────────┐
│  FFT Toolbox                                        │
│                                                     │
│  Input signal: [/lowstate/imu_state/gyroscope[2] ▼] │
│                                                     │
│  Time range:   [Use full range]  [Last N seconds: 5]│
│  Window:       [Hann ▼]                             │
│                                                     │
│  Output suffix: [_fft_gyroZ]                        │
│                                                     │
│  [Compute FFT]                [Close]               │
└─────────────────────────────────────────────────────┘

Tham số quan trọng:

Tham số	Gợi ý	Lý do
Window	Hann	Giảm spectral leakage — phù hợp nhất cho tín hiệu robot không tuần hoàn hoàn hảo
Time range	Last 10-30 seconds	Đủ dài để FFT có độ phân giải tần số tốt (Δf = 1/T)
Phân giải tần số	Δf = 1/T	Nếu T = 10s thì Δf = 0.1 Hz — đủ để phân biệt 1 Hz vs 1.1 Hz

Chiến lược áp dụng FFT trên G1

Áp dụng FFT theo thứ tự này để systematic debug:

Bước 1: FFT gyroscope pitch axis (ωy) — quan trọng nhất

Input: /lowstate/imu_state/gyroscope[1]   ← ωy (vận tốc góc sagittal plane)

Đây là trục ảnh hưởng lớn nhất đến balance forward-backward. Trong walking gait bình thường:

Biểu đồ FFT bình thường (G1 walking bước đơn giản):

Biên độ
  │
  │      █                     ← peak ~1.5 Hz: gait frequency (1 bước/0.67s)
  │
  │        █                   ← peak ~3 Hz: harmonic bậc 2 của gait
  │
  │            █ █             ← peaks 5-15 Hz: interaction hip-waist
  │
  │                            ← quiet: không có energy ở 20-80 Hz (tốt!)
  └──────────────────────────── Hz
      1  2  3  5   10   20   50

Bước 2: FFT gyroscope roll axis (ωx)

Input: /lowstate/imu_state/gyroscope[0]

Tín hiệu lateral — dao động nhỏ hơn khi walking thẳng. Nếu có peak lớn ở đây → robot đang bù lateral sway quá mức → kiểm tra hip roll controller.

Bước 3: FFT accelerometer Z (az)

Input: /lowstate/imu_state/accelerometer[2]

Accelerometer Z phản ánh lực va chạm. Mỗi lần chân chạm đất (heel strike) tạo ra một xung. FFT sẽ cho thấy:

Peak tại tần số bước (giống gyroscope)
Peak tại tần số cộng hưởng của khung robot — đây là thứ cần tìm

Đọc biểu đồ FFT: Phát hiện Mechanical Resonance

Sau khi compute, PlotJuggler thêm kênh mới:

/lowstate/imu_state/gyroscope[1]_fft_gyroY

Kéo kênh này vào một panel trống. Trục X bây giờ là tần số (Hz) thay vì thời gian.

Các vùng tần số quan trọng với G1:

0 - 3 Hz:     Vùng gait — peaks bình thường ở tần số bước đi
3 - 15 Hz:    Harmonics gait + dao động thân trên — vài peaks là bình thường
15 - 50 Hz:   Vùng nguy hiểm — peak mạnh ở đây = mechanical resonance hoặc controller ringing
50 - 250 Hz:  Tần số điều khiển (500 Hz / 2 = 250 Hz Nyquist) — nên gần như phẳng

Dấu hiệu mechanical resonance cần chú ý:

Hiện tượng trên FFT	Nghĩa là
Peak sắc nhọn tại 20-40 Hz trên gyroY	Cộng hưởng khớp knee/hip trong sagittal plane — kiểm tra damping
Peak sắc nhọn tại 30-60 Hz trên gyroX	Cộng hưởng lateral — thường do khớp ankle roll có compliance cao
Peak tại 10-20 Hz biến mất khi robot đứng yên	Resonance bị kích thích bởi gait — dynamic coupling giữa bước đi và tần số tự nhiên khung
Dải tần số rộng (broadband) thay vì peak nhọn	Friction không đều trong khớp — khác với resonance

Ví dụ thực tế:

Trong một session debug G1 typical:

ωy FFT với G1 đang walking:
  - Peak 1.4 Hz (gait) → bình thường (walking cadence ~84 bước/phút)
  - Peak 2.8 Hz (harmonic bậc 2) → bình thường
  - Peak 28 Hz sharp → !! Mechanical resonance đầu gối phải !!

Kiểm tra chéo:
  - Lặp lại FFT trên motor_state[3]/tau (torque knee phải)
  - Nếu peak 28 Hz cũng xuất hiện trên torque → controller đang "chiến đấu" với resonance
  - Giải pháp: thêm notch filter 28 Hz vào joint PD controller

Dùng nhiều FFT panel để so sánh

PlotJuggler cho phép tạo nhiều FFT panels cạnh nhau. Chia đôi màn hình và so sánh:

Trái: FFT gyroscope[1] (pitch axis)
Phải: FFT motor_state[3]/tau (knee torque)

→ Nếu cả hai đều có peak tại 28 Hz: resonance mechanical
→ Nếu chỉ torque có peak: controller oscillation không lan sang IMU
→ Nếu chỉ IMU có peak: vibration từ môi trường (mặt đất rung, quạt máy)

Áp dụng transforms (Derivative, Moving Average, Integral, Scale) lên time series trong PlotJuggler — công cụ tương tự dùng để chuẩn bị tín hiệu trước khi đưa vào FFT — nguồn: repo PlotJuggler/PlotJuggler

Phần 3: Moving Average — Làm mượt dữ liệu Foot Force

Tại sao foot_force cần làm mượt?

Kênh /lowstate/foot_force[0..3] đo lực tiếp xúc của chân với mặt đất, dạng int16 (đơn vị tương đối, không phải Newton). Tín hiệu thô rất "nhọn":

Foot force RAW (500 Hz):

Force │
 100  │     ████         ████
  80  │   ██████       ██████
  60  │ ████████     ████████
  40  │██████████   ██████████
  20  │
   0  │──│──│──│──│──│──│──│──│── thời gian (s)
       stance  swing  stance  swing

Heel strike tạo xung nhọn (1-2 ms) rất khó phân tích visually

Với bộ lọc Moving Average 25 samples (= 50 ms tại 500 Hz):

Foot force SMOOTHED:

Force │
 100  │      ▄████▄          ▄████▄
  80  │   ▄████████▄      ▄████████▄
  60  │ ▄████████████▄  ▄████████████▄
  40  │████████████████████████████████
  20  │
   0  │──│────────│──│──│────────│──── thời gian (s)
         ← stance phase → swing
         
Pattern gait rõ ràng — dễ đọc, dễ phân tích

Cách áp dụng Moving Average trong PlotJuggler

Đây là built-in transform — không cần plugin riêng:

Cách 1: Chuột phải trực tiếp trên plot

Kéo /lowstate/foot_force[0] vào một panel
Khi đường đã hiện lên, chuột phải vào đường đó → "Apply Transform"
Chọn "Moving Average" từ danh sách dropdown
Dialog nhập tham số xuất hiện:

┌─────────────────────────────────────────────────────┐
│  Moving Average Transform                           │
│                                                     │
│  Window size (samples): [25                    ]    │
│                                                     │
│  * Với tần số 500 Hz:                               │
│    25 samples = 50 ms                               │
│    50 samples = 100 ms                              │
│    250 samples = 500 ms                             │
│                                                     │
│  [Apply]                  [Cancel]                  │
└─────────────────────────────────────────────────────┘

Nhập 25 → Nhấn Apply

PlotJuggler thay thế đường gốc bằng đường đã smooth. Tên kênh được cập nhật thành /lowstate/foot_force[0]::MovingAverage(25).

Cách 2: Từ Signal List (áp dụng cho tất cả 4 kênh cùng lúc)

Trong Signal List, tìm /lowstate/foot_force
Nhấn mũi tên expand → thấy [0], [1], [2], [3]
Nhấn Ctrl+A để select tất cả 4 kênh
Kéo cả 4 kênh vào một panel
Chuột phải vào bất kỳ đường nào → "Apply Transform to ALL curves" → Moving Average → Window 25

Chọn window size phù hợp

Window size	Thời gian (500 Hz)	Dùng khi
5 samples	10 ms	Chỉ muốn loại bỏ spike điện — vẫn thấy toe/heel strike rõ
25 samples	50 ms	Khuyến nghị — làm mượt heel strike nhưng giữ nguyên shape gait
50 samples	100 ms	Phân tích stance/swing pattern tổng thể
250 samples	500 ms	Low-pass mạnh — chỉ thấy weight shift giữa hai chân

Lưu ý: G1's foot_force không phải lực tuyệt đối Newton — là tín hiệu tương đối từ strain gauge. Để so sánh giữa các session, chú ý robot đặt trên surface nào và trọng lượng robot. Dùng để phân tích pattern (chân nào lift đầu tiên? stance phase bao lâu?) thay vì so sánh giá trị tuyệt đối.

Phân tích gait từ foot_force smoothed

Với 4 kênh foot_force đã smooth, plot vào cùng một panel:

Kênh:
  /lowstate/foot_force[0]  ← Chân trái (left)
  /lowstate/foot_force[1]  ← Chân phải (right)
  /lowstate/foot_force[2]  ← hoặc heel/toe của chân trái (tùy phiên bản G1)
  /lowstate/foot_force[3]  ← tương ứng chân phải

Pattern bình thường (walking):
  Left:  ████████▀▀▀▀▀▀▀████████▀▀▀▀▀▀▀████████
  Right: ▀▀▀▀████████▀▀▀▀▀▀▀████████▀▀▀▀▀▀▀████
         ← stance phase overlaps ở đầu và cuối mỗi bước

Các dấu hiệu bất thường cần tìm:

Dấu hiệu	Nghĩa là
Một chân luôn có force lớn hơn chân kia > 20%	Bất đối xứng gait — kiểm tra PD gain hai bên
Spike force > 2× giá trị trung bình trong < 5ms	Heel strike quá cứng — kiểm tra ankle compliance
Force không về 0 trong swing phase	Sensor drift hoặc chân không lift đủ cao
Cả hai chân cùng về 0 đột ngột	Robot nhảy hoặc slip

Kết hợp Moving Average với Phase Portrait từ bài 2

Bạn đã biết cách vẽ phase portrait (q vs dq) từ bài trước. Thêm một lớp insight bằng cách sync với foot_force:

Mở hai tab trong PlotJuggler:
- Tab "Kinematics": phase portrait ankle pitch (khớp 4 và 10)
- Tab "Force": foot_force smoothed
Dùng Vertical Marker (chuột phải → Add Vertical Line) đặt tại thời điểm heel strike (khi foot_force tăng đột biến)
Marker xuất hiện trên cả hai tab đồng thời — bạn thấy ngay vị trí và vận tốc ankle lúc chạm đất

Thông tin này trực tiếp cho bạn biết: ankle có đang ở góc tốt khi chạm đất không? Nếu q[4] (ankle pitch phải) đang ở -0.3 rad lúc heel strike trong khi thiết kế mong muốn -0.1 rad → landing too hard, cần điều chỉnh foot placement trong motion planner.

Workflow kết hợp 3 công cụ: Session debug hoàn chỉnh

Dưới đây là quy trình 10 bước cho một session debug IMU điển hình:

# 1. Setup môi trường (xem bài 1 nếu chưa cấu hình)
source /opt/ros/humble/setup.bash
export RMW_IMPLEMENTATION=rmw_cyclonedds_cpp
export CYCLONEDDS_URI=file:///home/$USER/cyclonedds_g1.xml

# 2. Ghi bag song song (luôn ghi khi debug quan trọng!)
ros2 bag record --storage mcap \
  -o ~/g1_bags/g1_imu_$(date +%Y%m%d_%H%M%S) \
  /lowstate

# 3. Launch PlotJuggler
ros2 run plotjuggler plotjuggler

# 4. Subscribe hoặc load file MCAP
# Live: Streaming → Start → ROS2 Topic Subscriber → /lowstate → OK
# Offline: File → Load Data File → chọn .mcap

Trong PlotJuggler (thứ tự thực hiện):

Bước 5: Tạo baseline panels
  - Panel A: /lowstate/imu_state/rpy[0] và rpy[1] (firmware roll/pitch)
  - Panel B: /lowstate/foot_force[0..3] (raw, chưa smooth)

Bước 6: Áp dụng ToolboxQuaternion
  - Tools → Toolboxes → Quaternion To RPY
  - Map quaternion[0..3] → W/X/Y/Z → Suffix: _pj → Compute
  - Thêm kênh mới vào Panel A để so sánh với firmware rpy

Bước 7: Áp dụng Moving Average cho foot_force
  - Chuột phải từng đường foot_force → Apply Transform → Moving Average → 25
  - Hoặc chọn tất cả → Apply to ALL curves

Bước 8: Chạy FFT trên gyroscope
  - Tools → Toolboxes → FFT
  - Input: /lowstate/imu_state/gyroscope[1] → Window: Hann → Last 10s → Compute
  - Thêm Panel C cho kênh FFT output
  - Lặp lại cho gyroscope[0] và accelerometer[2]

Bước 9: Phân tích kết quả
  - So sánh rpy firmware vs quaternion Euler → có drift không?
  - Tìm peak bất thường trong FFT tại 15-50 Hz
  - Đọc pattern gait từ foot_force smoothed

Bước 10: Lưu layout
  - File → Save Layout → g1_imu_fft_v1.xml

Tổng kết: Những gì bạn vừa học được

Sau bài này, bạn có trong tay ba công cụ phân tích IMU quan trọng:

Công cụ	Mở như thế nào	Câu hỏi trả lời
ToolboxQuaternion	Tools → Toolboxes → Quaternion To RPY	Robot đang nghiêng bao nhiêu? Firmware có đúng không?
ToolboxFFT	Tools → Toolboxes → FFT	Có tần số rung bất thường không? Cộng hưởng ở đâu?
Moving Average	Chuột phải → Apply Transform → Moving Average	Pattern gait như thế nào? Chân nào lift trước?

Khi nào dùng từng công cụ:

Robot đứng yên mà rpy drift → ToolboxQuaternion: so sánh firmware vs raw quaternion để xác định vấn đề ở filter hay sensor
Khớp kêu hoặc rung kỳ lạ khi walking → ToolboxFFT: tìm peak tần số trong gyroscope và torque để xác định cộng hưởng cơ học
Gait bất đối xứng hoặc heel strike quá cứng → Moving Average foot_force + phase portrait ankle: xem chân nào chịu lực bất thường và ở góc nào

Bài tiếp theo sẽ đi xa hơn: thay vì dùng tool có sẵn, chúng ta sẽ viết script Lua tùy chỉnh để tính những metric mà PlotJuggler không có built-in — power consumption per joint, slip index tính từ foot_force và velocity, hay tỷ số torque/velocity để phát hiện motor đang hoạt động phi tuyến.

Roadmap series

#	Bài	Nội dung
1	Cài đặt & kết nối live	Cài PlotJuggler, CycloneDDS, subscribe `/lowstate`
2	Layout 23 khớp + ghi MCAP	Multi-panel layout, file `.mcap`, phase portrait
3	Debug IMU + FFT (bài này)	Quaternion → Euler, FFT phân tích rung, Moving Average foot_force
4	Lua Transforms	Script Lua tính power per joint, derived signal
5	ZMQ + video sim2real	Cầu nối ZMQ từ MuJoCo, overlay camera feed

Cấu trúc IMU của G1 — nhắc lại nhanh

Kênh IMU trong /lowstate gồm:

/lowstate/imu_state/
  ├── quaternion[0]    ← w (phần thực — scalar)
  ├── quaternion[1]    ← x (phần ảo — pitch axis)
  ├── quaternion[2]    ← y (phần ảo — roll axis)
  ├── quaternion[3]    ← z (phần ảo — yaw axis)
  ├── rpy[0]           ← roll  (rad) — từ firmware
  ├── rpy[1]           ← pitch (rad) — từ firmware
  ├── rpy[2]           ← yaw   (rad) — từ firmware
  ├── accelerometer[0] ← ax (m/s²) — trục X body frame
  ├── accelerometer[1] ← ay (m/s²) — trục Y body frame
  ├── accelerometer[2] ← az (m/s²) — trục Z body frame (≈9.81 khi đứng thẳng)
  ├── gyroscope[0]     ← ωx (rad/s) — vận tốc góc quanh trục X
  ├── gyroscope[1]     ← ωy (rad/s) — vận tốc góc quanh trục Y
  └── gyroscope[2]     ← ωz (rad/s) — vận tốc góc quanh trục Z

Câu hỏi tự nhiên: G1 đã có sẵn /lowstate/imu_state/rpy[0..2] — tại sao cần ToolboxQuaternion?

Phần 1: ToolboxQuaternion — Quaternion sang Euler Angles

Tại sao quaternion phức tạp hơn Euler?

Khi debug, bạn cần cả hai: quaternion để đảm bảo tính toán đúng, Euler angles để nhìn vào biểu đồ và hiểu ngay robot đang làm gì.

Mở ToolboxQuaternion

Truy cập từ menu:

Menu PlotJuggler → Tools → Toolboxes → Quaternion To RPY

Hoặc nhìn vào toolbar phía trên cùng — biểu tượng hình hộp công cụ (🧰). Nếu không thấy, kiểm tra:

# Confirm plugin được load
ros2 run plotjuggler plotjuggler --version
# Vào App → Preferences → Loaded Plugins
# Tìm "ToolboxQuaternion" trong danh sách

Mapping kênh quaternion G1 vào Toolbox

Khi cửa sổ Quaternion To RPY Toolbox mở ra, bạn thấy 4 dropdown để gán input:

┌─────────────────────────────────────────────────────┐
│  Quaternion To RPY                                  │
│                                                     │
│  W (scalar):  [/lowstate/imu_state/quaternion[0] ▼] │
│  X:           [/lowstate/imu_state/quaternion[1] ▼] │
│  Y:           [/lowstate/imu_state/quaternion[2] ▼] │
│  Z:           [/lowstate/imu_state/quaternion[3] ▼] │
│                                                     │
│  Suffix: [_imu]                                     │
│                                                     │
│  [Compute]                    [Close]               │
└─────────────────────────────────────────────────────┘

Gán input:

W (scalar): /lowstate/imu_state/quaternion[0] — đây là phần thực (w) trong quaternion G1
X: /lowstate/imu_state/quaternion[1]
Y: /lowstate/imu_state/quaternion[2]
Z: /lowstate/imu_state/quaternion[3]
Suffix: Nhập tên hậu tố cho output, ví dụ _imu_pj để tránh trùng với kênh rpy gốc

Nhấn Compute. PlotJuggler tính toán và thêm 3 kênh mới vào Signal List:

/lowstate/imu_state/quaternion_imu_pj/roll    ← giá trị rad, range [-π, +π]
/lowstate/imu_state/quaternion_imu_pj/pitch   ← range [-π/2, +π/2]
/lowstate/imu_state/quaternion_imu_pj/yaw     ← range [-π, +π]

Công thức chuyển đổi bên trong Toolbox

Toolbox dùng convention ZYX (Tait-Bryan angles) — phổ biến nhất trong robotics và aerospace:

roll  = atan2( 2(w·x + y·z),   1 - 2(x² + y²) )
pitch = arcsin( 2(w·y - z·x) )
yaw   = atan2( 2(w·z + x·y),   1 - 2(y² + z²) )

Với G1 đứng thẳng, chờ đợi:

roll ≈ 0 (không nghiêng sang bên)
pitch ≈ 0 (không nghiêng trước/sau)
yaw thay đổi theo hướng mặt của robot (không quan trọng khi đứng tại chỗ)

Vẽ so sánh: Quaternion Euler vs Firmware RPY

Sau khi compute xong, kéo 6 kênh vào hai panel song song để so sánh:

Panel trên (Quaternion → Euler):
  /lowstate/imu_state/quaternion_imu_pj/roll
  /lowstate/imu_state/quaternion_imu_pj/pitch

Panel dưới (Firmware RPY):
  /lowstate/imu_state/rpy[0]   ← firmware roll
  /lowstate/imu_state/rpy[1]   ← firmware pitch

Trong điều kiện bình thường, hai cặp phải gần như trùng khớp. Nếu chúng lệch nhau:

Triệu chứng	Nguyên nhân có thể
Offset cố định (ví dụ: Euler = firmware + 0.05 rad)	G1's firmware áp dụng calibration offset khác
Lệch chỉ khi robot di chuyển nhanh	Latency trong filter firmware so với raw quaternion
Lệch đột ngột khi quay nhanh	Gimbal lock trong firmware Euler — quaternion không bị ảnh hưởng
Lệch khi robot đứng yên + drift tăng dần	IMU drift chưa được compensate — cần kiểm tra magnetometer hoặc tích hợp EKF

Bật Toolbox cho streaming live

Mặc định, ToolboxQuaternion chỉ tính trên dữ liệu đã load. Để bật chế độ streaming real-time (khi dùng ROS2 Topic Subscriber thay vì file MCAP):

Trong cửa sổ Toolbox → Check "Apply to streaming data" → Compute

Sau đó, khi PlotJuggler đang streaming live từ G1, ba kênh roll/pitch/yaw mới sẽ cập nhật real-time cùng lúc với quaternion gốc.

Phần 2: ToolboxFFT — Phân tích Tần số Rung Động

Analogy để hiểu FFT trong 30 giây

Với G1, thay vì âm thanh, chúng ta có tín hiệu gyroscope (rad/s) theo thời gian. FFT tách tín hiệu này thành:

Tần số gait (~1-2 Hz): robot đang bước đi, thân người đung đưa nhẹ
Tần số balance controller (~10-30 Hz): controller đang "giật" liên tục để giữ thẳng
Mechanical resonance của khớp (~20-80 Hz): nếu có peak bất thường ở đây → vấn đề cơ học

Mở ToolboxFFT

Menu PlotJuggler → Tools → Toolboxes → FFT

Cửa sổ FFT Toolbox hiện ra:

┌─────────────────────────────────────────────────────┐
│  FFT Toolbox                                        │
│                                                     │
│  Input signal: [/lowstate/imu_state/gyroscope[2] ▼] │
│                                                     │
│  Time range:   [Use full range]  [Last N seconds: 5]│
│  Window:       [Hann ▼]                             │
│                                                     │
│  Output suffix: [_fft_gyroZ]                        │
│                                                     │
│  [Compute FFT]                [Close]               │
└─────────────────────────────────────────────────────┘

Tham số quan trọng:

Tham số	Gợi ý	Lý do
Window	Hann	Giảm spectral leakage — phù hợp nhất cho tín hiệu robot không tuần hoàn hoàn hảo
Time range	Last 10-30 seconds	Đủ dài để FFT có độ phân giải tần số tốt (Δf = 1/T)
Phân giải tần số	Δf = 1/T	Nếu T = 10s thì Δf = 0.1 Hz — đủ để phân biệt 1 Hz vs 1.1 Hz

Chiến lược áp dụng FFT trên G1

Áp dụng FFT theo thứ tự này để systematic debug:

Bước 1: FFT gyroscope pitch axis (ωy) — quan trọng nhất

Input: /lowstate/imu_state/gyroscope[1]   ← ωy (vận tốc góc sagittal plane)

Đây là trục ảnh hưởng lớn nhất đến balance forward-backward. Trong walking gait bình thường:

Biểu đồ FFT bình thường (G1 walking bước đơn giản):

Biên độ
  │
  │      █                     ← peak ~1.5 Hz: gait frequency (1 bước/0.67s)
  │
  │        █                   ← peak ~3 Hz: harmonic bậc 2 của gait
  │
  │            █ █             ← peaks 5-15 Hz: interaction hip-waist
  │
  │                            ← quiet: không có energy ở 20-80 Hz (tốt!)
  └──────────────────────────── Hz
      1  2  3  5   10   20   50

Bước 2: FFT gyroscope roll axis (ωx)

Input: /lowstate/imu_state/gyroscope[0]

Tín hiệu lateral — dao động nhỏ hơn khi walking thẳng. Nếu có peak lớn ở đây → robot đang bù lateral sway quá mức → kiểm tra hip roll controller.

Bước 3: FFT accelerometer Z (az)

Input: /lowstate/imu_state/accelerometer[2]

Accelerometer Z phản ánh lực va chạm. Mỗi lần chân chạm đất (heel strike) tạo ra một xung. FFT sẽ cho thấy:

Peak tại tần số bước (giống gyroscope)
Peak tại tần số cộng hưởng của khung robot — đây là thứ cần tìm

Đọc biểu đồ FFT: Phát hiện Mechanical Resonance

Sau khi compute, PlotJuggler thêm kênh mới:

/lowstate/imu_state/gyroscope[1]_fft_gyroY

Kéo kênh này vào một panel trống. Trục X bây giờ là tần số (Hz) thay vì thời gian.

Các vùng tần số quan trọng với G1:

0 - 3 Hz:     Vùng gait — peaks bình thường ở tần số bước đi
3 - 15 Hz:    Harmonics gait + dao động thân trên — vài peaks là bình thường
15 - 50 Hz:   Vùng nguy hiểm — peak mạnh ở đây = mechanical resonance hoặc controller ringing
50 - 250 Hz:  Tần số điều khiển (500 Hz / 2 = 250 Hz Nyquist) — nên gần như phẳng

Dấu hiệu mechanical resonance cần chú ý:

Hiện tượng trên FFT	Nghĩa là
Peak sắc nhọn tại 20-40 Hz trên gyroY	Cộng hưởng khớp knee/hip trong sagittal plane — kiểm tra damping
Peak sắc nhọn tại 30-60 Hz trên gyroX	Cộng hưởng lateral — thường do khớp ankle roll có compliance cao
Peak tại 10-20 Hz biến mất khi robot đứng yên	Resonance bị kích thích bởi gait — dynamic coupling giữa bước đi và tần số tự nhiên khung
Dải tần số rộng (broadband) thay vì peak nhọn	Friction không đều trong khớp — khác với resonance

Ví dụ thực tế:

Trong một session debug G1 typical:

ωy FFT với G1 đang walking:
  - Peak 1.4 Hz (gait) → bình thường (walking cadence ~84 bước/phút)
  - Peak 2.8 Hz (harmonic bậc 2) → bình thường
  - Peak 28 Hz sharp → !! Mechanical resonance đầu gối phải !!

Kiểm tra chéo:
  - Lặp lại FFT trên motor_state[3]/tau (torque knee phải)
  - Nếu peak 28 Hz cũng xuất hiện trên torque → controller đang "chiến đấu" với resonance
  - Giải pháp: thêm notch filter 28 Hz vào joint PD controller

Dùng nhiều FFT panel để so sánh

PlotJuggler cho phép tạo nhiều FFT panels cạnh nhau. Chia đôi màn hình và so sánh:

Trái: FFT gyroscope[1] (pitch axis)
Phải: FFT motor_state[3]/tau (knee torque)

→ Nếu cả hai đều có peak tại 28 Hz: resonance mechanical
→ Nếu chỉ torque có peak: controller oscillation không lan sang IMU
→ Nếu chỉ IMU có peak: vibration từ môi trường (mặt đất rung, quạt máy)

Phần 3: Moving Average — Làm mượt dữ liệu Foot Force

Tại sao foot_force cần làm mượt?

Kênh /lowstate/foot_force[0..3] đo lực tiếp xúc của chân với mặt đất, dạng int16 (đơn vị tương đối, không phải Newton). Tín hiệu thô rất "nhọn":

Foot force RAW (500 Hz):

Force │
 100  │     ████         ████
  80  │   ██████       ██████
  60  │ ████████     ████████
  40  │██████████   ██████████
  20  │
   0  │──│──│──│──│──│──│──│──│── thời gian (s)
       stance  swing  stance  swing

Heel strike tạo xung nhọn (1-2 ms) rất khó phân tích visually

Với bộ lọc Moving Average 25 samples (= 50 ms tại 500 Hz):

Foot force SMOOTHED:

Force │
 100  │      ▄████▄          ▄████▄
  80  │   ▄████████▄      ▄████████▄
  60  │ ▄████████████▄  ▄████████████▄
  40  │████████████████████████████████
  20  │
   0  │──│────────│──│──│────────│──── thời gian (s)
         ← stance phase → swing
         
Pattern gait rõ ràng — dễ đọc, dễ phân tích

Cách áp dụng Moving Average trong PlotJuggler

Đây là built-in transform — không cần plugin riêng:

Cách 1: Chuột phải trực tiếp trên plot

Kéo /lowstate/foot_force[0] vào một panel
Khi đường đã hiện lên, chuột phải vào đường đó → "Apply Transform"
Chọn "Moving Average" từ danh sách dropdown
Dialog nhập tham số xuất hiện:

┌─────────────────────────────────────────────────────┐
│  Moving Average Transform                           │
│                                                     │
│  Window size (samples): [25                    ]    │
│                                                     │
│  * Với tần số 500 Hz:                               │
│    25 samples = 50 ms                               │
│    50 samples = 100 ms                              │
│    250 samples = 500 ms                             │
│                                                     │
│  [Apply]                  [Cancel]                  │
└─────────────────────────────────────────────────────┘

Nhập 25 → Nhấn Apply

PlotJuggler thay thế đường gốc bằng đường đã smooth. Tên kênh được cập nhật thành /lowstate/foot_force[0]::MovingAverage(25).

Cách 2: Từ Signal List (áp dụng cho tất cả 4 kênh cùng lúc)

Trong Signal List, tìm /lowstate/foot_force
Nhấn mũi tên expand → thấy [0], [1], [2], [3]
Nhấn Ctrl+A để select tất cả 4 kênh
Kéo cả 4 kênh vào một panel
Chuột phải vào bất kỳ đường nào → "Apply Transform to ALL curves" → Moving Average → Window 25

Chọn window size phù hợp

Window size	Thời gian (500 Hz)	Dùng khi
5 samples	10 ms	Chỉ muốn loại bỏ spike điện — vẫn thấy toe/heel strike rõ
25 samples	50 ms	Khuyến nghị — làm mượt heel strike nhưng giữ nguyên shape gait
50 samples	100 ms	Phân tích stance/swing pattern tổng thể
250 samples	500 ms	Low-pass mạnh — chỉ thấy weight shift giữa hai chân

Lưu ý: G1's foot_force không phải lực tuyệt đối Newton — là tín hiệu tương đối từ strain gauge. Để so sánh giữa các session, chú ý robot đặt trên surface nào và trọng lượng robot. Dùng để phân tích pattern (chân nào lift đầu tiên? stance phase bao lâu?) thay vì so sánh giá trị tuyệt đối.

Phân tích gait từ foot_force smoothed

Với 4 kênh foot_force đã smooth, plot vào cùng một panel:

Kênh:
  /lowstate/foot_force[0]  ← Chân trái (left)
  /lowstate/foot_force[1]  ← Chân phải (right)
  /lowstate/foot_force[2]  ← hoặc heel/toe của chân trái (tùy phiên bản G1)
  /lowstate/foot_force[3]  ← tương ứng chân phải

Pattern bình thường (walking):
  Left:  ████████▀▀▀▀▀▀▀████████▀▀▀▀▀▀▀████████
  Right: ▀▀▀▀████████▀▀▀▀▀▀▀████████▀▀▀▀▀▀▀████
         ← stance phase overlaps ở đầu và cuối mỗi bước

Các dấu hiệu bất thường cần tìm:

Dấu hiệu	Nghĩa là
Một chân luôn có force lớn hơn chân kia > 20%	Bất đối xứng gait — kiểm tra PD gain hai bên
Spike force > 2× giá trị trung bình trong < 5ms	Heel strike quá cứng — kiểm tra ankle compliance
Force không về 0 trong swing phase	Sensor drift hoặc chân không lift đủ cao
Cả hai chân cùng về 0 đột ngột	Robot nhảy hoặc slip

Kết hợp Moving Average với Phase Portrait từ bài 2

Bạn đã biết cách vẽ phase portrait (q vs dq) từ bài trước. Thêm một lớp insight bằng cách sync với foot_force:

Mở hai tab trong PlotJuggler:
- Tab "Kinematics": phase portrait ankle pitch (khớp 4 và 10)
- Tab "Force": foot_force smoothed
Dùng Vertical Marker (chuột phải → Add Vertical Line) đặt tại thời điểm heel strike (khi foot_force tăng đột biến)
Marker xuất hiện trên cả hai tab đồng thời — bạn thấy ngay vị trí và vận tốc ankle lúc chạm đất

Workflow kết hợp 3 công cụ: Session debug hoàn chỉnh

Dưới đây là quy trình 10 bước cho một session debug IMU điển hình:

# 1. Setup môi trường (xem bài 1 nếu chưa cấu hình)
source /opt/ros/humble/setup.bash
export RMW_IMPLEMENTATION=rmw_cyclonedds_cpp
export CYCLONEDDS_URI=file:///home/$USER/cyclonedds_g1.xml

# 2. Ghi bag song song (luôn ghi khi debug quan trọng!)
ros2 bag record --storage mcap \
  -o ~/g1_bags/g1_imu_$(date +%Y%m%d_%H%M%S) \
  /lowstate

# 3. Launch PlotJuggler
ros2 run plotjuggler plotjuggler

# 4. Subscribe hoặc load file MCAP
# Live: Streaming → Start → ROS2 Topic Subscriber → /lowstate → OK
# Offline: File → Load Data File → chọn .mcap

Trong PlotJuggler (thứ tự thực hiện):

Bước 5: Tạo baseline panels
  - Panel A: /lowstate/imu_state/rpy[0] và rpy[1] (firmware roll/pitch)
  - Panel B: /lowstate/foot_force[0..3] (raw, chưa smooth)

Bước 6: Áp dụng ToolboxQuaternion
  - Tools → Toolboxes → Quaternion To RPY
  - Map quaternion[0..3] → W/X/Y/Z → Suffix: _pj → Compute
  - Thêm kênh mới vào Panel A để so sánh với firmware rpy

Bước 7: Áp dụng Moving Average cho foot_force
  - Chuột phải từng đường foot_force → Apply Transform → Moving Average → 25
  - Hoặc chọn tất cả → Apply to ALL curves

Bước 8: Chạy FFT trên gyroscope
  - Tools → Toolboxes → FFT
  - Input: /lowstate/imu_state/gyroscope[1] → Window: Hann → Last 10s → Compute
  - Thêm Panel C cho kênh FFT output
  - Lặp lại cho gyroscope[0] và accelerometer[2]

Bước 9: Phân tích kết quả
  - So sánh rpy firmware vs quaternion Euler → có drift không?
  - Tìm peak bất thường trong FFT tại 15-50 Hz
  - Đọc pattern gait từ foot_force smoothed

Bước 10: Lưu layout
  - File → Save Layout → g1_imu_fft_v1.xml

Tổng kết: Những gì bạn vừa học được

Sau bài này, bạn có trong tay ba công cụ phân tích IMU quan trọng:

Công cụ	Mở như thế nào	Câu hỏi trả lời
ToolboxQuaternion	Tools → Toolboxes → Quaternion To RPY	Robot đang nghiêng bao nhiêu? Firmware có đúng không?
ToolboxFFT	Tools → Toolboxes → FFT	Có tần số rung bất thường không? Cộng hưởng ở đâu?
Moving Average	Chuột phải → Apply Transform → Moving Average	Pattern gait như thế nào? Chân nào lift trước?

Khi nào dùng từng công cụ:

Robot đứng yên mà rpy drift → ToolboxQuaternion: so sánh firmware vs raw quaternion để xác định vấn đề ở filter hay sensor
Khớp kêu hoặc rung kỳ lạ khi walking → ToolboxFFT: tìm peak tần số trong gyroscope và torque để xác định cộng hưởng cơ học
Gait bất đối xứng hoặc heel strike quá cứng → Moving Average foot_force + phase portrait ankle: xem chân nào chịu lực bất thường và ở góc nào

Roadmap series

Cấu trúc IMU của G1 — nhắc lại nhanh

Phần 1: ToolboxQuaternion — Quaternion sang Euler Angles

Tại sao quaternion phức tạp hơn Euler?

Mở ToolboxQuaternion

Mapping kênh quaternion G1 vào Toolbox

Công thức chuyển đổi bên trong Toolbox

Vẽ so sánh: Quaternion Euler vs Firmware RPY

Bật Toolbox cho streaming live

Phần 2: ToolboxFFT — Phân tích Tần số Rung Động

Analogy để hiểu FFT trong 30 giây

Mở ToolboxFFT

Chiến lược áp dụng FFT trên G1

Đọc biểu đồ FFT: Phát hiện Mechanical Resonance

Dùng nhiều FFT panel để so sánh

Phần 3: Moving Average — Làm mượt dữ liệu Foot Force

Tại sao foot_force cần làm mượt?

Cách áp dụng Moving Average trong PlotJuggler

Chọn window size phù hợp

Phân tích gait từ foot_force smoothed

Kết hợp Moving Average với Phase Portrait từ bài 2

Workflow kết hợp 3 công cụ: Session debug hoàn chỉnh

Tổng kết: Những gì bạn vừa học được

Bài viết liên quan

Nguyễn Anh Tuấn

Bài viết liên quan

PlotJuggler + G1: Cài đặt ROS2 và kết nối live

Checklist sim2real cho controller G1

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

Roadmap series

Cấu trúc IMU của G1 — nhắc lại nhanh

Phần 1: ToolboxQuaternion — Quaternion sang Euler Angles

Tại sao quaternion phức tạp hơn Euler?

Mở ToolboxQuaternion

Mapping kênh quaternion G1 vào Toolbox

Công thức chuyển đổi bên trong Toolbox

Vẽ so sánh: Quaternion Euler vs Firmware RPY

Bật Toolbox cho streaming live

Phần 2: ToolboxFFT — Phân tích Tần số Rung Động

Analogy để hiểu FFT trong 30 giây

Mở ToolboxFFT

Chiến lược áp dụng FFT trên G1

Đọc biểu đồ FFT: Phát hiện Mechanical Resonance

Dùng nhiều FFT panel để so sánh

Phần 3: Moving Average — Làm mượt dữ liệu Foot Force

Tại sao foot_force cần làm mượt?

Cách áp dụng Moving Average trong PlotJuggler

Chọn window size phù hợp

Phân tích gait từ foot_force smoothed

Kết hợp Moving Average với Phase Portrait từ bài 2

Workflow kết hợp 3 công cụ: Session debug hoàn chỉnh

Tổng kết: Những gì bạn vừa học được

Bài viết liên quan

Nguyễn Anh Tuấn

Bài viết liên quan

PlotJuggler + G1: Cài đặt ROS2 và kết nối live

Checklist sim2real cho controller G1

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