PlotJuggler Lua Transforms: Tính power, tracking error cho G1

Ba bài trước chúng ta đã có đủ dữ liệu thô từ G1: stream live từ ROS2 (bài 1), layout 23 khớp có ghi MCAP (bài 2), và phân tích IMU + FFT phát hiện rung (bài 3).

Nhưng dữ liệu thô thường không đủ để chẩn đoán. Ví dụ: /lowstate/motor_state[i]/tau cho bạn torque, /lowstate/motor_state[i]/dq cho bạn velocity — nhưng công suất cơ học của khớp đó (τ × dq) lại không tồn tại trong bất kỳ kênh nào. Để biết khớp nào đang "ngốn" điện nhiều nhất lúc robot đi bộ, bạn phải tự tính.

PlotJuggler giải quyết vấn đề này bằng Custom Function Editor — một scripting engine nhúng trong GUI cho phép bạn viết hàm Lua ngắn để tạo ra kênh tín hiệu mới từ các kênh có sẵn, realtime.

Roadmap series

Hai loại tính toán trong PlotJuggler

Trước khi vào code, cần hiểu PlotJuggler cung cấp hai cơ chế để tính tín hiệu dẫn xuất, dùng cho mục đích khác nhau:

Loại 1: Function Transform (Shift+T hoặc icon "f(x)" trên panel)
─────────────────────────────────────────────────────────────────
  • Biến đổi 1 kênh → 1 kênh output mới (1-to-1)
  • Dùng cho phép toán đơn giản: abs(x), x * 180/pi, x[t] - x[t-1]
  • Không truy cập kênh khác (chỉ thấy "giá trị hiện tại" của kênh đang kéo)
  • Kết quả hiển thị ngay trên đồ thị nhưng không lưu vào data tree

Loại 2: Custom Function (Tools → Custom Functions → Add)
─────────────────────────────────────────────────────────
  • Viết hàm Lua nhận nhiều kênh đầu vào → 1 kênh output mới (multi-in, single-out)
  • Kênh output xuất hiện trong data tree, có thể kéo vào bất kỳ panel nào
  • Hàm chạy mỗi khi có timestamp mới (hoặc khi replay file .mcap)
  • Hỗ trợ biến toàn cục (persist qua các lần gọi) → tính tích phân, rolling buffer

Bài này tập trung vào Custom Function — đủ mạnh để tính power per joint, tracking error, và bất kỳ tín hiệu kết hợp nào bạn cần.

Về scripting language: PlotJuggler dùng Lua (thông qua thư viện sol2) cho tính năng Custom Function từ trước tới nay. Tên "Lua Transforms" trong series này phản ánh đúng thực tế.

Mở Custom Script Editor

Từ menu PlotJuggler:

Tools → Custom Functions → Add new custom function

Hoặc phím tắt Ctrl+Shift+C. Cửa sổ Function Editor mở ra với hai vùng:

┌─────────────────────────────────────────────────────────┐
│  GLOBAL CODE (chạy 1 lần khi khởi tạo)                 │
│  ─ khai báo biến persistent, hằng số                   │
├─────────────────────────────────────────────────────────┤
│  FUNCTION CODE (chạy mỗi timestamp)                    │
│  ─ nhận input → return output                          │
├─────────────────────────────────────────────────────────┤
│  INPUT CHANNELS  │  OUTPUT CHANNELS                    │
│  (kéo thả từ    │  (tên kênh mới sẽ                   │
│   data tree)    │   tạo ra)                            │
└─────────────────────────────────────────────────────────┘

Panel Input Channels quan trọng — bạn phải khai báo các kênh /lowstate/... bạn muốn dùng vào đây trước khi script có thể đọc chúng.

Transform 1: Power per joint

Bài toán

Mỗi khớp G1 có hai kênh:

• tau — torque motor (N·m)
• dq — velocity joint thực tế (rad/s)

Công suất cơ học = τ × dq (W). Với 23 khớp, tổng năng lượng tiêu thụ cơ học có thể vượt 500W khi G1 đi nhanh. Biết khớp nào đang tiêu thụ nhiều nhất cho phép bạn:

1. Phát hiện friction cao bất thường (ma sát khớp → nhiệt → hỏng bearing)
2. Verify controller đang phân phối load hợp lý giữa các khớp
3. Estimate battery drain realtime

Code script

