199 lines
13 KiB
Python
199 lines
13 KiB
Python
"""
|
|
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
|