Wyprowadzenie analityczne

Zadana poza efektora to macierz transformacji jednorodnej $4\times 4$, górne $3\times 3$ to orientacja (macierz rotacji $R$), a ostatnia kolumna to pozycja $\mathbf{p}$:

Zbiór $SE(3)$ to zbiór wszystkich takich macierzy, grupa sztywnych przesunięć i obrotów w przestrzeni. Szukamy wektora kątów $q = (q_1, \dots, q_6)$ spełniającego $f(q) = T^*$, gdzie $f$ to odwzorowanie FK (kinematyka prosta).

Obserwacja kluczowa: obroty w przegubach 4, 5, 6 dzieją się wokół osi przecinających się w jednym punkcie (środku nadgarstka $\mathbf{p}_\mathrm{wc}$). Obrót wokół osi przechodzącej przez punkt nie przesuwa tego punktu. Wobec tego położenie $\mathbf{p}_\mathrm{wc}$ nie zależy od$q_4, q_5, q_6$, zależy tylko od $q_1, q_2, q_3$.

W konwencji Craiga dla Pumy zachodzi $d_6 = 0$, więc środek nadgarstka pokrywa się z początkiem układu {6}. Jeśli dodatkowo nie ma offsetu narzędzia, środek nadgarstka to po prostu pozycja efektora:

Gdy jest narzędzie (pewien stały offset $T_\mathrm{tool}$ między układem {6} a układem TCP), cofamy się o ten offset:

Reasumując plan: najpierw znajdziemy $q_1, q_2, q_3$ z pozycji środka nadgarstka (3 równania, 3 niewiadome), potem $q_4, q_5, q_6$ z warunku, że orientacja pozostałych trzech przegubów ma domknąć zadane $R$.

const T06 = options.toolOffset
  ? mul4(target, invSE3(options.toolOffset))
  : target;

const R = extractRotation(T06);      // górne 3×3
const [px, py, pz] = extractPosition(T06);  // ostatnia kolumna

Pozycja początku układu {4} (czyli środka nadgarstka) w bazie to ostatnia kolumna iloczynu czterech macierzy $T_0^4 = T_0^1\,T_1^2\,T_2^3\,T_3^4$. Wstawiając parametry z tabeli DH (patrz wyprowadzenie poniżej), otrzymujemy:

gdzie $c_i = \cos q_i$, $s_i = \sin q_i$, $c_{23} = \cos(q_2+q_3)$, $s_{23} = \sin(q_2+q_3)$.

Zauważmy, że w pierwszych dwóch równaniach powtarza się to samo wyrażenie w nawiasie. Wprowadźmy skrót:

Geometrycznie $\rho$ to radialna odległość środka nadgarstka od pionowej osi bazy w obróconym o $q_1$ układzie, innymi słowy, „jak daleko" wrist centre jest od pionowej osi obrotu barku.

Z dwóch pierwszych równań mamy $p_x = c_1\,\rho - d_3\,s_1$ i $p_y = s_1\,\rho + d_3\,c_1$. Są to składowe obrotu o kąt $q_1$ wektora $(\rho, d_3)$ w płaszczyźnie XY:

Mnożąc obie strony tej równości przez $R_z(q_1)^{\top}$ (transpozycja = inwersja, bo macierz obrotu jest ortogonalna) i odczytując:

Podnosząc oba równania do kwadratu i dodając, lewa strona daje $\rho^2 + d_3^2$, a prawa $(p_x c_1 + p_y s_1)^2 + (-p_x s_1 + p_y c_1)^2$ = po rozwinięciu i uproszczeniu (krzyżowe wyrazy się znoszą, $c_1^2 + s_1^2 = 1$) $p_x^2 + p_y^2$. Stąd kluczowa identyczność:

Ta równość jest fundamentem całej dekompozycji. Lewa strona zależy wyłącznie od zadanych $p_x, p_y$ (wiemy je), prawa strona zawiera tylko $\rho$. Dlatego $\rho$ wyliczamy natychmiast, bez znajomości $q_2, q_3$.

