Moduł 12 · analityczne

IK dla geometrii nie-pieperowskich

Jak wyprowadzić zamkniętą IK dla manipulatorów BEZ sferycznego nadgarstka — case study UR5 (Hawkins/Kufieta) i ES5 (Załącznik A dysertacji Gruszki 2024). Plus krótko o redundancji Franka Panda.

O czym jest ten moduł

Moduł 1 pokazał piękne, „klasyczne" wyprowadzenie IK dla PUMA 560 — manipulator z trzema osiami nadgarstka schodzącymi w jednym punkcie (forma A warunku Piepera). To pozwoliło na dekompozycję 3+3: najpierw pozycja, potem orientacja. Pięć stron algebry, jeden czysty wynik.

Większość współczesnych cobotów nie spełnia jednak formy A. UR5, UR10, Franka Panda, KUKA iiwa — wszystkie one mają geometrie zaprojektowane pod inne cele (smukłość, lekkość, równa dystrybucja mas), które łamią klasyczną dekompozycję. Czy to znaczy że nie można ich rozwiązać analitycznie? Nie — można, ale trzeba zmienić podejście.

Co wynika z tego modułu

Po przerobieniu modułu student potrafi: (1) rozpoznać formę warunku Piepera na podstawie geometrii DH, (2) wyprowadzić IK dla manipulatora UR5 (forma B, metoda Hawkins/Kufieta), (3) wyprowadzić IK dla ES5 z dysertacji Gruszki, (4) rozumie różnicę między manipulatorami 6-DOF a redundantnym 7-DOF (Franka Panda) i wie dlaczego ten ostatni wymaga zupełnie innego podejścia.

Krok 1

Dwie formy warunku Piepera + przypadek brzegowy

Donald Pieper w 1968 roku pokazał, że istnieje rozwiązanie zamknięte IK dla manipulatora 6-DOF jeśli spełniony jest jeden z dwóch warunków geometrycznych:

Forma A — trzy kolejne osie obrotu przecinają się w jednym punkcie.
Forma B — trzy kolejne osie obrotu są wzajemnie równoległe.

W obu przypadkach 6-wymiarowy problem rozpada się na dwa niezależne 3-wymiarowe (pozycja + orientacja w jakimś rozumieniu), a wyniki są wzorami zamkniętymi z funkcjami trygonometrycznymi.Jeśli żaden warunek nie zachodzi — IK nadal może być rozwiązalna analitycznie, ale wymaga zaawansowanych metod algebraicznych (Raghavan–Roth, redukcja do równania 16. stopnia w jednej zmiennej).

Forma A

3 osie schodzą w jednym punkcie

Przykłady: PUMA 560, Stäubli TX, ABB IRB

Forma B

3 osie wzajemnie równoległe

Przykłady: UR5/UR10/UR16, ES5, KUKA iiwa (częściowo)

Brak warunku Piepera

Ani A, ani B

Przykłady: Manipulatory eksperymentalne — wymagana metoda Raghavan–Roth (równanie 16. stopnia)

Zasada Piepera (1968): jeśli manipulator 6-DOF ma 3 kolejne osie spełniające A lub B, to istnieje rozwiązanie zamknięte IK (dekompozycja na 3+3 niewiadome). To warunek wystarczający, nie konieczny — manipulatory bez Piepera też mogą mieć zamknięte rozwiązania, ale wymagają zaawansowanych metod (rezultanty, metoda Raghavan–Roth).

Praktyczna konsekwencja: projektanci robotów zwykle wybierają geometrię spełniającą jedną z form Piepera celowo — żeby IK miało zamknięte rozwiązanie i było obliczane w mikrosekundach zamiast milisekundach. Manipulatory bez Piepera stosuje się rzadko (głównie w eksperymentalnych systemach badawczych).

Krok 2

UR5/UR10/UR16 — dlaczego forma A nie działa

Manipulatory Universal Robots (UR3, UR5, UR10, UR16) mają geometrię zaprojektowaną pod cele współpracy z człowiekiem: smukłe, bez ostrych krawędzi, równa dystrybucja masy. Cecha kinematyczna: kolejne osie q₂, q₃, q₄ są wzajemnie równoległe (oś pozioma, w jednym kierunku), a osie nadgarstka są przesunięte względem siebie o niezerowy $d_5$ — co oznacza że nie schodzą w jednym punkcie.

Parametry DH UR5 (modyfikowany Craig, [m, rad])

i	$\alpha_{i-1}$	$a_{i-1}$	$d_i$	$\theta_i$	uwagi
1	0	0	0,089	$q_1$	obrót podstawy
2	$+\pi/2$	0	0	$q_2$	bark
3	0	−0,425	0	$q_3$	forma B: q₂ ∥ q₃ ∥ q₄
4	0	−0,392	0,109	$q_4$	łokieć
5	$+\pi/2$	0	0,095	$q_5$	d₅ ≠ 0 — forma A wykluczona
6	$-\pi/2$	0	0,082	$q_6$	kołnierz

Wniosek: dekompozycja 3+3 z M1 nie zadziała — nie istnieje „środek nadgarstka" jako wspólny punkt 3 ostatnich osi. Ale spełniona jest forma B (q₂ ∥ q₃ ∥ q₄), więc da się rozpisać rozwiązanie zamknięte — tylko innym algorytmem.

Metoda Hawkins/Kufieta (2013/2014): kolejność wyprowadzania to $q_1 \to q_5,q_6 \to q_2,q_3,q_4$ . W skrócie: z geometrii „cylindra zakazanego" wokół podstawy wyznacza się q₁ przez przekształcenia atan2 z d_5 jako stałym offsetem. Mając q₁, można wyizolować q₅ i q₆ z elementów macierzy ${}^6R_1$ . Mając konfigurację kiści, wracamy do podproblemu 3R-planarnego w pionowej płaszczyźnie po q₁ — i z twierdzenia cosinusów (jak w M1) wyznaczamy q₂, q₃, q₄.

Liczba rozwiązań: 8 — tak samo jak Pumy. Dwa branche shoulder × dwa elbow × dwa wrist. Pomimo zupełnie innej algebry, struktura rozwiązań pozostaje taka sama dla każdego manipulatora spełniającego jakąkolwiek formę Piepera.

▸ Źródła i implementacje referencyjne UR5 IK

Hawkins, K. (2013). „Analytic inverse kinematics for the Universal Robots UR-5/UR-10 arms." Technical Report, Georgia Institute of Technology — pierwszy formalny artykuł, pełne wyprowadzenie wszystkich 8 gałęzi.
Kufieta, K. (2014). „Force estimation in robotic manipulators: modeling, simulation and experiments." MSc Thesis, NTNU — alternatywne wyprowadzenie z czytelniejszą notacją, używane w wielu open-source implementacjach.
Implementacje: ur_kinematics w ROS, ur_rtde w Pythonie, ikfast (autogenerowany z URDF), C++ w ur_robot_driver.

W aplikacji nie implementujemy pełnej IK UR5 — to wykraczałoby poza zakres modułu i wymagało osobnego modelu URDF. Skupiamy się tu na ES5 (mamy go już w M9–M11), dla którego mamy gotowe wyprowadzenie z dysertacji [Gruszka 2024, Załącznik A].

Krok 3

ES5 (EasyRobots) — geometria i pełne wyprowadzenie z dysertacji

ES5 to manipulator firmy EasyRobots, dla którego M9 (dynamika) i M10 (silnik) korzystają z parametrów inercji. Tu domykamy pętlę — pokazujemy jego kinematyczne rozwiązanie IK. Geometrycznie ES5 jest podobny do UR (forma B — równoległość q₂, q₃, q₄), ale ma inne wymiary i konwencję DH.

Parametry DH ES5 (modyfikowany Craig, [m, rad])

źródło: src/lib/robots/es5.ts

i	$\alpha_{i-1}$	$a_{i-1}$	$d_i$	$\theta_i$	uwagi
1	0	0	0	$q_1$	obrót podstawy
2	$+\pi/2$	0	0	$q_2$	bark
3	0	0,425	0	$q_3$	forma B: q₂ ∥ q₃ ∥ q₄
4	0	0,395	0,1105	$q_4$	łokieć z odsadzeniem d₄
5	$-\pi/2$	0	0,101	$q_5$	kostka nadgarstka
6	$+\pi/2$	0	0,0765	$q_6$	kołnierz końcówki

Kolejność wyprowadzania współrzędnych w dysertacji: θ₁ → θ₅ → θ₆ → θ₃ → θ₂ → θ₄. Ta sekwencja nie jest oczywista — jest algebraicznie wymuszona faktem, że pewne równania trygonometryczne dają się rozwiązać tylko gdy znane są inne współrzędne. To kontrastuje z M1 (Puma), gdzie kolejność q₁ → q₂,₃ → q₄,₅,₆ wynikała wprost z dekompozycji formy A.

Poniżej pełne wyprowadzenie 6 współrzędnych zgodnie z Załącznikiem A dysertacji. Każdy krok zawiera referencję do oryginalnego numeru równania w pracy (eq. A.x).

Punkt wyjścia — zadana macierz transformacji

Zadana jest macierz transformacji TCP względem bazy ${}^6T_0$ w postaci ogólnej (eq. A.1 z dysertacji):

{}^6T_0 = \begin{bmatrix} r_{11} & r_{12} & r_{13} & p_x \\ r_{21} & r_{22} & r_{23} & p_y \\ r_{31} & r_{32} & r_{33} & p_z \\ 0 & 0 & 0 & 1 \end{bmatrix}

Dla czytelności zapisu używamy w dysertacji skrótu $s_i \equiv \sin\theta_i$ i $c_i \equiv \cos\theta_i$ , oraz $c_{23} \equiv \cos(\theta_2+\theta_3)$ , $c_{234} \equiv \cos(\theta_2+\theta_3+\theta_4)$ itd. (skutek równoległości osi q₂, q₃, q₄ — sumy ich kątów zachowują się jak jeden „efektywny" kąt).

Krok 1

θ₁ — z drugiej kolumny przekształconej macierzy

Klucz wyprowadzenia: jeśli weźmiemy ${}^5T_1$ obliczone na dwa różne sposoby — analitycznie z parametrów DH, oraz przez izolację z zadanego ${}^6T_0$ przez mnożenie odwrotnościami — dostaniemy układ równań w którym pewne komórki zależą tylko od θ₁:

({}^1T_0)^{-1} \cdot {}^6T_0 \cdot ({}^6T_5)^{-1} = {}^5T_1 \qquad \text{(eq. A.2)}

Analitycznie (z DH) ${}^5T_1$ ma w komórce y wektora translacji (drugi wiersz, czwarta kolumna) wartość stałą: $-d_4$ . Z drugiej strony, po wykonaniu mnożenia po lewej i porównaniu — dostajemy równanie w którym jedyną niewiadomą jest $\theta_1$ :

-s_1(p_x - d_6 r_{13}) + c_1(p_y - d_6 r_{23}) = -d_4 \qquad \text{(eq. A.5)}

Podstawienie helpers — pozycja środka nadgarstka (środka układu 5) rzutowana na xy bazy:

{}^5p_{0x} = p_x - d_6 r_{13}, \qquad {}^5p_{0y} = p_y - d_6 r_{23} \qquad \text{(eq. A.6)}

{}^5p_{0xy} = \sqrt{({}^5p_{0x})^2 + ({}^5p_{0y})^2}, \qquad \alpha = \mathrm{atan2}({}^5p_{0y}, {}^5p_{0x})

Po podstawieniu i wykorzystaniu wzoru na sin różnicy kątów:

{}^5p_{0xy} \sin(\theta_1 - \alpha) = d_4 \qquad \text{(eq. A.10)}

Stąd ostateczna postać dla θ₁:

\boxed{\;\theta_1 = \arcsin\!\Big(\tfrac{d_4}{\,{}^5p_{0xy}\,}\Big) \pm \mathrm{atan2}({}^5p_{0x}, {}^5p_{0y})\;} \qquad \text{(eq. A.13)}

Python · θ₁implementacja eq. A.6–A.13: dwie gałęzie shoulder

import numpy as np

# T_target: macierz 4x4 zadanej pozy efektora
R = T_target[:3, :3]
px, py = T_target[0, 3], T_target[1, 3]
r13, r23 = R[0, 2], R[1, 2]

# eq. A.6: środek układu 5 w bazie (cofamy się o d6 wzdłuż z6_world)
p5x = px - D6 * r13
p5y = py - D6 * r23
p5xy = np.hypot(p5x, p5y)

# eq. A.13: dwie gałęzie shoulder
asin_val = np.arcsin(np.clip(D4 / p5xy, -1, 1))
alpha    = np.arctan2(p5y, p5x)
theta1_candidates = [
    (alpha + asin_val,            "right"),
    (alpha + np.pi - asin_val,    "left"),
]

Dwa rozwiązania — odpowiadają dwóm konfiguracjom barku (shoulder-left / shoulder-right). Analog do gałęzi shoulder w M1.

Krok 2

θ₅ — z położenia ostatniego członu względem drugiego

Analiza struktury kinematycznej pokazuje, że współrzędna y wektora ${}^6p_1$ (translacja od początku ogniwa 2 do początku ogniwa 6) zależy wyłącznie od θ₅:

{}^6p_{1y} = d_4 + d_6 \cos\theta_5 \qquad \text{(eq. A.14)}

Z drugiej strony, ten sam ${}^6p_{1y}$ można wyrazić przez obrót $({}^1R_0)^{-1} \cdot {}^6p_0$ (eq. A.16–A.18):

{}^6p_{1y} = {}^6p_{0x} \cdot (-s_1) + {}^6p_{0y} \cdot c_1 \qquad \text{(eq. A.18)}

Przyrównanie i wyizolowanie $\cos\theta_5$ :

\boxed{\;\theta_5 = \pm\arccos\!\Big(\tfrac{-{}^6p_{0x}\,s_1 + {}^6p_{0y}\,c_1 - d_4}{d_6}\Big)\;} \qquad \text{(eq. A.20)}

Python · θ₅dwie gałęzie wrist (flip/noflip)

for theta1, shoulder in theta1_candidates:
    c1, s1 = np.cos(theta1), np.sin(theta1)
    cos5 = (px * s1 - py * c1 - D4) / D6
    if abs(cos5) > 1:
        continue
    base_t5 = np.arccos(np.clip(cos5, -1, 1))
    for wrist_sign in (+1, -1):
        theta5 = wrist_sign * base_t5
        # ... dalej θ₆ i (θ₃, θ₂, θ₄)

Dwa rozwiązania — odpowiadają dwóm orientacjom kiści względem ogniwa 4 (analog do gałęzi wrist flip z M1).

Uwaga o pierwszej osobliwości: gdy $\theta_5 = 0°$ , szósta oś obrotu staje się równoległa do osi 2, 3 i 4 — pojawia się nadmiarowość stopni swobody (obrót osi 2, 3, 4 manipuluje TCP niezależnie od rotacji osi 6). To jest klasyczny wrist singularity analogiczny do Pumy.

Krok 3

θ₆ — z komórek macierzy 6T1

Wyznaczając jawnie ${}^6T_1$ z parametrów DH (eq. A.21) i porównując z formą uzyskaną przez ${}^6R_1 = ({}^1R_0)^{-1}\cdot{}^6R_0$ (eq. A.26–A.28), z komórek [2,1] oraz [2,2] dostajemy układ:

\begin{cases} -s_5 c_6 = -s_1 r_{11} + c_1 r_{21}, \\ \phantom{-}s_5 s_6 = -s_1 r_{12} + c_1 r_{22}. \end{cases} \qquad \text{(eq. A.29)}

Stąd osobno $\sin\theta_6$ i $\cos\theta_6$ , i ich złożenie przez atan2:

\boxed{\;\theta_6 = \mathrm{atan2}\!\Big(\tfrac{-s_1 r_{12} + c_1 r_{22}}{s_5},\; \tfrac{s_1 r_{11} - c_1 r_{21}}{s_5}\Big)\;} \qquad \text{(eq. A.31)}

Python · θ₆atan2(sin θ₆, cos θ₆) z komórek macierzy ⁶R₁

c5, s5 = np.cos(theta5), np.sin(theta5)
r11, r12 = R[0, 0], R[0, 1]
r21, r22 = R[1, 0], R[1, 1]

if abs(s5) < EPS:                        # wrist singularity
    theta6 = 0.0
else:
    sin6 = ( s1 * r12 - c1 * r22) / s5
    cos6 = (-s1 * r11 + c1 * r21) / s5
    theta6 = np.arctan2(sin6, cos6)

atan2 (a nie acos) — żeby zachować pełen zakres $[-\pi, \pi]$ . Dzielenie przez $s_5$ jest niesingularne dopóki $\theta_5 \neq 0$ .

Krok 4

θ₃ — z twierdzenia cosinusów w płaszczyźnie x-z

Po znalezieniu θ₁, θ₅, θ₆ wracamy do zadania pozycji. Wyznaczamy ${}^4T_1$ przez ciąg mnożeń odwrotnościami:

{}^4T_1 = ({}^1T_0)^{-1} \cdot {}^6T_0 \cdot ({}^5T_4)^{-1} \cdot ({}^6T_5)^{-1} \qquad \text{(eq. A.32)}

Z geometrii (Rys. A.1 z dysertacji) — robot rzutowany na płaszczyznę xz tworzy trójkąt o bokach $a_2$ , $a_3$ i przekątnej $|{}^4p_1|$ . Z twierdzenia cosinusów (jak w M1):

\cos\beta = \frac{a_2^2 + a_3^2 - |{}^4p_1|^2}{2 a_2 a_3}, \qquad \beta = \pi - \theta_3

Stąd:

\boxed{\;\theta_3 = \pm\arccos\!\Big(\tfrac{-a_2^2 - a_3^2 + ({}^4p_{1x})^2 + ({}^4p_{1z})^2}{2 a_2 a_3}\Big)\;} \qquad \text{(eq. A.38)}

Python · θ₃trójkąt 2R w płaszczyźnie xz (składowa y wektora ⁴p₁ stała = -d₄)

# Wylicz T_1_4 = (T_0_1)^-1 · T_target · (T_5_6)^-1 · (T_4_5)^-1
T01 = link_transform(0, theta1)
T45 = link_transform(4, theta5)
T56 = link_transform(5, theta6)
T14 = inv_se3(T01) @ T_target @ inv_se3(T56) @ inv_se3(T45)
p1x_4, _, p1z_4 = T14[:3, 3]

# Twierdzenie cosinusów dla trójkąta 2R w płaszczyźnie xz:
a2, a3 = A3, A4                          # ramię i przedramię
p1n2 = p1x_4**2 + p1z_4**2
cos3 = (p1n2 - a2**2 - a3**2) / (2 * a2 * a3)
if abs(cos3) > 1:
    continue
base_t3 = np.arccos(np.clip(cos3, -1, 1))
for elbow_sign in (+1, -1):
    theta3 = elbow_sign * base_t3
    # ... dalej θ₂, θ₄

Dwa rozwiązania — gałęzie elbow-up i elbow-down, analog do M1.

Krok 5

θ₂ — z trygonometrii trójkąta O₂O₃O₄

Z tego samego rzutu (Rys. A.1) widzimy: $\theta_2 = \gamma - \alpha$ , gdzie γ to kąt do ${}^4p_1$ , a α to wewnętrzny kąt trójkąta przy O₂. Wyrażając przez znane wielkości:

\gamma = \mathrm{atan2}({}^4p_{1x}, {}^4p_{1z})

α z prawa sinusów dla trójkąta O₂O₃O₄:

\frac{\sin\alpha}{a_3} = \frac{\sin\beta}{|{}^4p_1|} \;\Rightarrow\; \alpha = \arcsin\!\Big(\frac{a_3 \sin\beta}{|{}^4p_1|}\Big)

Stąd:

\boxed{\;\theta_2 = \mathrm{atan2}({}^4p_{1x}, {}^4p_{1z}) - \arcsin\!\Big(\tfrac{a_3 \sin\beta}{|{}^4p_1|}\Big)\;} \qquad \text{(eq. A.43)}

Python · θ₂rozwiązanie układu liniowego p1x = K·c2 − M·s2, p1z = K·s2 + M·c2

c3, s3 = np.cos(theta3), np.sin(theta3)
K  = a2 + a3 * c3
Mt = a3 * s3
theta2 = np.arctan2(K * p1z_4 - Mt * p1x_4,
                    K * p1x_4 + Mt * p1z_4)

Krok 6

θ₄ — z komórek [1,1] i [1,2] macierzy 4T3

Ostatnia współrzędna — analogicznie do θ₆, ale na innym poziomie dekompozycji macierzy:

{}^4T_3 = ({}^1T_0)^{-1} \cdot ({}^2T_1)^{-1} \cdot ({}^3T_2)^{-1} \cdot {}^6T_0 \cdot ({}^5T_4)^{-1} \cdot ({}^6T_5)^{-1} \qquad \text{(eq. A.44)}

Z komórek [1,1] i [1,2] tej macierzy wyciągamy $\sin\theta_4$ i $\cos\theta_4$ jako kombinacje liniowe znanych już $r_{ij}, c_1, s_1, c_5, s_5, c_6, s_6, c_{23}, s_{23}$ (eq. A.45 — dwie długie formuły 6-składnikowe). Następnie:

\boxed{\;\theta_4 = \mathrm{atan2}(\sin\theta_4,\; \cos\theta_4)\;} \qquad \text{(eq. A.46)}

Python · θ₄odczyt z elementów macierzy T_3_4 (α₃=0 → cos θ₄ w [0,0], −sin θ₄ w [0,1])

T12 = link_transform(1, theta2)
T23 = link_transform(2, theta3)
T03 = T01 @ T12 @ T23
T34 = inv_se3(T03) @ T_target @ inv_se3(T56) @ inv_se3(T45)
theta4 = np.arctan2(-T34[0, 1], T34[0, 0])

solutions.append((theta1, theta2, theta3, theta4, theta5, theta6))

Postać końcowa — 6 równań

Komplet wzorów (eq. A.47–A.52 z dysertacji) — gotowe do wstawienia do kodu solvera analitycznego dla ES5:

\theta_1 = \arcsin\!\Big(\tfrac{d_4}{{}^5p_{0xy}}\Big) \pm \mathrm{atan2}({}^5p_{0x}, {}^5p_{0y})

\theta_2 = \mathrm{atan2}({}^4p_{1x}, {}^4p_{1z}) - \arcsin\!\Big(\tfrac{a_3 \sin\beta}{|{}^4p_1|}\Big)

\theta_3 = \pm\arccos\!\Big(\tfrac{-a_2^2 - a_3^2 + ({}^4p_{0x})^2 + ({}^4p_{1z})^2}{2 a_2 a_3}\Big)

\theta_4 = \mathrm{atan2}(\sin\theta_4,\; \cos\theta_4)

\theta_5 = \pm\arccos\!\Big(\tfrac{-{}^6p_{0x} s_1 + {}^6p_{0y} c_1 - d_4}{d_6}\Big)

\theta_6 = \mathrm{atan2}\!\Big(\tfrac{-s_1 r_{12} + c_1 r_{22}}{s_5},\; \tfrac{s_1 r_{11} - c_1 r_{21}}{s_5}\Big)

Liczba rozwiązań: 2 (shoulder θ₁) × 2 (elbow θ₃) × 2 (wrist θ₅) = 8 konfiguracji — identycznie jak dla Pumy 560, choć geometria jest zupełnie inna. To uniwersalny wynik dla manipulatorów 6-DOF spełniających jakąkolwiek formę warunku Piepera.

Krok 4

Porównanie 3 robotów obok siebie

Cecha	PUMA 560 (M1)	UR5 (Hawkins/Kufieta)	ES5 (Gruszka 2024)
Forma Piepera	A (osie 4,5,6 schodzą)	B (osie 2,3,4 równoległe)	B (osie 2,3,4 równoległe)
DOF	6	6	6
Liczba rozwiązań	8	8	8
Kolejność wyprowadz.	q₁ → q₂,q₃ → q₄,q₅,q₆	q₁ → q₅,q₆ → q₂,q₃,q₄	q₁ → q₅ → q₆ → q₃ → q₂ → q₄
Środek nadgarstka	TAK — w punkcie przecięcia osi 4,5,6	NIE — osie nie schodzą (d_5 ≠ 0)	NIE — osie nie schodzą
Singularności	oś 1, łokieć wew./zew., nadgarstek	cylinder zakazany, łokieć, nadgarstek	analogiczne do UR (θ₅=0 → wrist)
Typowe zastosowanie	przemysł motoryzacyjny, edukacja	cobot, lekkie zadania, lead-through	cobot/przemysł, dydaktyka dysertacji

Wniosek dydaktyczny: mimo zupełnie różnych geometrii i algorytmów wyprowadzania, liczba rozwiązań (8) i ogólna struktura wyboru (shoulder × elbow × wrist) są uniwersalne dla każdego manipulatora 6-DOF spełniającego warunek Piepera. To głęboka prawda: forma rozwiązania zależy od geometrii, ale liczba i typ branch'y — od struktury problemu IK.

Krok 5

Bonus: Franka Panda — 7 DOF, redundancja i null-space

Franka Emika Panda łamie wszystkie założenia, na których opierał się ten moduł — bo ma siedem stopni swobody zamiast sześciu. Dla zadania SE(3) (6-DOF) oznacza to nadmiarowość 1 DOF: dla każdej osiągalnej pozy końcówki istnieje cała ciągła rodzina konfiguracji (parametryzowana jednym dodatkowym kątem — zwykle nazywanym kątem łokcia albo swivel angle).

Konsekwencje:

Zamknięta IK dla Franki istnieje, ale parametryczna — funkcja $\theta_2(\psi), \theta_4(\psi), \theta_6(\psi)$ jednego wolnego parametru ψ ∈ ℝ.
W praktyce do sterowania w pełnym 7-DOF używa się metod jakobianowych z projekcją w null-space — temat M3 (Jakobian Transpose, DLS) i częściowo M7 (analiza singularności). Pseudoinwersja jakobianu daje minimalne $\dot q$ , a null-space pozwala dodatkowo „celować" w ukryte cele (np. unikać przeszkód, optymalizować manipulability).
Zalety nadmiarowości: Panda omijająca przeszkody między bazą a celem; możliwość zachowania ergonomicznej konfiguracji łokcia podczas zmiany pozy końcówki; więcej bezpieczeństwa w pobliżu człowieka.

Pełne wyprowadzenie 7-DOF IK wykracza poza ten moduł — wymaga osobnego materiału z parametryzacją kąta łokcia, obsługą singularności łokcia (gdy oś 4 jest prawie wyprostowana) i integracją z null-space optymalizacją. Sugestia: prześledź P. Beeson & B. Ames (2015) „TRAC-IK" — niereferencyjna ale praktycznie stosowana biblioteka działająca na Franka, UR i Pumie.

Kompletna funkcja Python — wszystkie kroki sklejone

Sześć snippetów Python z kroków 1–6 wyprowadzenia powyżej, połączone w jedną funkcję solve_es5_ik(T_target). Zwraca listę do 8 rozwiązań (krotki 6 kątów), zgodnie z gałęziami shoulder × elbow × wrist.

Python · pełna implementacjakopiuj-wklej do notatnika i uruchom (wymaga numpy + funkcji DH)

import numpy as np

# Parametry DH ES5 (modified Craig) — z src/lib/robots/es5.ts:
A3 = 0.425    # ramię
A4 = 0.395    # przedramię
D4 = 0.1105   # odsadzenie przedramienia
D5 = 0.101    # kostka nadgarstka
D6 = 0.0765   # wystawienie końcówki
EPS = 1e-9

def solve_es5_ik(T_target, link_transform, inv_se3):
    """
    Analityczne IK dla ES5 (forma B Piepera) wg Załącznika A
    dysertacji [Gruszka 2024].

    link_transform(i, theta_i) — macierz 4×4 ⁱ⁻¹T_i z DH (Craig).
    inv_se3(T) — szybka odwrotność SE(3): R^T blok + -R^T·t.
    Obie funkcje musisz dostarczyć (kilka linijek każda).

    Zwraca: list[tuple[float, ...]] — do 8 krotek (q1..q6).
    """
    R = T_target[:3, :3]
    px, py = T_target[0, 3], T_target[1, 3]
    r11, r12, r13 = R[0]
    r21, r22, r23 = R[1]

    # === Krok 1: θ₁ (dwie gałęzie shoulder) ===
    p5x = px - D6 * r13
    p5y = py - D6 * r23
    p5xy = np.hypot(p5x, p5y)
    if p5xy < EPS:
        return []
    ratio = D4 / p5xy
    if abs(ratio) > 1:
        return []
    asin_val = np.arcsin(np.clip(ratio, -1, 1))
    alpha    = np.arctan2(p5y, p5x)
    theta1_candidates = [
        (alpha + asin_val,           "right"),
        (alpha + np.pi - asin_val,   "left"),
    ]

    solutions = []
    for theta1, shoulder in theta1_candidates:
        c1, s1 = np.cos(theta1), np.sin(theta1)

        # === Krok 2: θ₅ (dwie gałęzie wrist) ===
        cos5 = (px * s1 - py * c1 - D4) / D6
        if abs(cos5) > 1:
            continue
        base_t5 = np.arccos(np.clip(cos5, -1, 1))
        for wrist_sign in (+1, -1):
            theta5 = wrist_sign * base_t5
            c5, s5 = np.cos(theta5), np.sin(theta5)

            # === Krok 3: θ₆ — atan2(sin θ₆, cos θ₆) z komórek ⁶R₁ ===
            if abs(s5) < EPS:
                theta6 = 0.0
            else:
                sin6 = ( s1 * r12 - c1 * r22) / s5
                cos6 = (-s1 * r11 + c1 * r21) / s5
                theta6 = np.arctan2(sin6, cos6)

            # === T_1_4 numerycznie (cofamy się przez T_5_6, T_4_5) ===
            T01 = link_transform(0, theta1)
            T45 = link_transform(4, theta5)
            T56 = link_transform(5, theta6)
            T14 = inv_se3(T01) @ T_target @ inv_se3(T56) @ inv_se3(T45)
            p1x_4, _, p1z_4 = T14[:3, 3]

            # === Krok 4: θ₃ (dwie gałęzie elbow) ===
            a2, a3 = A3, A4
            p1n2 = p1x_4**2 + p1z_4**2
            cos3 = (p1n2 - a2**2 - a3**2) / (2 * a2 * a3)
            if abs(cos3) > 1:
                continue
            base_t3 = np.arccos(np.clip(cos3, -1, 1))
            for elbow_sign in (+1, -1):
                theta3 = elbow_sign * base_t3

                # === Krok 5: θ₂ (z układu liniowego K·c2 - M·s2 = p1x, ...) ===
                c3, s3 = np.cos(theta3), np.sin(theta3)
                K  = a2 + a3 * c3
                Mt = a3 * s3
                theta2 = np.arctan2(K * p1z_4 - Mt * p1x_4,
                                    K * p1x_4 + Mt * p1z_4)

                # === Krok 6: θ₄ (z elementu T_3_4) ===
                T12 = link_transform(1, theta2)
                T23 = link_transform(2, theta3)
                T03 = T01 @ T12 @ T23
                T34 = inv_se3(T03) @ T_target @ inv_se3(T56) @ inv_se3(T45)
                theta4 = np.arctan2(-T34[0, 1], T34[0, 0])

                solutions.append((theta1, theta2, theta3, theta4, theta5, theta6))

    return solutions

Co warto zauważyć: kolejność wyprowadzania współrzędnych jest algebraicznie wymuszona — najpierw θ₁ z geometrii rzutu, potem θ₅ i θ₆ z analizy macierzy ${}^6T_1$ , dopiero na końcu θ₃, θ₂, θ₄ z numerycznie wyliczonej ${}^4T_1$ . To inna kolejność niż klasyczna dekompozycja 3+3 Pumy z M1 — bo geometria ES5 (forma B) wymaga innego rozdzielenia.

Referencyjna implementacja w TypeScript

Powyższy algorytm Python jest 1:1 przetłumaczony do TypeScriptu jako src/lib/solvers/analytical-es5.ts w aplikacji. Wersja TS jest używana w playgroundzie poniżej (interaktywne wyznaczanie 8 rozwiązań na żywo). Przechodzi smoke test FK→IK→FK na 5 konfiguracjach z błędem maszynowej precyzji (~10⁻¹⁵ rad).

TypeScriptAPI solvera ES5 w aplikacji

# Uruchom test:
npx tsx src/lib/solvers/__es5_smoke.ts

# Użycie:
import { solveEs5Analytical } from "@/lib/solvers/analytical-es5";
import { forwardKinematics } from "@/lib/robots/dh";
import { ES5 } from "@/lib/robots/es5";

const target = forwardKinematics(ES5, [0.3, 0.4, 0.5, 0.6, 0.7, 0.8]);
const solutions = solveEs5Analytical(target);
// solutions: do 8 rozwiązań z gałęziami shoulder/elbow/wrist

Interaktywny playground — ES5 + 8 rozwiązań IK

Playground ma dwa równoległe stany:

q (kąty przegubów) — sterowane sliderami po prawej. Robot 3D pokazuje aktualną konfigurację.
T* (poza docelowa) — wpisywana w panelu „Poza docelowa" jako pozycja (x, y, z) i orientacja RPY. Solver liczy IK z T*, pokazując wszystkie konfiguracje, którymi można trafić w tę pozę.

Wskaźnik TCP odbiega od T* o ... mm mówi czy aktualne q faktycznie trafia w T*. Kliknięcie wiersza w tabeli rozwiązań ładuje wybraną gałąź do sliderów — robot ustawia się tam i wskaźnik staje się zielony.

Eksperymenty do wypróbowania:

Wpisz pozycję z = 0.05 (TCP nisko nad podłożem) — większość rozwiązań prawdopodobnie odpadnie (poza zasięgiem ramienia w dół). Zobaczysz krótszą listę albo komunikat „brak rozwiązań".
Kliknij ← zrzut FK(q), potem przesuń sliderem θ₅ blisko 0 i ponownie zrób zrzut — wokół tej pozy gałęzie wrist (flip/noflip) zaczynają się zlewać: ta sama orientacja osiągalna z różnymi θ₄ i θ₆. To singularność nadgarstka.
Kliknij wiersz „shoulder=left" — robot „obraca się dookoła osi 1" w symetryczną konfigurację. Ten sam TCP, zupełnie inna postawa ramienia.
Kliknij wiersz „elbow=down" — łokieć ląduje pod linią bark↔nadgarstek. W praktyce takie konfiguracje są rzadziej używane przemysłowo (mniej ergonomiczne, większe ryzyko kolizji z otoczeniem).

Konfiguracja q [°]

θ₁0.0°

θ₂0.0°

θ₃0.0°

θ₄0.0°

θ₅0.0°

θ₆0.0°

Pose docelowa T*

Pozycja [m]

xyz

Orientacja RPY [°]

RPY

✓ TCP na pozie docelowej (|Δp| < 1 mm)

Rozwiązania IK z pozy docelowej: 0(maks. 8 — shoulder × elbow × wrist)

kliknij żeby załadować

Brak rozwiązań — poza zasięgiem albo geometryczna degeneracja. Spróbuj zmienić pozycję lub orientację w panelu po prawej.

S=shoulder (left/right), E=elbow (up/down), W=wrist (flip/noflip). Wiersz w kolorze zielonym to gałąź, do której aktualnie ustawione są przeguby. Klikając inny wiersz załadujesz tę konfigurację — robot trafi w tę samą pozę docelową inną drogą.

Co dalej

Solver ES5 (analytical-es5.ts) jest gotowy — może być teraz wpięty do M6 (Benchmark) jako drugi analityk obok Pumy.
M3 (Jakobianowe) — metody iteracyjne nie wymagają spełnienia warunku Piepera, działają dla dowolnej geometrii (Franka, UR, custom). To uniwersalne rozwiązanie dla przypadków, w których wyprowadzenie analityczne jest zbyt skomplikowane.
M6 (Benchmark) — porównanie analityka vs iteracja na konkretnych przypadkach testowych. Analityk wygrywa w prędkości (µs vs ms), iteracja w uniwersalności.