Podstawy matematyczne

Poza robota (lub jego końcówki) to element $SE(3) = SO(3) \ltimes \mathbb{R}^3$ — pół-bezpośredni iloczyn rotacji i translacji. Macierzowa reprezentacja jednorodna (4×4):

Tangent space (twist) $\mathfrak{se}(3) \cong \mathbb{R}^6$: para $(\omega, v)$ — prędkość kątowa i liniowa. Jakobian $J(q) \in \mathbb{R}^{6 \times n}$ mapuje $\dot q \to (v, \omega)$:

Dla Pandy $n = 7$, więc $J \in \mathbb{R}^{6 \times 7}$ — redundancja. $J$ ma więcej kolumn niż wierszy → nieskończenie wiele rozwiązań dla zadanego $(v, \omega)$. Wykorzystanie tej redundancji to temat modułu 11.

Denavit-Hartenberg (1955) — uniwersalna konwencja zapisu kinematyki manipulatora seryjnego (łańcucha kinematycznego bez gałęzi). Idea: każde ogniwo opisujemy czterema liczbami zamiast pełną macierzą 4×4. Z 7 ogniw Pandy: 28 liczb, nie 7×16=112.

Cztery parametry DH

Dla każdego przegubu $i$:

• $a_{i-1}$ — długość ogniwa poprzedniego, czyli długość wspólnej normalnej między osiami $z_{i-1}$ i $z_i$. Mierzymy wzdłuż osi $x_{i-1}$.
• $\alpha_{i-1}$ — kąt skręcenia osi $z$ między ogniwami; obrót wokół $x_{i-1}$.
• $d_i$ — offset wzdłuż osi $z_i$, gdzie ogniwo łączy się z poprzednim.
• $\theta_i$ — kąt przegubu; dla przegubu rewolutnego ten parametr jest zmienną konfiguracyjną $q_i$.

Konwencja (Modified DH, Craig): osie $z_i$ układa się wzdłuż osi obrotu i-tego przegubu (dla rewolutnych) lub osi przesuwu (dla pryzmatycznych). Osie $x_i$ wzdłuż wspólnej normalnej z $z_{i-1}$ (lub dowolne jeśli osie są równoległe).

Macierz pojedynczego przejścia $T_i^{i-1}$

Złożenie 4 elementarnych transformacji w ścisłym porządku:

Po rozwinięciu mamy konkretną macierz 4×4:

gdzie $c_x = \cos x$, $s_x = \sin x$. Implementacja w src/lib/panda/fk.ts (dhTransform) — 16 obliczanych pól, 8 wywołań sin/cos.

FK to złożenie 7 macierzy DH plus statyczna transformacja flange:

Implementacja w TS — kanoniczna pętla src/lib/panda/fk.ts:

let T = I4;
for (let i = 0; i < 7; i++) {
  T = mat4mul(T, dhTransform(PANDA_DH[i], q[i]));
  frames.push(T);  // zapamiętaj dla jakobianu / wizualizacji
}
T = mat4mul(T, flangeOffset);  // statyczne 10.7 cm

Wynik dla q_home: $g(q_{\text{home}}).p \approx (0.307, 0, 0.487)$ m. Smoke test w lib/panda/__smoke.ts sprawdza to per build.

Złożoność: O(n) mnożeń macierzy, czyli ~7×16 float operations = ~112 mnożeń + sin/cos w każdym kroku. Czas wykonania w przeglądarce: ~5 μs per ewaluacja FK. Pandy 1 kHz control loop < 0.5% budżetu czasowego.

Dla zadanego pose $g_d \in SE(3)$ znajdź $q$ takie, że $g(q) = g_d$. Pandzie 7-DOF brak postaci zamkniętej (nie spełnia warunku Piepera — ostatnie 3 osie nie przecinają się w jednym punkcie). Dla takich robotów używamy numeryki.

Newton-Raphson na pose error

