first commit

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