first commit

README.md

@@ -0,0 +1,84 @@
+# 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
+
+```bash
+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`.