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.