Niech $e(q) = \log(g_d^{-1} g(q)) \in \mathfrak{se}(3)$ = 6-wektor błędu (twist od bieżącej do docelowej pozy). Linearyzacja: $e(q + \Delta q) \approx e(q) + J(q) \Delta q$. Krok Newtona:

Iteruj aż $\|e\| < \epsilon$. Konwergencja kwadratowa w pobliżu rozwiązania, ale wrażliwa na singularności i lokalne minima.

Damped Least Squares (DLS)

W pobliżu singularności $J^+$ wybucha. Fix: $J^+_\lambda = J^\top (JJ^\top + \lambda^2 I)^{-1}$ z $\lambda \in [0.001, 0.05]$. Patrz moduł 11 dla szczegółów.

TRAC-IK (Beeson & Ames 2015)

Hybryda: dwie metody równolegle: SQP (KDL standard) + twist-aware DLS. Bierze pierwsze udane. Threshold: 95% success rate na typowych zadaniach Pandy, vs 70% klasycznego KDL.

IKFast (Diankov 2010)

Generowanie analitycznej IK przez Gröbner basis dla konkretnego robota. Dla 6-DOF: zamknięta postać do 16 rozwiązań. Dla 7-DOF wymaga ustalenia 1 parametru redundancji (np. zawsze $q_7 = 0$, lub elbow angle = const).

Pełne wyprowadzenie analityczne IK (Puma560, 6-DOF, warunek Piepera) w siostrzanym projekcie ik-platform — środek nadgarstka, q₁ z geometrii, q₂q₃ z prawa kosinusów, q₄q₅q₆ z macierzy rotacji, 8 rodzin rozwiązań.

Jakobian geometryczny $J(q) \in \mathbb{R}^{6 \times 7}$ opisuje wzajemność prędkości joints i prędkości TCP (linear + kątowej). Manipulowalność mówi jak dobrze robot jest „przygotowany" w danej konfiguracji — czyli jak duże prędkości TCP może osiągać dla jednostkowych prędkości joints.

Druga równość: $\sigma_i$ to wartości szczególne $J$, więc $w$ = iloczyn długości półosi elipsoidy manipulowalności. Mała $w$ oznacza, że jedna z osi jest prawie zerowa — niemal-singularność. $w = 0$ dokładnie wtedy, gdy $J$ traci rank.

W demo poniżej widzisz elipsoidę manipulowalności (część liniowa, 3×3) w pozycji TCP. Suwakami joints obserwuj jak zmienia się jej kształt — przy konfiguracjach „rozprostowanego ramienia" elipsoida się spłaszcza.

W papersachi bibliotekach robotyki spotkasz różne definicje „jakobianu". Najczęstsze nieporozumienie w  implementacji: liczę spatialJacobian, a wzór wymaga body. Trzy warianty, wszystkie 6×n:

1. Spatial Jacobian $J_s(q)$

Definiowany tak, że dla twistu w world frame:

gdzie $v_s$ = prędkość liniowa frame'u końcówki wyrażona w world ALE z punktem zaczepienia w origin world (interpretacja „extended twist"). Kolumna i-ta $J_s$: $\text{Ad}_{g_i^{w}}(z_i)$ gdzie $z_i$ = oś joint i, $g_i^w$ = pose frame'u i w world. To jakobian używany w klasycznych pochodnych algebra Liego (Murray-Li-Sastry).

2. Body Jacobian $J_b(q)$