# T_target: macierz 4x4 (poza efektora), np. NumPy array.
# R = T_target[:3, :3], p = T_target[:3, 3]
import numpy as np

R = T_target[:3, :3]
p = T_target[:3, 3]
# Środek nadgarstka: cofamy się o d6 wzdłuż osi z układu 6.
# Dla Pumy d6 = 0, więc p_wc == p, ale piszemy ogólnie:
p_wc = p - D6 * R[:, 2]
px_w, py_w, pz_w = p_wc

Z identyczności kroku 1 wyznaczamy $\rho$, ale z dokładnością do znaku (bo pierwiastek kwadratowy daje dwie wartości):

Jeśli $p_x^2 + p_y^2 < d_3^2$, zadanie jest nieosiągalne, cel leży w „zakazanym" cylindrze o promieniu $d_3$ wokół pionowej osi bazy. W pozostałych przypadkach znak $\rho$ wyznacza, czy bark Pumy zwraca się w kierunku celu (shoulder right, $\rho > 0$), czy odwrotnie, o $\pi$ obrócony (shoulder left, $\rho < 0$). Obie konfiguracje dają identyczną pozycję wrist centre, stąd „gałąź" w algorytmie.

Mając $\rho$ i $d_3$, rozwiązujemy układ dwóch liniowych równań w $(\cos q_1, \sin q_1)$ z poprzedniej strony:

Formalnie rozwiązujemy go przez wyznacznik, ale ładniej zapisuje się to przez różnicę dwóch funkcji atan2, $q_1$ jest różnicą kąta do celu ($p_x, p_y$) i kąta „w bok" o odsadzeniu $d_3$:

# ρ, promień rzutu wrist centre na płaszczyznę xy bazy.
# Dwa znaki dają shoulder LEFT i RIGHT.
r_xy_sq = px_w**2 + py_w**2
disc_q1 = r_xy_sq - D3**2
if disc_q1 < 0:
    return []  # cel poza "cylindrem zakazanym"
rho_abs = np.sqrt(max(0.0, disc_q1))
phi = np.arctan2(py_w, px_w)

solutions = []
for shoulder_sign in (+1, -1):           # +1 = right, -1 = left
    rho = shoulder_sign * rho_abs
    q1 = phi - np.arctan2(D3, rho)
    # ... dalej q3, q2, q4-q6 dla tej gałęzi

Dwuargumentowa funkcja atan2(y, x), w odróżnieniu od zwykłego arctan(y/x), zwraca kąt na pełnym przedziale $(-\pi, \pi]$, uwzględniając znaki obu argumentów. To ważne: zwykły arctan traci informację o ćwiartce, przez co „widzi" kąty z góry i z dołu jako identyczne.

Częsty błąd w kodach studenckich: użycie atan2(py, px) bez drugiego członu atan2(d₃, ρ). Dla Pumy daje to stały błąd rzędu 7° , odpowiadający kątowi „w bok" wymuszonemu przez odsadzenie $d_3$.

const phi   = Math.atan2(py, px);                           // kąt do celu w XY
const rho   = rhoSign * Math.sqrt(px*px + py*py - D3*D3);   // ±
const q1    = phi - Math.atan2(D3, rho);                    // q₁ na obu gałęziach

