85 lines
4.9 KiB
Markdown
85 lines
4.9 KiB
Markdown
# 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`.
|