DuongTD/mprim_gen
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

first commit

Browse Source
This commit is contained in:
duongtd
2026-07-28 23:35:42 +07:00
commit 08eb7a6b78
22 changed files with 3559 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

198
mprim_config.py Normal file
View File

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