mprim_gen — Sinh file .mprim cho SBPL lattice planner (Python)

Port lại từ các script MATLAB gốc (genmprim*.m của Maxim Likhachev), viết lại theo kiến trúc module hóa để dễ thêm robot mới mà không phải sửa code logic.

Cấu trúc file

mprim_gen/
├── mprim_config.py      # RobotConfig — định nghĩa thông số + validate (KHÔNG có logic sinh)
├── kinematics.py        # 2 interpolator: unicycle (diff + ackermann) / holonomic (omni)
├── mprim_core.py        # engine dùng chung: build primitive set, xoay theo góc, tự kiểm, ghi file
├── generate_mprim.py    # CLI chạy
├── test_mprim_gen.py    # test tự động: ràng buộc SBPL + chất lượng hình học
├── visualize_mprim.py   # vẽ primitive tại 1 góc, kèm vạch heading từng pose
└── configs/             # MỖI ROBOT = 1 file config nhỏ ở đây
    ├── differential.py
    ├── omnidirectional.py
    └── ackermann.py

Nguyên tắc: khi thêm robot mới, bạn KHÔNG đụng vào mprim_core.py hay kinematics.py — chỉ copy 1 file trong configs/, đổi số liệu, chạy lại.

Cách dùng

python3 generate_mprim.py configs/differential.py -o diff.mprim
python3 generate_mprim.py configs/omnidirectional.py -o omni.mprim
python3 generate_mprim.py configs/ackermann.py -o ackermann.mprim

python3 test_mprim_gen.py                       # chạy toàn bộ test trước khi tích hợp
python3 visualize_mprim.py diff.mprim --angle 1 # kiểm tra bằng mắt ở góc lẻ

Những gì generator ĐẢM BẢO trên file output

Đây là các ràng buộc SBPL enforce lúc nạp file (ReadinMotionPrimitive trong environment_navxythetalat.cpp) — engine tự kiểm sau khi sinh, vi phạm là raise ngay lúc generate thay vì fail lúc khởi động planner trên robot:

1. additionalactioncostmult là số nguyên (SBPL đọc bằng fscanf %d, giá trị 1.5/2.5 làm hỏng cả file).
2. Pose cuối của quỹ đạo rời rạc hóa ra đúng endpose_c.
3. Heading cuối khớp chính xác góc rời rạc đích (không còn kiểu lệch 10 độ do ép l = 0 mà không tính lại rv như bản MATLAB gốc).
4. Khoảng cách giữa 2 intermediate pose <= 1 cell và ~ đều nhau trên mọi primitive (pose_spacing_m) → path SBPL trả ra có pose cách đều, footprint sweep không bỏ sót cell.
5. Không có primitive suy biến, cảnh báo khi trùng endpose, cảnh báo khi cung sau quantize vi phạm min_turning_radius_m (ackermann).

Thông số cần đo/lấy từ robot thật để điền vào config

Thông số	Cách lấy
resolution_m	PHẢI khớp resolution của costmap (global/local costmap yaml)
forward_long_m	quãng đường robot đi trong ~1 chu kỳ điều khiển ở vận tốc hành trình
turn_radius_m	>= max_vel/max_yawrate của robot, và >= resolution/(1-cos(bước góc)) để quantize không làm méo cung (~0.66m với lưới 5cm, 16 góc)
min_turning_radius_m (Ackermann)	đo trực tiếp trên robot (bán kính hẹp nhất không trượt bánh)
sidestep_m, diagonal_m (omni)	quãng đường đi ngang/chéo robot làm an toàn được
max_vel_mps, max_yawrate_radps	từ config local planner (vd robot_max_v_ac/robot_max_w_ac) — chỉ để sanity-check
cost mult	số nguyên; điều chỉnh planner ưu tiên hành động nào. Bộ tham chiếu an toàn: backward=40, turninplace=20, forwardandturn=2

Khác biệt so với bản MATLAB gốc (lưu ý quan trọng)

Bản gốc định nghĩa tay 3 bộ template số nguyên riêng cho góc 0°/45°/22.5°. Bản Python định nghĩa primitive một lần theo mét (bán kính cung, chiều dài) rồi tự xoay + làm tròn về cell cho từng góc. Ưu điểm: dễ tham số hóa theo robot thật, primitive trái/phải đối xứng tuyệt đối (template tay của bản gốc bị lệch trái/phải tới 8°). Đánh đổi: sai số lượng tử hóa nhỏ ở các góc lẻ (< nửa cell, được dàn đều dọc quỹ đạo, heading không bị ảnh hưởng).

Khác biệt hành vi so với bản gốc: khi nghiệm "đoạn thẳng + cung" đòi đảo chiều (do quantize), bản gốc ép l = 0 nhưng giữ nguyên rv làm heading cuối lệch tới ~10° (chỉ in warning trong MATLAB console); bản này tính lại rv để heading cuối luôn chính xác — đây là điều kiện SBPL dùng để chấp nhận file.

Tích hợp vào planner

File output dùng trực tiếp với sbpl_lattice_planner qua key primitive_filename trong sbpl_global_params.yaml. Sau khi đổi file mprim cần kiểm tra: resolution_m khớp costmap, và log khởi động planner không có ERROR: incorrect primitive.