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