Custom Function trong PlotJuggler là single-output: mỗi function instance tạo ra đúng 1 kênh output. API nhận input theo vị trí:

calc(time, value, v1, v2, ...)
         ↑       ↑   ↑
      kênh main  additional sources (v1, v2, ...)

Tạo function mới cho khớp cần debug (ví dụ joint 10 — hip trái):

1. Tools → Custom Functions → Add new custom function
2. Đặt tên output: "power_hip_left"
3. Kéo /lowstate/motor_state[10]/tau  vào "main input" (= value trong hàm)
4. Kéo /lowstate/motor_state[10]/dq   vào "additional source" (= v1 trong hàm)

Global Code (chạy 1 lần khi khởi tạo):

-- Không cần biến global cho transform này

Function Code:

-- value = tau (N·m), v1 = dq (rad/s)
-- Công suất cơ học = τ × dq
return value * v1

Kết quả: kênh power_hip_left xuất hiện trong data tree, hiển thị công suất (Watt) theo thời gian.

Lặp lại cho các khớp nghi ngờ (thường knee + ankle cùng bên để phát hiện vấn đề trong chuỗi động học). Để debug nhanh, không cần tạo cả 23 — chỉ cần 3–5 khớp liên quan đến hành vi bất thường quan sát được.

Kéo các kênh power_* vào một panel, dùng View → Stacked Layout để xem cạnh nhau. Khớp nào có spike cao nhất khi G1 bước lên bậc thang → đó là khớp chịu lực nhất.

Transform 2: Tổng công suất cơ học

Vì Custom Function là single-output, để tính tổng power bạn có hai cách:

Cách A (đơn giản): Sau khi có 23 kênh power_* riêng lẻ, dùng Function Transform (Shift+T) trên một kênh bất kỳ rồi dùng Built-in Add transform để cộng chúng lại. PlotJuggler cũng có Sum(a, b) trong built-in transforms.

Cách B (1 function, đưa nhiều cặp vào): Tạo 1 function nhận nhiều additional sources (v1..v46), nhưng cách này đòi kéo 46 kênh vào GUI — chỉ hợp lý nếu bạn muốn debug 5–6 khớp chủ chốt:

-- main = tau_hip_left, v1 = dq_hip_left
-- v2 = tau_knee_left,  v3 = dq_knee_left
-- v4 = tau_ankle_left, v5 = dq_ankle_left
-- (và tương tự cho chân phải nếu muốn)

local p_hip_L   = value * v1
local p_knee_L  = v2    * v3
local p_ankle_L = v4    * v5

local total = p_hip_L + p_knee_L + p_ankle_L

-- Bỏ phần regenerative braking (âm) nếu chỉ quan tâm "tiêu điện"
if total < 0 then total = 0 end
return total

Tại sao loại giá trị âm? Khi motor brake (velocity cùng chiều torque âm), tích τ × dq < 0 — robot đang tái sinh năng lượng. Giữ giá trị âm nếu muốn tính energy balance; cắt về 0 nếu chỉ quan tâm lúc robot "tốn điện".

Overlay kênh total power với IMU rpy[1] (pitch angle): khi G1 lean forward nhiều → power tăng tương ứng không? Nếu không → controller đang bù không đúng.

Transform 3: Velocity tracking error

G1 controller gửi lệnh velocity dq_d (desired) và thực tế đo được là dq. Tracking error phản ánh chất lượng motor control và độ mòn của khớp:

tracking_error_i = |dq_actual_i - dq_desired_i| / (|dq_desired_i| + ε)

Kênh dq có trong /lowstate/motor_state[i]/dq. Kênh dq_d nằm ở /lowstate/motor_state[i]/dq_d.

Tạo Custom Function mới (ví dụ cho joint 10 — hip trái):

main input: /lowstate/motor_state[10]/dq   (dq_actual = value)
additional: /lowstate/motor_state[10]/dq_d (dq_desired = v1)
output name: "tracking_error_hip_left"

Global Code:

EPSILON = 0.001   -- tránh chia cho 0 khi robot đứng yên (global, không có 'local')

Function Code:

-- value = dq_actual (rad/s thực tế)
-- v1    = dq_desired (rad/s mục tiêu)
local abs_error  = math.abs(value - v1)
local normalized = abs_error / (math.abs(v1) + EPSILON)
return normalized

