first commit

kinematics.py

@@ -0,0 +1,140 @@
+"""
+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}