141 lines
7.0 KiB
Python
141 lines
7.0 KiB
Python
"""
|
|
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}
|