Po wyznaczeniu $q_1$ obracamy cały układ o ten kąt wokół pionowej osi bazy. W tak obróconym układzie całe ramię robota leży w jednej pionowej płaszczyźnie, oznaczmy jej współrzędne jako $(\rho, z)$, gdzie $\rho$ to odległość od pionowej osi bazy (dodatnia po „stronie" celu), a $z$, wysokość nad płaszczyzną podstawy.

W tej płaszczyźnie zostało nam zadanie 2-wymiarowe: połączyć bark $(0, 0)$ ze środkiem nadgarstka $(\rho, z)$ za pomocą dwóch ogniw, ramienia o długości $a_2$ i przedramienia o pewnej efektywnej długości $L$, z łokciem gdzieś pomiędzy.

Czym jest $L$? Początek układu {4} (wrist centre) nie leży na prostym przedłużeniu ramienia, jest przesunięty względem łokcia o dwa składowe: $a_3$ wzdłuż osi X układu {3} (mały bok, ok. 2 cm) i $d_4$ wzdłuż osi Z układu {3} (długi, ok. 43 cm). Skąd ta konstrukcja? Fizycznie wynika z tego, że silnik napędzający przedramię musi się gdzieś zmieścić obok łokcia, więc geometrycznie przedramię jest „schowane" za mały boczny segment.

W płaszczyźnie ramienia wektor od łokcia do środka nadgarstka ma długość:

i jest odchylony od osi X układu {3} o stały kąt:

O tym odchyleniu pamiętamy przy obliczaniu $q_3$ w następnym kroku.

Spojrzenie geometryczne. W płaszczyźnie ramienia bark $A$, łokieć $E$ i środek nadgarstka $W$ tworzą trójkąt z trzema znanymi długościami: $a_2$ (ramię), $L$ (efektywne przedramię) i $D = \sqrt{\rho^2 + p_z^2}$ (przekątna bark→wrist). Każdy trójkąt o danych trzech bokach ma jednoznacznie wyznaczone wszystkie kąty wewnętrzne, w szczególności kąt $\gamma$ przy łokciu:

Z prawa cosinusów (zastosowanego do boku $D$ przeciwległego kątowi $\gamma$):

Pełny kąt wewnętrzny trójkąta wyciąga się standardowo przez atan2 (a nie acos), żeby zachować informację o znaku $\sin\gamma$:

Znak $\pm$ przed $\sin\gamma$ odpowiada wyborowi elbow up (łokieć powyżej linii bark↔wrist) lub elbow down (poniżej), dwa odbicia trójkąta względem boku $D$.

Powiązanie z q₃. Kąt $\gamma$ przy łokciu nie jest tożsamy z $q_3$. Gdyby przedramię biegło dokładnie wzdłuż osi $\hat{x}_3$ (czyli $d_4 = 0$), wówczas $q_3 = \pi - \gamma$. W Pumie jednak przedramię jest „odchylone" od osi $\hat{x}_3$ o stały kąt $\beta = \operatorname{atan2}(d_4, a_3) \approx 87{,}3^\circ$ (z kroku 3), więc:

Postać końcowa (wykorzystywana w kodzie, jeden atan2 zamiast łańcucha kątów):

Znak $\pm$ odpowiada dwóm gałęziom geometrycznym, łokieć „w górę" lub „w dół". Gdy $L^2 < K^2$, cel leży poza zasięgiem ramienia dla danej gałęzi barku, kombinacja shoulder+elbow nie ma rozwiązania (trójkąt o takich bokach nie istnieje, bo nie spełnia nierówności trójkąta).

# Wewnątrz pętli po shoulder_sign:
K = (rho**2 + pz_w**2 - A2**2 - A3**2 - D4**2) / (2 * A2)
L = np.sqrt(A3**2 + D4**2)
beta = np.arctan2(D4, A3)
disc = L**2 - K**2
if disc < 0:
    continue                              # cel poza zasięgiem
sqrt_d = np.sqrt(disc)

for elbow_sign in (+1, -1):               # +1 = up, -1 = down
    q3 = np.arctan2(elbow_sign * sqrt_d, K) - beta
    # ... dalej q2, q4-q6 dla tej gałęzi

Mając już znane $q_3$, wracamy do dwóch równań pozycji z kroku 1, tym razem traktując $q_2$ jako jedyną niewiadomą. Składowe radialna i pionowa to:

Stosujemy wzory na sumę kątów: $c_{23} = c_2 c_3 - s_2 s_3$ oraz $s_{23} = s_2 c_3 + c_2 s_3$. Wstawiając do pierwszego równania:

Grupujemy wyrazy przy $c_2$ i $s_2$ osobno:

Tak samo dla $p_z$: $-(a_3 s_3 + d_4 c_3)\,c_2 - (a_2 + a_3 c_3 - d_4 s_3)\,s_2$. Wprowadzając skróty na powtarzające się grupy:

(zauważ: $M, N$ zależą tylko od znanego $q_3$ i stałych DH), oba równania zapisują się zwarto jako układ liniowy 2×2:

Wyznacznik macierzy współczynników: $\det = M\cdot(-M) - (-N)\cdot(-N) = -(M^2 + N^2)$. Z reguły Cramera (lub bezpośrednio przez inwersję 2×2):

Wyznacznik $M^2 + N^2$ znika tylko, gdy jednocześnie $a_2 = a_3 = d_4 = 0$, niemożliwe dla fizycznego manipulatora. Mając $c_2$ i $s_2$ jednocześnie, kąt wyznacza atan2:

# Wewnątrz pętli po elbow_sign (q3 znane):
c3, s3 = np.cos(q3), np.sin(q3)
M = A2 + A3 * c3 - D4 * s3
N = A3 * s3 + D4 * c3
denom = M**2 + N**2
c2 = (M * rho - N * pz_w) / denom
s2 = (-M * pz_w - N * rho) / denom
q2 = np.arctan2(s2, c2)

Dlaczego nie q₂ = arcsin(s₂) lub arccos(c₂)? Każda z tych funkcji zwraca wartość tylko z połowy okręgu, tracimy informację o znaku drugiego składnika. atan2(s, c) używa znaków obu argumentów, dając jednoznaczny kąt na całym okręgu $(-\pi, \pi]$. Klasyczny błąd implementacyjny: „policz arctan(s/c) i zgadnij ćwiartkę po znakach", działa tylko po stronie poprawnie pamiętających autorów.

Do tego momentu zajmowaliśmy się pozycją środka nadgarstka. Teraz przechodzimy do orientacji: trzy ostatnie przeguby $q_4, q_5, q_6$ muszą obrócić nadgarstek tak, aby pełna macierz $R_0^6$ zgodziła się z zadanym $R$.

Część rotacyjną macierzy $T_0^3$, czyli $R_0^3$, wyliczamy ze znanych już $q_1, q_2, q_3$. Kluczowa obserwacja: w konwencji Craiga $q_2$ i $q_3$ obracają się wokół tej samej (poziomej) osi $\hat{z}_2 = \hat{z}_3$, bo $\alpha_2 = 0$ w tabeli DH (między układami {2} i {3} nie ma skręcenia osi). Obroty wokół wspólnej osi zwyczajnie się dodają: $R_z(q_2)\,R_z(q_3) = R_z(q_2 + q_3)$. Razem:

Wynik jawny (przepisany dla wygody, używamy go bezpośrednio w kodzie):

Dalej: jeśli $R = R_0^6 = R_0^3 \cdot R_3^6$, to nadgarstek musi dostarczyć „brakującą" rotację:

Równość $(R_0^3)^{-1} = (R_0^3)^{\top}$ wynika z ortogonalności macierzy rotacji (wiersze są ortonormalne). W implementacji liczymy więc po prostu iloczyn transponowanej macierzy przez $R$, bez wywoływania numerycznej inwersji.

# Wewnątrz pętli po elbow (q2, q3 znane):
c1, s1 = np.cos(q1), np.sin(q1)
c23 = np.cos(q2 + q3)
s23 = np.sin(q2 + q3)

R03 = np.array([
    [ c1*c23, -c1*s23, -s1],
    [ s1*c23, -s1*s23,  c1],
    [-s23,    -c23,      0],
])
R36 = R03.T @ R                          # ortogonalność: T zamiast inv

Trzy ostatnie ogniwa Pumy to klasyczny nadgarstek typu ZYZ: dwa skręcenia osi $\pm\pi/2$ między kolejnymi przegubami sprawiają, że osie $\hat z_4$, $\hat z_5$, $\hat z_6$ są wzajemnie prostopadłe i schodzą się w jednym punkcie (środek nadgarstka). Z tabeli DH ($\alpha_3 = -\pi/2$, $\alpha_4 = \pi/2$, $\alpha_5 = -\pi/2$), iloczyn: