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`.

configs/ackermann.py

@@ -0,0 +1,35 @@
+"""Config Ackermann (xe có bánh lái). Chỉnh số liệu theo robot thực tế."""
+
+from mprim_config import RobotConfig, KinematicType
+
+config = RobotConfig(
+    name="ackermann",                       # tên file output: ackermann.mprim
+    kinematic_type=KinematicType.ACKERMANN,  # xe bánh lái -> nội suy cung tròn unicycle,
+                                             # có kiểm tra min_turning_radius
+
+    resolution_m=0.05,          # [m] PHẢI khớp costmap resolution
+    numberofangles=16,          # 16 góc heading -> 1 bước góc = 22.5 độ
+
+    forward_short_m=0.20,       # [m] bước tiến ngắn (xe dài nên bước ngắn cũng dài hơn AMR)
+    forward_long_m=0.60,        # [m] bước tiến dài
+    backward_m=0.20,            # [m] bước lùi thẳng
+
+    min_turning_radius_m=0.45,  # [m] bán kính quay tối thiểu VẬT LÝ, đo từ robot thật
+    turn_steps=(1,),            # chỉ rẽ 22.5 độ mỗi primitive
+    turn_radius_m=0.70,         # [m] cao hơn min_turning_radius làm dự phòng quantize; với
+                                #     lưới 5cm cần >= ~0.66 để cung không bị méo (validate
+                                #     sẽ cảnh báo nếu thiếu)
+
+    has_backward_prims=True,        # cho phép lùi thẳng
+    has_backward_turn_prims=True,   # xe bánh lái cần lùi + đánh lái để quay đầu
+    has_turn_in_place_prims=False,  # xe Ackermann KHÔNG quay tại chỗ được
+
+    # SỐ NGUYÊN (SBPL đọc %d). Ackermann lùi tốn hơn nhiều (khó điều khiển, hạn chế tầm nhìn).
+    forwardcostmult=1,          # tiến thẳng — mốc chuẩn
+    backwardcostmult=6,         # lùi thẳng
+    forwardandturncostmult=3,   # tiến + đánh lái
+    backwardandturncostmult=8,  # lùi + đánh lái — đắt nhất, chỉ dành cho quay đầu
+    turninplacecostmult=5,      # không dùng khi has_turn_in_place_prims=False
+
+    pose_spacing_m=0.05,        # [m] khoảng cách pose trên path
+)

configs/differential.py

@@ -0,0 +1,46 @@
+"""Config differential drive (2 bánh vi sai). Chỉnh số liệu theo robot thực tế.
+
+Cost mult lấy theo bộ tuned unicycle_highcost_5cm.mprim (backward=40, turninplace=20,
+forwardandturn=2): lùi và quay tại chỗ đắt tới mức chỉ được chọn khi không còn đường
+khác — an toàn cho AMR có cảm biến chính hướng về phía trước. Muốn robot thoải mái
+lùi/xoay hơn thì hạ dần (vd 5/5/3) nhưng đó là quyết định vận hành có chủ đích.
+"""
+
+from mprim_config import RobotConfig, KinematicType
+
+config = RobotConfig(
+    name="differential",                    # tên file output: differential.mprim
+    kinematic_type=KinematicType.DIFFERENTIAL,  # 2 bánh vi sai -> nội suy unicycle
+
+    resolution_m=0.05,          # [m] PHẢI khớp costmap resolution (costmap_global_params.yaml)
+    numberofangles=16,          # 16 góc heading -> 1 bước góc = 22.5 độ
+
+    forward_short_m=0.10,       # [m] bước tiến ngắn ~2 cell, để tinh chỉnh gần goal
+    forward_long_m=0.40,        # [m] bước tiến dài ~8 cell — quãng đường 1 chu kỳ điều khiển
+                                #     ở tốc độ hành trình
+    backward_m=0.10,            # [m] bước lùi thẳng
+
+    # Rẽ = cung tròn: (1,) nghĩa là chỉ có primitive rẽ 22.5 độ (giống bộ tham chiếu).
+    # Thêm 2 vào tuple nếu cần rẽ 45 độ — đổi lại branching factor tăng, planner chậm hơn.
+    turn_steps=(1,),
+    turn_radius_m=0.70,         # [m] >= max_vel/max_yawrate = 0.4/0.6 ~ 0.67 để local planner
+                                #     bám cung không phải giảm tốc; >= 0.66 để quantize 5cm
+                                #     không làm méo cung
+
+    has_backward_prims=True,        # cho phép lùi thẳng (cost 40 nên chỉ dùng khi kẹt)
+    has_backward_turn_prims=False,  # lùi + rẽ tắt: hành động mù cảm biến, bật lại nếu thật cần
+    has_turn_in_place_prims=True,   # cho phép quay tại chỗ (cost 20 nên hiếm khi được chọn)
+
+    # SỐ NGUYÊN (SBPL đọc %d)
+    forwardcostmult=1,          # tiến thẳng — mốc chuẩn
+    backwardcostmult=40,        # lùi 0.1m "đắt" bằng tiến 4m -> chỉ lùi khi không còn cách
+    forwardandturncostmult=2,   # rẽ đắt gấp đôi đi thẳng -> ưu tiên đường ít cua
+    backwardandturncostmult=40, # (không dùng khi has_backward_turn_prims=False)
+    turninplacecostmult=20,     # xoay 22.5 độ "đắt" bằng tiến ~4m -> tránh xoay giữa đường
+
+    pose_spacing_m=0.05,        # [m] = resolution -> path SBPL trả ra có pose cách đều ~5cm
+
+    # giới hạn động học thật (hybrid_local_planner_params.yaml: robot_max_v_ac / robot_max_w_ac)
+    max_vel_mps=0.4,            # [m/s] vận tốc dài tối đa
+    max_yawrate_radps=0.6,      # [rad/s] vận tốc xoay tối đa
+)

configs/omnidirectional.py

@@ -0,0 +1,38 @@
+"""Config omnidirectional (mecanum/omni/swerve). Chỉnh số liệu theo robot thực tế."""
+
+from mprim_config import RobotConfig, KinematicType
+
+config = RobotConfig(
+    name="omnidirectional",                 # tên file output: omnidirectional.mprim
+    kinematic_type=KinematicType.OMNIDIRECTIONAL,  # holonomic -> nội suy tuyến tính x/y/theta
+
+    resolution_m=0.05,          # [m] PHẢI khớp costmap resolution
+    numberofangles=16,          # 16 góc heading -> 1 bước góc = 22.5 độ
+
+    forward_short_m=0.10,       # [m] bước tiến ngắn ~2 cell
+    forward_long_m=0.40,        # [m] bước tiến dài ~8 cell
+    backward_m=0.10,            # [m] bước lùi thẳng
+
+    turn_steps=(1,),            # chỉ có primitive rẽ 22.5 độ; thêm 2 nếu cần rẽ 45 độ
+    turn_radius_m=0.70,         # [m] bán kính cung rẽ (omni vẫn dùng cung cho mượt)
+
+    sidestep_m=0.15,            # [m] đặc thù omni: đi ngang giữ nguyên heading
+    diagonal_m=0.15,            # [m] đặc thù omni: đi chéo 45 độ giữ nguyên heading
+
+    has_backward_prims=True,        # cho phép lùi thẳng
+    has_backward_turn_prims=True,   # omni lùi không mù như diff-drive, giữ bật
+    has_turn_in_place_prims=True,   # cho phép quay tại chỗ
+
+    # SỐ NGUYÊN (SBPL đọc %d). Omni lùi/xoay "rẻ" hơn diff-drive vì không cần quay đầu.
+    forwardcostmult=1,          # tiến thẳng — mốc chuẩn
+    backwardcostmult=3,         # lùi rẻ hơn diff-drive (robot đối xứng, không mù hẳn)
+    forwardandturncostmult=3,   # vừa đi vừa rẽ
+    backwardandturncostmult=4,  # lùi + rẽ
+    turninplacecostmult=4,      # quay tại chỗ
+    sidestepcostmult=2,         # đi ngang — hơi đắt hơn tiến để không "cua" bằng sidestep
+    diagonalcostmult=2,         # đi chéo 45 độ
+
+    pose_spacing_m=0.05,        # [m] khoảng cách pose trên path
+
+    # chưa khai báo max_vel/max_yawrate: bỏ qua sanity-check động học
+)

generate_mprim.py

@@ -0,0 +1,66 @@
+#!/usr/bin/env python3
+"""
+generate_mprim.py
+------------------
+CLI để sinh file .mprim từ 1 file config trong configs/.
+
+Cách dùng:
+    python3 generate_mprim.py configs/differential.py
+    python3 generate_mprim.py configs/omnidirectional.py -o my_output.mprim
+    python3 generate_mprim.py configs/ackermann.py
+
+Muốn thêm robot mới: copy 1 file trong configs/, đổi số liệu, chạy lại.
+KHÔNG cần sửa code trong mprim_core.py / kinematics.py.
+"""
+
+import argparse
+import importlib.util
+import os
+import sys
+
+sys.path.insert(0, os.path.dirname(os.path.abspath(__file__)))
+
+from mprim_config import KinematicType
+from mprim_core import generate_mprim_file
+from kinematics import unicycle_poses, holonomic_poses
+
+# differential và ackermann dùng chung interpolator: đường đi của xe bánh lái theo
+# cung tròn trùng với nghiệm unicycle (đoạn thẳng + cung); khác nhau chỉ ở tập
+# primitive và ràng buộc min_turning_radius (mprim_core xử lý).
+KINEMATICS_MAP = {
+    KinematicType.DIFFERENTIAL: unicycle_poses,
+    KinematicType.OMNIDIRECTIONAL: holonomic_poses,
+    KinematicType.ACKERMANN: unicycle_poses,
+}
+
+
+def load_config(config_path: str):
+    spec = importlib.util.spec_from_file_location("robot_config_module", config_path)
+    module = importlib.util.module_from_spec(spec)
+    spec.loader.exec_module(module)
+    if not hasattr(module, "config"):
+        raise ValueError(f"{config_path} phải định nghĩa biến `config = RobotConfig(...)`")
+    return module.config
+
+
+def main():
+    parser = argparse.ArgumentParser(description="Sinh file .mprim từ config robot")
+    parser.add_argument("config", help="Đường dẫn tới file config Python (vd: configs/differential.py)")
+    parser.add_argument("-o", "--output", default=None, help="Đường dẫn file .mprim output")
+    args = parser.parse_args()
+
+    cfg = load_config(args.config)
+    kinematics_fn = KINEMATICS_MAP[cfg.kinematic_type]
+
+    output_path = args.output or f"{cfg.name}.mprim"
+    n, warnings = generate_mprim_file(cfg, kinematics_fn, output_path)
+
+    print(f"[OK] {cfg.kinematic_type.value}: đã sinh {n} primitives -> {output_path}")
+    if warnings:
+        print(f"[WARN] {len(warnings)} cảnh báo:")
+        for w in warnings:
+            print(f"  - {w}")
+
+
+if __name__ == "__main__":
+    main()

kinematics.py

@@ -0,0 +1,140 @@
+"""
+kinematics.py
+-------------
+Mỗi loại drive có một cách nội suy quỹ đạo (intermediate poses) khác nhau giữa
+điểm đầu và điểm cuối của 1 motion primitive. Đây là phần logic THỰC SỰ khác
+nhau giữa các robot — phần còn lại (symmetry theo góc, ghi file...) dùng chung.
+
+Mọi interpolator đều nhận:
+    startpt = (x, y, theta)   # luôn là (0, 0, currentangle)
+    endpt   = (x, y, theta)   # điểm kết thúc ĐÃ quantize về lưới (mét, rad)
+    dtheta_c        = số bước góc rời rạc thay đổi (int, có dấu, +: CCW)
+    numberofangles  = tổng số góc rời rạc
+    spacing_m       = khoảng cách mong muốn giữa 2 pose liên tiếp [m]
+    spacing_rad     = bước góc tối đa giữa 2 pose liên tiếp [rad]
+
+Trả về (poses, info):
+    poses: list (x, y, theta); pose đầu = startpt, pose cuối = ĐÚNG endpt,
+           heading cuối = ĐÚNG startangle + dtheta_c bước góc (SBPL yêu cầu để nạp file).
+    info:  dict chẩn đoán {"radius_m": bán kính cung (None nếu thẳng),
+                            "line_lead_m": đoạn thẳng dẫn vào của nghiệm line+arc}
+           — engine dùng để cảnh báo vi phạm min_turning_radius.
+
+Số sample tính THEO CHIỀU DÀI (n = ceil(L/spacing)+1) thay vì cố định — nhờ vậy
+khoảng cách pose ~ đều nhau trên mọi primitive dài ngắn khác nhau, và path SBPL
+ghép từ nhiều primitive cũng đều theo.
+"""
+
+import numpy as np
+
+
+def _num_samples(path_len_m, sweep_rad, spacing_m, spacing_rad):
+    """Số pose sao cho bước dài <= spacing_m VÀ bước góc <= spacing_rad."""
+    n_lin = int(np.ceil(abs(path_len_m) / spacing_m)) if spacing_m > 0 else 0
+    n_ang = int(np.ceil(abs(sweep_rad) / spacing_rad)) if spacing_rad > 0 else 0
+    return max(2, n_lin + 1, n_ang + 1)
+
+
+def _distribute_endpoint_error(poses, ex, ey):
+    """Dàn sai số điểm cuối tuyến tính dọc quỹ đạo để pose cuối khớp ĐÚNG lưới cell.
+
+    Chỉ dịch (x, y), KHÔNG đụng theta — heading đã chính xác theo cấu trúc nghiệm,
+    dịch vị trí nhỏ (<= nửa cell) chỉ gây lệch tangent vài độ, chấp nhận được.
+    """
+    errx = ex - poses[-1][0]
+    erry = ey - poses[-1][1]
+    n = len(poses)
+    return [(x + errx * i / (n - 1), y + erry * i / (n - 1), th)
+            for i, (x, y, th) in enumerate(poses)]
+
+
+def unicycle_poses(startpt, endpt, dtheta_c, numberofangles, spacing_m, spacing_rad):
+    """Nội suy cho robot nonholonomic (differential VÀ ackermann — cùng hình học đường đi).
+
+    Nghiệm "đoạn thẳng l + cung tròn bán kính k" giải từ hệ tuyến tính, y hệt
+    genmprim_unicycle*.m gốc. Khác bản gốc ở 2 điểm:
+    1. Khi dấu l mâu thuẫn với chiều đi (nghiệm đòi lùi rồi mới tiến), bản gốc chỉ ép
+       l = 0 mà giữ nguyên rv -> heading cuối lệch tới ~10 độ so với endpose (bug F2).
+       Ở đây ép l = 0 RỒI TÍNH LẠI rv = rotation_angle -> heading cuối luôn chính xác,
+       sai số dồn hết vào vị trí và được _distribute_endpoint_error dàn đều.
+    2. Sample theo chiều dài thay vì 10 điểm cố định.
+    """
+    sx, sy, sth = startpt
+    ex, ey, _ = endpt
+    rotation_angle = dtheta_c * (2 * np.pi / numberofangles)
+    eth_cont = sth + rotation_angle  # heading cuối liên tục (không wrap — sin/cos như nhau)
+    dist = np.hypot(ex - sx, ey - sy)
+
+    # --- đi thẳng / lùi không đổi hướng ---
+    if dtheta_c == 0:
+        n = _num_samples(dist, 0.0, spacing_m, spacing_rad)
+        # theta giữ = heading; hướng vector đi có thể lệch heading vài độ do endpose đã
+        # quantize về cell — sai số lưới, không tránh được, giống bản MATLAB gốc.
+        poses = [(sx + (ex - sx) * t, sy + (ey - sy) * t, sth)
+                 for t in np.linspace(0.0, 1.0, n)]
+        return poses, {"radius_m": None, "line_lead_m": None}
+
+    # --- quay tại chỗ ---
+    if dist < 1e-9:
+        n = _num_samples(0.0, rotation_angle, spacing_m, spacing_rad)
+        poses = [(sx, sy, sth + rotation_angle * t) for t in np.linspace(0.0, 1.0, n)]
+        return poses, {"radius_m": None, "line_lead_m": None}
+
+    # --- đoạn thẳng + cung tròn (unicycle, tv & rv hằng) ---
+    # [dx, dy]^T = [cos(sth), sin(eth)-sin(sth); sin(sth), -(cos(eth)-cos(sth))] @ [l, k]^T
+    # với l = chiều dài đoạn thẳng dẫn vào (có dấu), k = tv/rv = bán kính cung (có dấu).
+    R = np.array([
+        [np.cos(sth), np.sin(eth_cont) - np.sin(sth)],
+        [np.sin(sth), -(np.cos(eth_cont) - np.cos(sth))],
+    ])
+    b = np.array([ex - sx, ey - sy])
+    S = np.linalg.pinv(R) @ b
+    l = float(S[0])
+    k = float(S[1])
+    if abs(k) < 1e-9:
+        # endpose thẳng hàng với heading nhưng vẫn phải đổi góc: cung suy biến thành
+        # "đi thẳng rồi xoay gắt ở cuối". Xảy ra khi turn_radius quá nhỏ so với cell
+        # (validate đã cảnh báo) — vẫn sinh ra pose hợp lệ, không chia cho 0.
+        k = 1e-9 if k >= 0 else -1e-9
+
+    rv = rotation_angle + l / k
+    tv = k * rv
+    if (l < 0 < tv) or (l > 0 > tv):
+        # Nghiệm đòi đảo chiều trong 1 primitive (do quantize đẩy endpose ra sau cung).
+        # Ép l = 0 và TÍNH LẠI rv để heading cuối = rotation_angle chính xác.
+        l = 0.0
+        rv = rotation_angle
+        tv = k * rv
+
+    arc_len = abs(k * rotation_angle)
+    n = _num_samples(abs(l) + arc_len, rotation_angle, spacing_m, spacing_rad)
+    poses = []
+    for t in np.linspace(0.0, 1.0, n):
+        if tv != 0.0 and abs(t * tv) < abs(l):
+            # pha đi thẳng dọc heading ban đầu
+            poses.append((sx + t * tv * np.cos(sth),
+                          sy + t * tv * np.sin(sth),
+                          sth))
+        else:
+            denom = tv if tv != 0.0 else 1e-9
+            th = rv * (t - l / denom) + sth
+            poses.append((sx + l * np.cos(sth) + k * (np.sin(th) - np.sin(sth)),
+                          sy + l * np.sin(sth) - k * (np.cos(th) - np.cos(sth)),
+                          th))
+    # chốt heading cuối đúng tuyệt đối (tránh sai số tích lũy float)
+    poses[-1] = (poses[-1][0], poses[-1][1], eth_cont)
+    poses = _distribute_endpoint_error(poses, ex, ey)
+    return poses, {"radius_m": abs(k), "line_lead_m": l}
+
+
+def holonomic_poses(startpt, endpt, dtheta_c, numberofangles, spacing_m, spacing_rad):
+    """Holonomic (omni/mecanum/swerve): (x, y) độc lập với heading nên nội suy tuyến tính
+    cả vị trí lẫn theta — mô hình LINESEGMENT_MPRIMS của genmprim.m gốc."""
+    sx, sy, sth = startpt
+    ex, ey, _ = endpt
+    rotation_angle = dtheta_c * (2 * np.pi / numberofangles)
+    dist = np.hypot(ex - sx, ey - sy)
+    n = _num_samples(dist, rotation_angle, spacing_m, spacing_rad)
+    poses = [(sx + (ex - sx) * t, sy + (ey - sy) * t, sth + rotation_angle * t)
+             for t in np.linspace(0.0, 1.0, n)]
+    return poses, {"radius_m": None, "line_lead_m": None}

mprim_config.py

@@ -0,0 +1,198 @@
+"""
+mprim_config.py
+----------------
+Cấu hình robot dùng chung cho mọi loại kinematic (differential / omnidirectional / ackermann).
+
+Chỉnh các thông số trong file configs/*.py theo robot thực tế của bạn.
+File này KHÔNG chứa logic sinh primitive — chỉ định nghĩa dữ liệu + validate.
+
+Ràng buộc quan trọng của format SBPL (lý do cho các validate bên dưới):
+- `additionalactioncostmult` được SBPL đọc bằng fscanf("%d") -> BẮT BUỘC là số nguyên,
+  giá trị lẻ (1.5, 2.5...) làm hỏng cả file ngay từ primitive đầu tiên chứa nó.
+- Pose cuối của quỹ đạo phải khớp endpose sau khi rời rạc hóa, nếu lệch quá nửa bước góc
+  (= 180/numberofangles độ) SBPL từ chối nạp file.
+- Khoảng cách giữa 2 intermediate pose liên tiếp không được vượt quá 1 cell, nếu không
+  bước quét footprint (get_2d_motion_cells) sẽ bỏ sót cell trên đường đi.
+"""
+
+import math
+from dataclasses import dataclass, field
+from enum import Enum
+from typing import Optional, Tuple
+
+
+class KinematicType(str, Enum):
+    DIFFERENTIAL = "differential"        # 2 bánh vi sai (unicycle model)
+    OMNIDIRECTIONAL = "omnidirectional"  # mecanum / omni / swerve (holonomic)
+    ACKERMANN = "ackermann"              # xe có bánh lái, bán kính quay tối thiểu
+
+
+@dataclass
+class RobotConfig:
+    # --- Thông tin chung ---
+    name: str                          # tên robot; dùng đặt tên file output <name>.mprim
+    kinematic_type: KinematicType      # differential / omnidirectional / ackermann — quyết
+                                       # định tập primitive và cách nội suy quỹ đạo
+
+    # --- Lưới rời rạc hóa (phải khớp với costmap / planner config) ---
+    resolution_m: float = 0.05         # [m] kích thước 1 cell — PHẢI khớp costmap resolution,
+                                       # SBPL so sánh và từ chối file nếu lệch
+    numberofangles: int = 16           # số góc heading rời rạc; bội số của 8, khuyến nghị 16
+                                       # (16 góc -> 1 bước góc = 22.5 độ)
+
+    # --- Primitive đi thẳng ---
+    forward_short_m: float = 0.10      # [m] bước tiến ngắn, dùng để tinh chỉnh vị trí gần
+                                       # goal; >= 0.75*resolution để không quantize về (0,0)
+    forward_long_m: float = 0.40       # [m] bước tiến dài — quãng đường ~1 chu kỳ điều khiển
+                                       # ở tốc độ hành trình; quyết định tốc độ expand của A*
+    backward_m: float = 0.10           # [m] bước lùi thẳng (chỉ dùng khi has_backward_prims)
+
+    # --- Primitive vừa đi vừa rẽ ---
+    # Rẽ được mô tả bằng CUNG TRÒN: turn_steps liệt kê số bước góc rời rạc mỗi primitive rẽ
+    # (1 = 22.5 độ với 16 góc), turn_radius_m là bán kính cung [m]. Điểm cuối được tự tính:
+    #   dx = R*sin(dtheta), dy = R*(1 - cos(dtheta))
+    # -> heading cuối LUÔN khớp góc rời rạc đích, không phải đoán độ lệch ngang bằng tay.
+    turn_steps: Tuple[int, ...] = (1,) # các mức rẽ; (1,) = chỉ rẽ 22.5 độ, (1, 2) thêm rẽ 45
+                                       # độ — mỗi mức thêm 2 primitive/góc (CCW + CW),
+                                       # branching factor tăng -> planner chậm hơn
+    turn_radius_m: float = 0.70        # [m] bán kính cung rẽ. Nên >= max_vel/max_yawrate để
+                                       # local planner bám được không phải giảm tốc; và >=
+                                       # resolution/(1-cos(bước góc)) (~0.66m với lưới 5cm/16
+                                       # góc) để quantize không làm méo cung. Ackermann: bắt
+                                       # buộc >= min_turning_radius_m.
+
+    # --- Chỉ dùng cho OMNIDIRECTIONAL ---
+    sidestep_m: float = 0.10           # [m] bước đi ngang (trái/phải) giữ nguyên heading
+    diagonal_m: float = 0.10           # [m] chiều dài bước đi chéo 45 độ giữ nguyên heading
+
+    # --- Chỉ dùng cho ACKERMANN ---
+    min_turning_radius_m: float = 0.35  # [m] bán kính quay tối thiểu VẬT LÝ (đo từ robot
+                                        # thật, không trượt bánh) — generator cảnh báo nếu
+                                        # cung sau quantize vi phạm giá trị này
+
+    # --- Bật/tắt từng họ primitive ---
+    has_backward_prims: bool = True         # cho phép lùi thẳng (tắt cho robot kéo hàng
+                                            # không được lùi kiểu noreverse_trolley)
+    has_backward_turn_prims: bool = False   # cho phép lùi + rẽ; mặc định tắt vì AMR cảm biến
+                                            # chính hướng trước thì lùi-rẽ là hành động mù
+    has_turn_in_place_prims: bool = True    # quay tại chỗ (ackermann thật thường phải tắt)
+
+    # --- Hệ số chi phí, theo convention SBPL: cost thật = cost_thời_gian * costmult ---
+    # BẮT BUỘC số nguyên >= 1 (SBPL đọc bằng %d). Tham khảo bộ tuned unicycle_highcost:
+    # backward=40, turninplace=20, forwardandturn=2 — lùi/xoay tại chỗ đắt tới mức chỉ được
+    # chọn khi không còn đường nào khác.
+    forwardcostmult: int = 1           # tiến thẳng — mốc chuẩn, luôn để 1
+    backwardcostmult: int = 40         # lùi thẳng; 40 nghĩa là lùi 0.1m "đắt" bằng tiến 4m
+    forwardandturncostmult: int = 2    # vừa tiến vừa rẽ; >1 để planner ưu tiên đường thẳng
+    backwardandturncostmult: int = 40  # lùi + rẽ (chỉ dùng khi has_backward_turn_prims)
+    turninplacecostmult: int = 20      # quay tại chỗ; giảm xuống nếu muốn robot xoay thay vì
+                                       # đi vòng, nhưng path sẽ xuất hiện cụm pose trùng điểm
+    sidestepcostmult: int = 10         # đi ngang (chỉ omni)
+    diagonalcostmult: int = 2          # đi chéo 45 độ (chỉ omni)
+
+    # --- Mật độ intermediate poses (quyết định độ đều của path SBPL trả ra) ---
+    # Số sample được tính THEO CHIỀU DÀI: n = ceil(L / pose_spacing_m) + 1, nên mọi primitive
+    # dài ngắn khác nhau đều có khoảng cách pose ~ pose_spacing_m -> path ra đều nhau.
+    pose_spacing_m: float = 0.05        # [m] khoảng cách giữa 2 pose liên tiếp; phải <=
+                                        # resolution_m để footprint sweep không bỏ sót cell
+    pose_angular_spacing_deg: float = 5.625  # [deg] bước góc tối đa giữa 2 pose (quay tại chỗ
+                                             # và phần cung tròn; footprint sweep cần đủ mịn)
+
+    # --- Giới hạn động học để sanity-check (tùy chọn, None = bỏ qua) ---
+    max_vel_mps: Optional[float] = None        # [m/s] vận tốc dài tối đa của robot thật
+    max_yawrate_radps: Optional[float] = None  # [rad/s] vận tốc xoay tối đa; cùng max_vel
+                                               # dùng để cảnh báo turn_radius quá gắt
+
+    def validate(self):
+        """Raise ValueError nếu config sai; trả về list cảnh báo (không chặn generate)."""
+        errors = []
+        warnings = []
+
+        if self.numberofangles % 8 != 0:
+            errors.append("numberofangles phải là bội số của 8 (khuyến nghị 16 hoặc 32).")
+        if self.resolution_m <= 0:
+            errors.append("resolution_m phải > 0.")
+
+        # costmult: SBPL đọc bằng fscanf("%d") — số lẻ làm hỏng cả file
+        for f in ("forwardcostmult", "backwardcostmult", "forwardandturncostmult",
+                  "backwardandturncostmult", "turninplacecostmult",
+                  "sidestepcostmult", "diagonalcostmult"):
+            v = getattr(self, f)
+            if not isinstance(v, int) or isinstance(v, bool) or v < 1:
+                errors.append(f"{f} = {v!r}: costmult phải là SỐ NGUYÊN >= 1 "
+                              "(SBPL đọc bằng fscanf %d, giá trị lẻ làm hỏng file).")
+
+        # độ dài primitive thẳng: quá ngắn sẽ quantize về (0,0) ở góc 45 độ -> self-loop
+        min_len = self.resolution_m * 0.75
+        for f in ("forward_short_m", "forward_long_m"):
+            if getattr(self, f) < min_len:
+                errors.append(f"{f} = {getattr(self, f)}: phải >= 0.75*resolution_m "
+                              f"({min_len:.3f}m), nếu không endpose quantize về (0,0) ở góc chéo.")
+            elif getattr(self, f) < self.resolution_m:
+                warnings.append(f"{f} < resolution_m: primitive chỉ đi được ~1 cell, "
+                                "cân nhắc tăng lên.")
+        if self.has_backward_prims and self.backward_m < min_len:
+            errors.append(f"backward_m = {self.backward_m}: phải >= {min_len:.3f}m "
+                          "(hoặc tắt has_backward_prims).")
+
+        # turn prims
+        if not self.turn_steps:
+            errors.append("turn_steps rỗng: cần ít nhất 1 bước góc (vd (1,)).")
+        for s in self.turn_steps:
+            if not isinstance(s, int) or s < 1 or s > self.numberofangles // 4:
+                errors.append(f"turn_steps chứa {s!r}: phải là int trong "
+                              f"[1, {self.numberofangles // 4}] (tối đa 90 độ mỗi primitive).")
+        if len(set(self.turn_steps)) != len(self.turn_steps):
+            errors.append("turn_steps có phần tử trùng nhau.")
+        if self.turn_radius_m <= 0:
+            errors.append("turn_radius_m phải > 0.")
+        else:
+            step_rad = 2 * math.pi / self.numberofangles
+            min_step = min(self.turn_steps or [1])
+            dy_min = self.turn_radius_m * (1 - math.cos(min_step * step_rad))
+            if dy_min < self.resolution_m:
+                # dy < 1 cell nghĩa là sai số quantize cùng cỡ với chính hình dạng cung ->
+                # cung bị méo nặng, bán kính thực tế sau quantize có thể nhỏ hơn nhiều
+                r_need = self.resolution_m / (1 - math.cos(min_step * step_rad))
+                warnings.append(
+                    f"turn_radius_m = {self.turn_radius_m}: độ lệch ngang của cung rẽ nhỏ nhất "
+                    f"({dy_min:.3f}m) < 1 cell -> quantize làm méo cung đáng kể. "
+                    f"Khuyến nghị turn_radius_m >= {r_need:.2f}m với lưới hiện tại.")
+
+        # sampling
+        if self.pose_spacing_m <= 0:
+            errors.append("pose_spacing_m phải > 0.")
+        elif self.pose_spacing_m > self.resolution_m:
+            errors.append(f"pose_spacing_m = {self.pose_spacing_m} > resolution_m: "
+                          "footprint sweep của SBPL sẽ bỏ sót cell giữa 2 pose liên tiếp.")
+        if self.pose_angular_spacing_deg <= 0:
+            errors.append("pose_angular_spacing_deg phải > 0.")
+
+        # giới hạn động học (tùy chọn)
+        if self.max_vel_mps is not None and self.max_yawrate_radps is not None:
+            r_min_kin = self.max_vel_mps / self.max_yawrate_radps
+            if self.turn_radius_m < r_min_kin:
+                warnings.append(
+                    f"turn_radius_m = {self.turn_radius_m} < max_vel/max_yawrate = "
+                    f"{r_min_kin:.3f}m: local planner phải giảm tốc mới bám được cung rẽ này.")
+
+        # ackermann
+        if self.kinematic_type == KinematicType.ACKERMANN:
+            if self.min_turning_radius_m <= 0:
+                errors.append("Ackermann cần min_turning_radius_m > 0.")
+            elif self.turn_radius_m < self.min_turning_radius_m:
+                errors.append(f"turn_radius_m = {self.turn_radius_m} < min_turning_radius_m = "
+                              f"{self.min_turning_radius_m}: primitive vi phạm giới hạn lái vật lý.")
+            if self.has_turn_in_place_prims:
+                warnings.append("Ackermann bật has_turn_in_place_prims: xe bánh lái thường "
+                                "KHÔNG quay tại chỗ được — chỉ giữ nếu robot có bánh xoay đặc biệt.")
+
+        # omni
+        if self.kinematic_type == KinematicType.OMNIDIRECTIONAL:
+            for f in ("sidestep_m", "diagonal_m"):
+                if getattr(self, f) < min_len:
+                    errors.append(f"{f} = {getattr(self, f)}: phải >= {min_len:.3f}m.")
+
+        if errors:
+            raise ValueError("Config không hợp lệ:\n  - " + "\n  - ".join(errors))
+        return warnings

mprim_core.py

@@ -0,0 +1,216 @@
+"""
+mprim_core.py
+-------------
+Logic dùng CHUNG cho mọi robot: định nghĩa tập primitive theo hệ tọa độ gắn với
+thân robot (body frame, heading = 0), tự động xoay sang từng góc rời rạc
+(0..numberofangles-1), quantize về lưới cell, nội suy quỹ đạo và ghi file .mprim.
+
+Khác các file .m gốc (định nghĩa tay 3 bộ template cell riêng cho 0/45/22.5 độ),
+ở đây primitive được định nghĩa 1 lần THEO NGHĨA VẬT LÝ (mét, bán kính cung) rồi
+xoay + làm tròn cho từng góc. Đánh đổi là sai số lượng tử hóa nhỏ ở các góc lẻ
+(< nửa cell, đã được dàn đều dọc quỹ đạo) — chấp nhận được với AMR thực tế.
+
+Sau khi sinh, engine TỰ KIỂM những ràng buộc mà SBPL sẽ enforce lúc nạp file
+(xem ReadinMotionPrimitive trong environment_navxythetalat.cpp) để lỗi lộ ra
+ngay lúc generate thay vì lúc khởi động planner trên robot.
+"""
+
+import math
+from dataclasses import dataclass
+from typing import Callable, Dict, List, Tuple
+
+import numpy as np
+
+from mprim_config import RobotConfig, KinematicType
+
+
+@dataclass
+class Primitive:
+    dx_body_m: float   # [m] dịch chuyển dọc trục robot (body frame)
+    dy_body_m: float   # [m] dịch chuyển ngang (body frame, +: trái)
+    dtheta_c: int      # số bước góc rời rạc thay đổi (+: CCW)
+    costmult: int      # hệ số chi phí SBPL (số nguyên, đã validate)
+    label: str = ""
+
+
+def _turn_endpoint(radius_m: float, steps: int, numberofangles: int) -> Tuple[float, float]:
+    """Điểm cuối body-frame của cung tròn bán kính radius_m quay steps bước góc.
+
+    dx = R*sin(|dtheta|), dy = sign(steps)*R*(1-cos(|dtheta|)) — heading cuối trùng
+    khít góc rời rạc đích theo cấu trúc, không cần người dùng đoán độ lệch ngang.
+    """
+    dtheta = abs(steps) * 2.0 * math.pi / numberofangles
+    return radius_m * math.sin(dtheta), math.copysign(radius_m * (1.0 - math.cos(dtheta)), steps)
+
+
+def _nonholonomic_prims(cfg: RobotConfig, turn_radius_m: float) -> List[Primitive]:
+    """Tập primitive chung cho differential và ackermann (chỉ khác toggle/costmult)."""
+    prims = [
+        Primitive(cfg.forward_short_m, 0.0, 0, cfg.forwardcostmult, "fwd_short"),
+        Primitive(cfg.forward_long_m, 0.0, 0, cfg.forwardcostmult, "fwd_long"),
+    ]
+    if cfg.has_backward_prims:
+        prims.append(Primitive(-cfg.backward_m, 0.0, 0, cfg.backwardcostmult, "bwd"))
+    for s in cfg.turn_steps:
+        dx, dy = _turn_endpoint(turn_radius_m, s, cfg.numberofangles)
+        prims += [
+            Primitive(dx, dy, s, cfg.forwardandturncostmult, f"fwd_turn_ccw{s}"),
+            Primitive(dx, -dy, -s, cfg.forwardandturncostmult, f"fwd_turn_cw{s}"),
+        ]
+    if cfg.has_turn_in_place_prims:
+        prims += [
+            Primitive(0.0, 0.0, 1, cfg.turninplacecostmult, "turn_in_place_ccw"),
+            Primitive(0.0, 0.0, -1, cfg.turninplacecostmult, "turn_in_place_cw"),
+        ]
+    if cfg.has_backward_turn_prims:
+        # lùi theo cung: heading quay CCW (+s) thì điểm cuối lệch về (-x, -y) — suy từ
+        # tích phân unicycle với tv < 0, rv > 0.
+        for s in cfg.turn_steps:
+            dx, dy = _turn_endpoint(turn_radius_m, s, cfg.numberofangles)
+            prims += [
+                Primitive(-dx, -dy, s, cfg.backwardandturncostmult, f"bwd_turn_ccw{s}"),
+                Primitive(-dx, dy, -s, cfg.backwardandturncostmult, f"bwd_turn_cw{s}"),
+            ]
+    return prims
+
+
+def build_primitive_set(cfg: RobotConfig) -> List[Primitive]:
+    """Sinh tập primitive body-frame theo loại kinematic."""
+    if cfg.kinematic_type == KinematicType.DIFFERENTIAL:
+        return _nonholonomic_prims(cfg, cfg.turn_radius_m)
+
+    if cfg.kinematic_type == KinematicType.ACKERMANN:
+        # validate() đã đảm bảo turn_radius_m >= min_turning_radius_m
+        return _nonholonomic_prims(cfg, cfg.turn_radius_m)
+
+    if cfg.kinematic_type == KinematicType.OMNIDIRECTIONAL:
+        # kế thừa toàn bộ primitive nonholonomic (vẫn hợp lệ với omni)...
+        prims = _nonholonomic_prims(cfg, cfg.turn_radius_m)
+        # ...cộng khả năng đặc thù omni: đi ngang & đi chéo 45 độ giữ nguyên heading
+        diag = cfg.diagonal_m / math.sqrt(2.0)
+        prims += [
+            Primitive(0.0, cfg.sidestep_m, 0, cfg.sidestepcostmult, "sidestep_left"),
+            Primitive(0.0, -cfg.sidestep_m, 0, cfg.sidestepcostmult, "sidestep_right"),
+            Primitive(diag, diag, 0, cfg.diagonalcostmult, "diag_fwd_left"),
+            Primitive(diag, -diag, 0, cfg.diagonalcostmult, "diag_fwd_right"),
+        ]
+        return prims
+
+    raise ValueError(f"Chưa hỗ trợ kinematic_type={cfg.kinematic_type}")
+
+
+# --- Tái hiện đúng phép rời rạc hóa của SBPL (environment_navxythetalat.cpp) ---
+
+def _contxy2disc(x: float, cellsize: float) -> int:
+    return int(math.floor(x / cellsize))
+
+
+def _conttheta2disc(theta: float, numberofangles: int) -> int:
+    step = 2.0 * math.pi / numberofangles
+    return int(round(((theta % (2.0 * math.pi)) / step))) % numberofangles
+
+
+def _sbpl_endpose_check(poses, endcell, resolution: float, numberofangles: int):
+    """Đúng phép kiểm tra SBPL chạy khi nạp file: pose cuối (tính từ TÂM cell (0,0))
+    phải rời rạc hóa ra đúng endcell — sai là SBPL từ chối cả file."""
+    half = resolution / 2.0
+    lx, ly, lth = poses[-1]
+    got = (_contxy2disc(half + lx, resolution),
+           _contxy2disc(half + ly, resolution),
+           _conttheta2disc(lth, numberofangles))
+    return got == tuple(endcell), got
+
+
+def generate_mprim_file(cfg: RobotConfig, kinematics_fn: Callable, output_path: str):
+    """Sinh file .mprim. Trả về (số primitive, list cảnh báo).
+
+    kinematics_fn(startpt, endpt, dtheta_c, numberofangles, spacing_m, spacing_rad)
+        -> (List[(x, y, theta)], info_dict)
+    """
+    warnings = list(cfg.validate())
+    prims = build_primitive_set(cfg)
+    numberofangles = cfg.numberofangles
+    resolution = cfg.resolution_m
+    spacing_rad = math.radians(cfg.pose_angular_spacing_deg)
+    ang_step = 2.0 * math.pi / numberofangles
+
+    lines = [
+        f"resolution_m: {resolution:.6f}",
+        f"numberofangles: {numberofangles}",
+        f"totalnumberofprimitives: {len(prims) * numberofangles}",
+    ]
+
+    worst_radius: Dict[str, float] = {}
+    for angleind in range(numberofangles):
+        currentangle = angleind * ang_step
+        endposes_seen: Dict[Tuple[int, int, int], str] = {}
+        for primid, p in enumerate(prims):
+            # xoay primitive (body frame) sang world frame tại góc hiện tại
+            dx_world = p.dx_body_m * math.cos(currentangle) - p.dy_body_m * math.sin(currentangle)
+            dy_world = p.dx_body_m * math.sin(currentangle) + p.dy_body_m * math.cos(currentangle)
+
+            endx_c = int(round(dx_world / resolution))
+            endy_c = int(round(dy_world / resolution))
+            endtheta_c = (angleind + p.dtheta_c) % numberofangles
+
+            # primitive suy biến: không đi, không xoay -> self-loop trong đồ thị SBPL
+            if endx_c == 0 and endy_c == 0 and p.dtheta_c == 0:
+                raise ValueError(
+                    f"Primitive '{p.label}' quantize về (0,0,không đổi góc) tại "
+                    f"startangle={angleind}: bước đi quá ngắn so với resolution.")
+
+            key = (endx_c, endy_c, endtheta_c)
+            if key in endposes_seen:
+                warnings.append(
+                    f"Trùng endpose {key} tại startangle={angleind}: '{p.label}' và "
+                    f"'{endposes_seen[key]}' — successor thừa, tốn expansion vô ích.")
+            endposes_seen[key] = p.label
+
+            startpt = (0.0, 0.0, currentangle)
+            endpt = (endx_c * resolution, endy_c * resolution, endtheta_c * ang_step)
+            poses, info = kinematics_fn(startpt, endpt, p.dtheta_c, numberofangles,
+                                        cfg.pose_spacing_m, spacing_rad)
+
+            # --- tự kiểm các ràng buộc SBPL enforce lúc nạp file ---
+            heading_err = abs((poses[-1][2] - endtheta_c * ang_step + math.pi)
+                              % (2.0 * math.pi) - math.pi)
+            if heading_err > 1e-6:
+                raise RuntimeError(
+                    f"BUG nội bộ: heading cuối lệch {math.degrees(heading_err):.3f} độ tại "
+                    f"'{p.label}' startangle={angleind} — interpolator phải trả heading chính xác.")
+            ok, got = _sbpl_endpose_check(poses, key, resolution, numberofangles)
+            if not ok:
+                raise RuntimeError(
+                    f"BUG nội bộ: pose cuối rời rạc hóa ra {got} != endpose {key} tại "
+                    f"'{p.label}' startangle={angleind} — SBPL sẽ từ chối file này.")
+            max_gap = max((math.hypot(x1 - x0, y1 - y0)
+                           for (x0, y0, _), (x1, y1, _) in zip(poses, poses[1:])),
+                          default=0.0)
+            if max_gap > resolution * 1.05:
+                warnings.append(
+                    f"Bước giữa 2 pose = {max_gap:.3f}m > 1 cell tại '{p.label}' "
+                    f"startangle={angleind}: footprint sweep có thể bỏ sót cell.")
+            if (cfg.kinematic_type == KinematicType.ACKERMANN
+                    and info.get("radius_m") is not None
+                    and info["radius_m"] < cfg.min_turning_radius_m * 0.999):
+                worst_radius[p.label] = min(worst_radius.get(p.label, math.inf),
+                                            info["radius_m"])
+
+            lines.append(f"primID: {primid}")
+            lines.append(f"startangle_c: {angleind}")
+            lines.append(f"endpose_c: {endx_c} {endy_c} {endtheta_c}")
+            lines.append(f"additionalactioncostmult: {p.costmult:d}")
+            lines.append(f"intermediateposes: {len(poses)}")
+            for (x, y, th) in poses:
+                lines.append(f"{x:.4f} {y:.4f} {th:.4f}")
+
+    for label, r in sorted(worst_radius.items()):
+        warnings.append(
+            f"'{label}': bán kính cung sau quantize nhỏ nhất = {r:.3f}m < "
+            f"min_turning_radius_m = {cfg.min_turning_radius_m}m — xe thật không bám được, "
+            "tăng turn_radius_m để có dự phòng quantize.")
+
+    with open(output_path, "w") as f:
+        f.write("\n".join(lines) + "\n")
+
+    return len(prims) * numberofangles, warnings

test_mprim_gen.py

@@ -0,0 +1,198 @@
+#!/usr/bin/env python3
+"""
+test_mprim_gen.py
+------------------
+Test tự động cho generator: sinh cả 3 config mẫu ra thư mục tạm rồi kiểm tra
+output theo đúng các ràng buộc SBPL enforce lúc nạp file + chất lượng hình học.
+
+Chạy:
+    python3 test_mprim_gen.py
+
+Các kiểm tra:
+  1. Header khớp số primitive thực tế; parse trọn vẹn không dư token.
+  2. additionalactioncostmult là SỐ NGUYÊN (SBPL đọc fscanf %d).
+  3. Pose cuối rời rạc hóa ra đúng endpose_c (đúng phép check của
+     ReadinMotionPrimitive — sai là SBPL từ chối cả file).
+  4. Heading cuối khớp CHÍNH XÁC góc rời rạc đích (bug l=0 cũ gây lệch 10 độ).
+  5. Khoảng cách 2 pose liên tiếp <= 1.05 cell (footprint sweep không sót cell)
+     và ĐỀU trong từng primitive (max/min <= 1.5).
+  6. Vi phạm nonholonomic (góc lệch vector di chuyển vs heading) < 9 độ với
+     differential/ackermann — bắt lỗi kiểu cung lùi sai hình học (28 độ cũ).
+  7. Không có primitive suy biến, không trùng endpose trong cùng start angle.
+  8. Config sai (costmult lẻ, spacing > resolution, ackermann turn_radius <
+     min_turning_radius) phải bị validate() chặn.
+"""
+
+import math
+import os
+import sys
+import tempfile
+
+sys.path.insert(0, os.path.dirname(os.path.abspath(__file__)))
+
+from mprim_config import RobotConfig, KinematicType
+from mprim_core import generate_mprim_file
+from generate_mprim import KINEMATICS_MAP, load_config
+
+CONFIG_DIR = os.path.join(os.path.dirname(os.path.abspath(__file__)), "configs")
+CHECKS = []
+
+
+def check(name, cond, detail=""):
+    CHECKS.append((name, cond, detail))
+    status = "PASS" if cond else "FAIL"
+    print(f"  [{status}] {name}" + (f" — {detail}" if detail and not cond else ""))
+
+
+def parse(path):
+    toks = open(path).read().split()
+    it = iter(toks)
+    def nxt():
+        return next(it)
+    assert nxt() == "resolution_m:"
+    res = float(nxt())
+    assert nxt() == "numberofangles:"
+    nang = int(nxt())
+    assert nxt() == "totalnumberofprimitives:"
+    ntot = int(nxt())
+    prims = []
+    while True:
+        try:
+            tag = nxt()
+        except StopIteration:
+            break
+        assert tag == "primID:"
+        pid = int(nxt())
+        assert nxt() == "startangle_c:"
+        sa = int(nxt())
+        assert nxt() == "endpose_c:"
+        end = (int(nxt()), int(nxt()), int(nxt()))
+        assert nxt() == "additionalactioncostmult:"
+        cm = nxt()
+        assert nxt() == "intermediateposes:"
+        n = int(nxt())
+        poses = [(float(nxt()), float(nxt()), float(nxt())) for _ in range(n)]
+        prims.append(dict(pid=pid, sa=sa, end=end, cm=cm, poses=poses))
+    return res, nang, ntot, prims
+
+
+def ang_diff(a, b):
+    return (a - b + math.pi) % (2 * math.pi) - math.pi
+
+
+def contxy2disc(x, cell):
+    return int(math.floor(x / cell))
+
+
+def verify_file(path, kinematic, min_turning_radius=None):
+    res, nang, ntot, prims = parse(path)
+    step = 2 * math.pi / nang
+
+    check("header khớp số primitive parse được", ntot == len(prims),
+          f"header {ntot} != parsed {len(prims)}")
+
+    bad_cm = [p["cm"] for p in prims if not p["cm"].lstrip("-").isdigit()]
+    check("costmult toàn số nguyên (fscanf %d)", not bad_cm, f"lẻ: {sorted(set(bad_cm))}")
+
+    # phép check của SBPL ReadinMotionPrimitive
+    bad_end = 0
+    for p in prims:
+        lx, ly, lth = p["poses"][-1]
+        got = (contxy2disc(res / 2 + lx, res), contxy2disc(res / 2 + ly, res),
+               int(round((lth % (2 * math.pi)) / step)) % nang)
+        if got != p["end"]:
+            bad_end += 1
+    check("pose cuối rời rạc hóa đúng endpose_c (điều kiện SBPL nạp file)", bad_end == 0,
+          f"{bad_end} primitive sai")
+
+    worst_heading = max(abs(ang_diff(p["poses"][-1][2], p["end"][2] * step)) for p in prims)
+    check("heading cuối khớp chính xác góc đích", worst_heading < 1e-4,
+          f"lệch tối đa {math.degrees(worst_heading):.2f} độ")
+
+    worst_gap, worst_ratio = 0.0, 1.0
+    for p in prims:
+        gaps = [math.hypot(x1 - x0, y1 - y0)
+                for (x0, y0, _), (x1, y1, _) in zip(p["poses"], p["poses"][1:])]
+        moving = [g for g in gaps if g > 1e-9]
+        if moving:
+            worst_gap = max(worst_gap, max(moving))
+            worst_ratio = max(worst_ratio, max(moving) / min(moving))
+    check("bước giữa 2 pose <= 1.05 cell (footprint sweep không sót)",
+          worst_gap <= res * 1.05, f"max {worst_gap:.4f}m")
+    check("spacing đều trong từng primitive (max/min <= 1.5)", worst_ratio <= 1.5,
+          f"tỉ lệ {worst_ratio:.2f}")
+
+    if kinematic in (KinematicType.DIFFERENTIAL, KinematicType.ACKERMANN):
+        worst_nh = 0.0
+        for p in prims:
+            if p["end"][0] == 0 and p["end"][1] == 0:
+                continue
+            for (x0, y0, t0), (x1, y1, t1) in zip(p["poses"], p["poses"][1:]):
+                dx, dy = x1 - x0, y1 - y0
+                if math.hypot(dx, dy) < 1e-9:
+                    continue
+                hd = math.atan2(dy, dx)
+                th = 0.5 * (t0 + t1)
+                worst_nh = max(worst_nh, min(abs(ang_diff(hd, th)),
+                                             abs(ang_diff(hd, th + math.pi))))
+        check("vi phạm nonholonomic < 9 độ", math.degrees(worst_nh) < 9.0,
+              f"max {math.degrees(worst_nh):.2f} độ")
+
+    degenerate = [p for p in prims
+                  if p["end"] == (0, 0, p["sa"])]
+    check("không có primitive suy biến (0,0,không đổi góc)", not degenerate,
+          f"{len(degenerate)} cái")
+
+    dup = 0
+    for sa in range(nang):
+        seen = set()
+        for p in prims:
+            if p["sa"] != sa:
+                continue
+            if p["end"] in seen:
+                dup += 1
+            seen.add(p["end"])
+    check("không trùng endpose trong cùng start angle", dup == 0, f"{dup} cặp trùng")
+
+
+def test_configs_generate():
+    with tempfile.TemporaryDirectory() as tmp:
+        for fname in ("differential.py", "omnidirectional.py", "ackermann.py"):
+            cfg = load_config(os.path.join(CONFIG_DIR, fname))
+            out = os.path.join(tmp, cfg.name + ".mprim")
+            n, warnings = generate_mprim_file(cfg, KINEMATICS_MAP[cfg.kinematic_type], out)
+            print(f"\n== {fname}: {n} primitives, {len(warnings)} cảnh báo ==")
+            for w in warnings:
+                print(f"   (warn) {w}")
+            verify_file(out, cfg.kinematic_type,
+                        min_turning_radius=getattr(cfg, "min_turning_radius_m", None))
+
+
+def test_validation_rejects_bad_configs():
+    print("\n== validate() phải chặn config sai ==")
+
+    def must_fail(name, **overrides):
+        base = dict(name="t", kinematic_type=KinematicType.DIFFERENTIAL)
+        base.update(overrides)
+        try:
+            RobotConfig(**base).validate()
+            check(name, False, "validate() đã KHÔNG chặn")
+        except ValueError:
+            check(name, True)
+
+    must_fail("chặn costmult không nguyên", forwardandturncostmult=2.5)
+    must_fail("chặn costmult < 1", backwardcostmult=0)
+    must_fail("chặn pose_spacing_m > resolution", pose_spacing_m=0.08)
+    must_fail("chặn forward quá ngắn (quantize về 0,0)", forward_short_m=0.02)
+    must_fail("chặn turn_steps rỗng", turn_steps=())
+    must_fail("chặn ackermann turn_radius < min_turning_radius",
+              kinematic_type=KinematicType.ACKERMANN,
+              min_turning_radius_m=0.45, turn_radius_m=0.30)
+
+
+if __name__ == "__main__":
+    test_configs_generate()
+    test_validation_rejects_bad_configs()
+    failed = [c for c in CHECKS if not c[1]]
+    print(f"\n{'='*60}\nTổng: {len(CHECKS)} check, {len(failed)} FAIL")
+    sys.exit(1 if failed else 0)

visualize_mprim.py

@@ -0,0 +1,92 @@
+#!/usr/bin/env python3
+"""
+visualize_mprim.py
+-------------------
+Vẽ nhanh tất cả primitive tại 1 góc bắt đầu (startangle) để kiểm tra bằng mắt
+trước khi tích hợp vào planner thật.
+
+Cách dùng:
+    python3 visualize_mprim.py diff.mprim --angle 0
+    python3 visualize_mprim.py omni.mprim --angle 4
+"""
+
+import argparse
+import matplotlib
+matplotlib.use("Agg")
+import matplotlib.pyplot as plt
+
+
+def parse_mprim(path):
+    with open(path) as f:
+        lines = [l.strip() for l in f.readlines()]
+
+    i = 0
+    resolution = float(lines[i].split(":")[1]); i += 1
+    numberofangles = int(lines[i].split(":")[1]); i += 1
+    total = int(lines[i].split(":")[1]); i += 1
+
+    prims = []
+    while i < len(lines) and lines[i]:
+        primid = int(lines[i].split(":")[1]); i += 1
+        startangle = int(lines[i].split(":")[1]); i += 1
+        endpose = lines[i].split(":")[1].split(); i += 1
+        costmult = float(lines[i].split(":")[1]); i += 1
+        n = int(lines[i].split(":")[1]); i += 1
+        poses = []
+        for _ in range(n):
+            x, y, th = map(float, lines[i].split())
+            poses.append((x, y, th))
+            i += 1
+        prims.append({
+            "primid": primid, "startangle": startangle,
+            "endpose_c": [int(v) for v in endpose],
+            "costmult": costmult, "poses": poses
+        })
+    return resolution, numberofangles, prims
+
+
+def main():
+    ap = argparse.ArgumentParser()
+    ap.add_argument("mprim_file")
+    ap.add_argument("--angle", type=int, default=0, help="startangle_c cần vẽ")
+    ap.add_argument("-o", "--output", default=None, help="file ảnh output (mặc định: <mprim>_angleN.png)")
+    args = ap.parse_args()
+
+    resolution, numberofangles, prims = parse_mprim(args.mprim_file)
+    subset = [p for p in prims if p["startangle"] == args.angle]
+
+    if not subset:
+        print(f"Không tìm thấy primitive nào với startangle_c={args.angle}")
+        return
+
+    import math
+
+    fig, ax = plt.subplots(figsize=(6, 6))
+    tick = resolution * 0.4  # độ dài vạch heading tại mỗi pose
+    for p in subset:
+        xs = [pt[0] for pt in p["poses"]]
+        ys = [pt[1] for pt in p["poses"]]
+        line, = ax.plot(xs, ys, marker="o", markersize=2,
+                        label=f'id{p["primid"]} (cost {p["costmult"]:g})')
+        # vạch heading: kiểm tra bằng mắt heading có bám tangent quỹ đạo không
+        # (bug kiểu "heading cuối lệch 10 độ" hiện rõ ở đây)
+        for (x, y, th) in p["poses"]:
+            ax.plot([x, x + tick * math.cos(th)], [y, y + tick * math.sin(th)],
+                    color=line.get_color(), linewidth=0.6, alpha=0.6)
+        ax.annotate(str(p["primid"]), (xs[-1], ys[-1]), fontsize=8)
+
+    ax.set_aspect("equal")
+    ax.grid(True)
+    ax.set_title(f"Primitives tại startangle_c={args.angle} / {numberofangles} (resolution={resolution}m)")
+    ax.set_xlabel("x (m)")
+    ax.set_ylabel("y (m)")
+    ax.legend(fontsize=7, loc="upper left", bbox_to_anchor=(1.02, 1))
+    plt.tight_layout()
+
+    out = args.output or args.mprim_file.replace(".mprim", f"_angle{args.angle}.png")
+    plt.savefig(out, dpi=130)
+    print(f"[OK] Đã lưu hình: {out}")
+
+
+if __name__ == "__main__":
+    main()