""" 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