Cách đọc kết quả:

tracking_error > 0.5  → khớp đang lag nặng (>50% error)
                         Nguyên nhân: friction cao, joint worn, controller gain thấp
tracking_error ≈ 0    → perfect tracking (hoặc robot đứng yên, dq_desired ≈ 0)
tracking_error > 2.0  → bất thường — kiểm tra hardware

Lưu và tải lại transform template

Mỗi lần tắt PlotJuggler, script mất. Để tái sử dụng:

Lưu script ra file .xml:

Trong Custom Series Editor → nút "Save" (biểu tượng floppy) → chọn đường dẫn
Ví dụ: ~/plotjuggler_scripts/g1_power_analysis.xml

Tải lại trong session mới:

Tools → Custom Series → Load from file → chọn .xml

Đặt tất cả scripts G1 vào một thư mục, đặt tên rõ ràng:

~/plotjuggler_scripts/
  ├── g1_power_per_joint.xml
  ├── g1_total_power.xml
  ├── g1_tracking_error.xml
  └── g1_imu_quaternion_euler.xml   ← từ bài 3

Khi bắt đầu debug session mới, load cả thư mục → tất cả kênh derived xuất hiện ngay.

Case study: Phát hiện khớp bị quá tải trong gait

Đây là kịch bản thực tế. Giả sử G1 đang thực hiện bài test đi bộ 30 giây, và bạn nhận thấy robot bắt đầu "khập khiễng" nhẹ sau 15 giây.

Bước 1: Replay file MCAP của session đó trong PlotJuggler.

Bước 2: Load script g1_power_per_joint.xml và g1_tracking_error.xml.

Bước 3: Tìm timestamp ~15 giây và xem bảng so sánh:

Timestamp: t=14.8s → t=18.2s

Joint 10 (hip left — khớp hông trái):
  power_10:          từ 45W → 89W  (+98%)  ← BẤT THƯỜNG
  tracking_error_10: từ 0.12 → 0.67        ← tracking kém dần

Joint 11 (knee left — đầu gối trái):
  power_11:          từ 38W → 92W  (+142%) ← CỰC BẤT THƯỜNG
  tracking_error_11: từ 0.08 → 0.51

Joint 12 (ankle left):
  power_12:          ổn định ~15W
  tracking_error_12: ổn định ~0.09

Kết luận: Chuỗi hip + knee trái đột ngột tăng gấp đôi power và tracking error trong khi ankle bình thường → vấn đề không phải ở cuối chuỗi động học mà ở giữa. Kiểm tra:

• Joint 10/11 có bị nhiệt cao không? (log temperature nếu G1 của bạn expose kênh này)
• Bearing joint 11 có tiếng lạ không?
• Đặt tracking_error_11 overlay với total_mechanical_power — nếu tracking error tăng trước khi power tăng → motor đang cố bù cho lực cản cơ học tăng dần.

Tổng kết

Custom Function trong PlotJuggler không phải replacement cho Python/pandas — đó là công cụ real-time debug tại hiện trường. Bạn không cần viết node ROS hay export bag mới khi muốn xem một derived signal mới: viết 2–5 dòng Lua trong Function Editor, kéo output kênh vào panel, xong.

Ba transform trong bài này (joint_power, total power, tracking_error) là foundation cho mọi session debug G1 cơ bản. Khi đã quen, bạn có thể thêm:

• energy_per_step — tích phân power qua một stride
• impedance_estimate — ước tính impedance từ tỉ số tau/dq
• gait_asymmetry — so sánh power bên trái vs bên phải

Bài tiếp theo kết thúc series với một kỹ thuật debug nâng cao nhất: ZMQ bridge từ MuJoCo sang PlotJuggler và đồng bộ camera feed để so sánh sim-to-real trực tiếp.

Bài viết liên quan

• PlotJuggler + G1: Debug IMU Quaternion → Euler + FFT phân tích rung — Phân tích tín hiệu IMU G1: quaternion sang Euler, FFT phát hiện tần số cộng hưởng
• Layout 23 khớp G1: MCAP replay và Phase Portrait — Tổ chức đa panel, ghi file MCAP để replay ngoại tuyến
• ZMQ + VideoViewer: Debug Sim-to-Real đồng bộ camera G1 — ZMQ bridge từ MuJoCo, overlay camera feed, so sánh sim vs real