Twist wyrażony w body frame (frame'ie końcówki):

$v_b$ = prędkość liniowa w reprezentacji body. Naturalne dla zadań w przestrzeni narzędzia (force-feedback, peg-in-hole — chcemy prędkość „przy końcówce"). Związek z spatial: $J_b = \text{Ad}_{g^{-1}} J_s$.

3. Geometric / hybrid Jacobian $J_g(q)$

Najczęściej w praktyce. Prędkość liniowa końcówki w world, prędkość kątowa w world. NIE jest „prawdziwym" twistem (mieszamy reprezentacje), ale jest najwygodniejszy obliczeniowo:

Kolumna i-ta dla przegubu rewolutnego:

To jest jakobian, który implementuję w lib/panda/jacobian.ts— używany w CHOMP, TrajOpt, MPC, redundancji. Wszystkie wartości w world frame, łatwo wizualizować i komponować z target velocity.

Konwersje między wariantami

Mając $J_g$ (najprostszy do obliczenia), pozostałe wyznaczamy przez transformację adjoint:

gdzie $g = (R, p)$, $[p]_\times$ = macierz skośnie-symetryczna z $p$. Adjoint mapuje twisty między frame'ami.

Praktyczna reguła wyboru

• Sterowanie w przestrzeni zadania (OSC, CBF, tracking pose) — geometric $J_g$: chcę „idź w +x w world".
• Force feedback / contact tasks — body $J_b$: siła mierzona przy końcówce.
• Lie-theoretic analysis (CHOMP geodesics, exp coords) — spatial $J_s$: kompozycja z exp/log naturalna.
• MoveIt2, Pinocchio — domyślnie geometric, ale z funkcjami konwersji.

Pułapka — manipulability i numeric stability

Wskaźnik manipulowalności $w(q) = \sqrt{\det(JJ^\top)}$ zależy od wyboru wariantu jakobianu. Na singularności wszystkie są równoważnie 0, ale w pobliżu wartości się różnią (kierunki głównych osi elipsoidy w  różnych frame'ach). Implementacje typowo używają geometric, ale warto być świadomym przy porównaniu z literaturą.

Singularność = konfiguracja $q^*$ taka, że $\text{rank}(J(q^*)) < 6$. W tej konfiguracji jakobian nie jest pełnego rzędu: istnieje kierunek w przestrzeni prędkości TCP, którego nie da się osiągnąć żadną prędkością joints (a fakt że Panda jest 7-DOF nie pomaga, bo redundancja jest w joints, nie w task space).

Klasyfikacja: brzegowe (TCP na granicy osiągalności — ramię całkowicie rozprostowane) i wewnętrzne (specjalne wewnętrzne konfiguracje, jak wyrównane osie sąsiednich joints).

Pokażemy mechanizm na klasycznym 2-link arm (RR planarny), gdzie singularność wyraża się jednym wzorem:

Singularne: $q_2 = 0$ (ramię rozprostowane) i $q_2 = \pm \pi$ (złożone). Suwakami zobacz jak $\det J$ przechodzi przez 0.

$\mathcal{Q}$ nie jest zwykłym $\mathbb{R}^n$ — kąty obrotowe są cyrkulane ($0 \equiv 2\pi$), więc każdy joint to $\mathbb{S}^1$, nie odcinek. Skutki praktyczne:

• Metryka w C-space nie jest euklidesowa — odległość między $q_a = 350°$ a $q_b = 10°$ to $20°$, nie $340°$. To wpływa na każdy planer (A*, RRT, CHOMP) — używa się angle wrap lub pracuje na cosinusach.
• Topologia drasycznie inna od euklidesa. Np. RRT* na torusie zachowuje się inaczej niż na płaszczyźnie (ścieżki mogą „okrążać" świat).
• Dla Pandy z limitami joints (np. $q_1 \in [-2.897, 2.897]$ rad, nie pełne $[-\pi, \pi]$) niektóre joints są quasi-cyrkulane — okrąg ze „ścianami". Topologicznie bliżej do odcinka.

Równanie ruchu manipulatora w postaci Lagrange'a:

gdzie $M$ macierz mas, $C \dot q$ człony Coriolisa i odśrodkowe, $g$ grawitacja, $\tau$ momenty napędowe, $F_{\text{ext}}$ zewnętrzne siły.

Algorytm rekursywny RNEA (Recursive Newton-Euler) liczy $\tau$ z $(q, \dot q, \ddot q)$ w czasie liniowym względem liczby joints. Implementacja: Pinocchio.

Pełna pochodna i implementacja w ik-platform/modules/9-dynamics — tu, w module planowania, dynamika pojawi się dopiero w modułach 10 (MPC dynamiczne) i 13 (kontakt). Do tego momentu pracujemy kinematycznie.