Moduł 11 · Redundancja (7 DOF)

Rozwiązywanie redundancji (7 DOF Pandy)

Pseudoinverse, DLS, null-space projection, task priority (Siciliano-Slotine), HQP/iHQP, kryteria w null space, whole-body control.

TL;DR

Panda ma 7 stopni swobody (DOF), ale zadanie (pose narzędzia $\in SE(3)$ ) ma 6 wymiarów. Mamy zatem jeden wymiar redundancji — nieskończoność konfiguracji realizuje tę samą pozę TCP, tworząc 1-wymiarowy null-space manifold w $\mathcal{Q}$ .

Ten moduł odpowiada na pytanie: jak wykorzystać tę dodatkową swobodę?

Pseudo-odwrotność Moore-Penrose'a + damped least squares (DLS) — stabilna inwersja $J^+ \in \mathbb{R}^{7 \times 6}$ .
Null-space projection — kanoniczny mechanizm wykonywania drugorzędnych zadań bez zaburzania TCP.
Task priority(Siciliano & Slotine) — wiele zadań w hierarchii, każde realizowane w null-space wcześniejszych.
HQP (Hierarchical Quadratic Programming) — rozszerzenie task priority na ograniczenia nierównościowe.
Kryteria w null-space: maksymalizuj manipulowalność, odsuwaj się od limitów joints, omijaj przeszkody — wszystko bez zaburzenia TCP.
Whole-body control (TSID, OpenSoT) — uogólnienie dla mobilnych manipulatorów i humanoidów (z floating base).

Pseudo-odwrotność i DLS

Jakobian Pandy: $J(q) \in \mathbb{R}^{6 \times 7}$ . „Fat matrix" — więcej kolumn niż wierszy. Standardowe równanie $J \dot q = \dot x_d$ ma nieskończenie wiele rozwiązań dla $\dot q$ przy zadanym $\dot x_d$ .

Moore-Penrose pseudo-inverse

Wybiera rozwiązanie o minimalnej normie:

\dot q = J^+ \dot x_d = J^\top (J J^\top)^{-1} \dot x_d

Spełnia $\arg\min_{\dot q} \|\dot q\|^2 \;\text{s.t.}\; J\dot q = \dot x_d$ . Czyli z wszystkich możliwych $\dot q$ , wybiera „najleniwszy" (energii kinetycznej minimum, jeśli $J$ jest unweighted).

Damped Least Squares (DLS)

W pobliżu singularności $\det(J J^\top) \to 0$ , więc klasyczna pseudo-inverse wybucha — małe odchylenie $\dot x_d$ wymaga olbrzymich $\dot q$ . Fix regularization (Levenberg-Marquardt):

J^+_{\text{DLS}} = J^\top (J J^\top + \lambda^2 I)^{-1}

Parametr $\lambda > 0$ :

Daleko od singularności ( $\sigma_{\min}(J) \gg \lambda$ ): $J^+_{\text{DLS}} \approx J^+$ — bezpieczne tracking.
Blisko singularności: $J^+_{\text{DLS}}$ ogranicza wzmocnienie, kosztem małego tracking error $\sim \lambda^2 / \sigma_{\min}^2$ .

Typowo $\lambda \in [0.001, 0.05]$ . Adaptive damping: $\lambda$ rośnie gdy $\sigma_{\min}$ maleje (Nakamura & Hanafusa 1986, Maciejewski & Klein 1985).

Weighted pseudo-inverse

Z wagą $W \in \mathbb{R}^{7 \times 7}$ (np. macierz mas $W = M(q)$ ):

\dot q = W^{-1} J^\top (J W^{-1} J^\top)^{-1} \dot x_d

Minimalizuje $\dot q^\top W \dot q$ . Z masą daje minimum energii kinetycznej. Z $W = \mathrm{diag}(\text{joint priorities})$ — preferuje ruch w lekkich/szybkich joints.

Projekcja w przestrzeń zerową (null-space)

Kluczowa formuła sterowania redundancją:

\dot q = \underbrace{J^+ \dot x_d}_{\text{primary task}} + \underbrace{(I - J^+ J) \dot h_0}_{\text{secondary task, w null-space}}

Niech $N = I - J^+ J \in \mathbb{R}^{7 \times 7}$ . $N$ jest projektorem ortogonalnym na null-space jakobianu. Dla każdego $\dot h_0 \in \mathbb{R}^7$ , $N \dot h_0$ nie zaburza TCP:

J (N \dot h_0) = (J - J J^+ J) \dot h_0 = (J - J) \dot h_0 = 0

Co to znaczy intuicyjnie? $\dot h_0$ to „idealna" prędkość joints, jaką byśmy chcieli osiągnąć. Projekcja $N$ wyrzuca z niej składową, która zaburzyłaby pose TCP — zostawia tylko czystą zmianę konfiguracji bez wpływu na narzędzie.

Dla Pandy 7-DOF: $\dim(\text{null}(J)) = 1$ (typowo). Czyli null-space to jednowymiarowa krzywa w $\mathcal{Q}$ — można poruszać się wzdłuż niej zachowując pose. Klasyczna wizualizacja: „elbow swing" — trzymając chwytak w miejscu, oscylujemy łokciem.

„Trzymaj TCP, bujaj łokciem" — null-space motion

seed (v₀)

amplituda0.40 rad

prędkość1.0×

Co zobaczyć: Klasyczna preferencja: ruch w przegubie 4 (łokieć). Łokieć (środkowa część ramienia) oscyluje wyraźnie, a TCP (zielona kropka między palcami) zostaje blisko pozycji domowej. Małe odchylenie TCP (kilka mm dla amp = 0.4 rad) bierze się z tego, że kierunek null-space liczymy raz

v_{\text{null}}(q_0)

, a w trakcie ruchu jakobian (i null-space) lekko się zmienia — to znana nieliniowość redundancji. Pełna nieliniowa kontrola (HQP, integracja na manifoldzie) potrzebna gdyby błąd musiał być zerowy.

Co się dzieje pod maską

Demo precomputuje $v_{\text{null}} = N(q_0) \cdot v_{\text{seed}}$ gdzie $v_{\text{seed}}$ wybierasz z dropdownu (np. $e_4$ — preferencja joint 4). Wynik jednostkowy $v_{\text{null}}$ to kierunek elbow swing. Animacja:

q(t) = q_0 + A \sin(\omega t) \cdot v_{\text{null}}

To nie jest pełne rozwiązanie redundancji (które byłoby prawdziwym całkowaniem $N(q) \cdot v_{\text{seed}}$ z tracking primary task). Pełne rozwiązanie wymaga drobnego kroku ODE + ciągłej korekcji TCP error. Dla amp < 0.5 rad błąd TCP pozostaje rzędu mm — wystarczająco dla wizualnej demonstracji.

Hierarchia zadań (task priority)

Siciliano & Slotine 1991 uogólnienie null-space na wiele zadań w hierarchii. Niech:

Zadanie 1 (priorytet wyższy): $J_1 \dot q = \dot x_{1,d}$ , np. śledzenie TCP.
Zadanie 2 (niższy priorytet): $J_2 \dot q = \dot x_{2,d}$ , np. śledzenie pose łokcia.

Rozwiązanie kaskadowe:

\dot q = J_1^+ \dot x_{1,d} + (I - J_1^+ J_1) (\widetilde{J}_2)^+ \big( \dot x_{2,d} - J_2 J_1^+ \dot x_{1,d} \big)

gdzie $\widetilde{J}_2 = J_2 (I - J_1^+ J_1)$ — projekcja $J_2$ na null-space zadania 1.

Augmented Jacobian — alternatywne sformułowanie: układamy zadania w jedną macierz $J_{\text{aug}} = [J_1^\top, J_2^\top]^\top$ , rozwiązujemy jako jeden problem. Prostsze implementacyjnie, ale gubi informację o hierarchii (zadania mieszają się jeśli $J_{\text{aug}}$ niespójne).

HQP — Hierarchical Quadratic Programming

Task priority (Siciliano) działa dla równań równościowych ( $J\dot q = \dot x_d$ ). W praktyce mamy też nierówności: joint limits, momenty napędowe ≤ $\tau_{\max}$ , brak kolizji.

HQP(Kanoun & Lamiraux 2009, Escande 2014):

Rozwiąż QP dla zadania 1 — minimalizuj $\|J_1 \dot q - \dot x_{1,d}\|^2$ z ograniczeniami nierównościowymi.
Znajdź zbiór wszystkich optymalnych $\dot q$ dla zadania 1 — to polytope w 7D.
Rozwiąż zadanie 2 wewnątrz tego polytopu.
I tak dalej dla kolejnych priorytetów.

Praktycznie implementuje się przez active set method lub cascaded QPs z propagacją Lagrange multipliers. Czas: ~5 ms dla 7-DOF z 3 zadaniami.

iHQP (Escande, Mansard, Wieber 2014) — Inverse HQP z optymalną złożonością. Standard w stack-of-tasks library.

Typowe kryteria w null-space

Wybór $\dot h_0$ = gradient miary $H(q)$ → null-space motion maksymalizuje $H$ bez wpływu na TCP:

\dot h_0 = k \cdot \nabla H(q)

Popularne $H$ :

Manipulowalność Yoshikawy $w(q) = \sqrt{\det(J J^\top)}$ (moduł 02). Trzymaj się z dala od singularności.
Margines joint limits: $H_{\text{JL}}(q) = -\sum_i \big[(q_i - q_{\text{mid},i}) / r_i\big]^2$ . Maks. (=0) w środku każdego zakresu. Gradient wypycha od limitów.
Distance to obstacles: $H_{\Phi}(q) = \min_i \Phi_i(q)$ gdzie $\Phi_i$ to SDF do i-tej przeszkody (moduł 03).
Distance to singularities: gradient manipulowalności $\nabla w(q)$ .
Kinetic energy: $-\tfrac{1}{2} \dot q^\top M(q) \dot q$ — preferuje ruch w lekkich joints.
Posture cost: $-\tfrac{1}{2} \|q - q_{\text{nominal}}\|^2$ — trzymaj się blisko ustalonej „naturalnej" pozy (przydatne dla human-robot interaction).

Kombinacja: $\dot h_0 = \sum_k w_k \nabla H_k$ z wagami $w_k$ . Wszystkie razem rzutowane w null-space.

Demo: trzy kryteria z osobnymi wagami

Implementacja dokładnie tej formuły: $\dot h_0 = \sum_k w_k \nabla H_k(q)$ z trzema kryteriami:

$H_1 = w(q)$ — manipulowalność Yoshikawy (gradient liczony różnicami skończonymi);
$H_2 = -\sum_i \big[(q_i - q_{\text{mid},i})/r_i\big]^2$ — margines joint limits;
$H_3 = -\tfrac{1}{2}\|q - q_{\text{home}}\|^2$ — posture cost.

Przy każdej zmianie suwaka uruchamiamy krótki gradient-descent w null-space (80 kroków, $\Delta t = 0{.}02$ s) od $q_{\text{home}}$ z ustalonym TCP target = $\mathrm{FK}(q_{\text{home}})$ . Primary task (TCP error → 0) realizuje proporcjonalny regulator $K_p = 12$ , secondary direction = ważona suma gradientów.

Multi-task w null space — TCP fixed, secondary z wagami

Eksperymenty: (1) ustaw posture = 0, manipulability = 2 — Panda zmieni konfigurację bez zmiany TCP, by maksymalizować

w(q)

(odsunąć się od singularności). Obserwuj że wartość

w(q)

rośnie. (2) dodaj joint limit = 2 — joints, które wcześniej zbliżały się do limitu, odsuwają się (zielony pasek robi się dłuższy). (3) wszystkie naraz — solver szuka balansu. Mimo zmian w q, TCP błąd zostaje < 1 mm (ciągłe tracking primary task). To esencja redundancji: extra DOF służy kryteriom które normalnie wymagałyby zmiany pozy TCP.

Co obserwować

Mimo zmian $q$ wielkością rzędu 1 rad, błąd TCP utrzymuje się < 1 mm — primary task dominuje, a secondary kryteria robią użytek wyłącznie ze stopni swobody w null-space. To esencja redundancji: dodatkowe DOF służą realizacji kryteriów, które bez redundancji wymagałyby zmiany pozy TCP — sprzeczność z primary task.

Limitacja implementacji: każdy gradient wchodzi do null-space z osobnym mnożnikiem i wagą użytkownika. W produkcji (np. TSID) używałbyś task priority lub HQP — gwarantuje, że secondary nigdy nie naruszy primary nawet przy źle skalowanych wagach. Tu polegamy na dominacji $K_p$ nad mnożnikiem secondary.

Whole-body control: TSID, OpenSoT

Manipulator stacjonarny (Panda na biurku) ma jedno zadanie redundancyjne — null-space motion. Ale dla humanoidów (z floating base, kontaktem nóg, dwóch ramion, głowy) liczba zadań jest większa, a wymiar problemu eksploduje (do 50+ DOF).

Frameworki:

TSID (Task Space Inverse Dynamics, LAAS/CNRS) — C++ implementacja whole-body control via QP, używane na HRP-2, TALOS, Pyrene. stack-of-tasks/tsid
OpenSoT (IIT) — modularna biblioteka stosu zadań z HQP solver. Używana na COMAN+, Centauro.
Drake/TaskSpaceController — Robot Locomotion Group (MIT), używane na Boston Dynamics Atlas (DARPA Robotics Challenge).
Pinocchio + custom QP — najczęstszy stack badawczy (Python).

Wspólna idea: stack of tasks — lista zadań z priorytetami, każde implementowane jako równość lub nierówność, QP solver znajdzie optymalne $\dot q$ lub $\ddot q$ (jeśli pracujemy w torque control z dynamiką).

Redundancja w MPC i trajopt

W modułach 07 (trajopt) i 10 (MPC) redundancja pojawia się jako koszt w null space:

J(\xi) = \underbrace{J_{\text{task}}(\xi)}_{\text{primary}} + w_{\text{ns}} \cdot J_{\text{secondary}}(\xi)

Solver NLP wykorzystuje redundancję w trakcie optymalizacji — wybiera trajektorię, która dobrze realizuje primary task ORAZ optymalizuje secondary. Brak jawnego podziału na priority (jak w HQP), ale z odpowiednimi wagami konwergencja zbliżona.

Przykład w CHOMP: dodaj człon $J_{\text{secondary}} = -\sum_t w(q_t)$ (maksymalizuj manipulowalność wzdłuż trajektorii). Solver będzie preferował trajektorie omijające singularności — efekt podobny do null-space optimization, ale w sformułowaniu trajopt.

Ściąga

Damped pseudo-inverse

J^+_{\text{DLS}} = J^\top (J J^\top + \lambda^2 I)^{-1}

$\lambda \in [0.001, 0.05]$ . Adaptive: $\lambda$ rośnie blisko singularności.

Null-space projection (kanoniczna formuła)

\dot q = J^+ \dot x_d + (I - J^+ J) \dot h_0

$N = I - J^+ J$ — projektor w null-space. Dla Pandy 7-DOF $\dim \text{null}(J) = 1$ .

Task priority (Siciliano)

Kaskada przez null-space wcześniejszych zadań. Ostatnie zadanie dostaje co zostało po wcześniejszych.

HQP

Rozwiąż QP wewnątrz polytopu optymalnych $\dot q$ dla wyższych priorytetów. Obsługuje nierówności (joint limits, momenty).

Najpopularniejsze $\nabla H$ w null-space

Manipulowalność Yoshikawy (moduł 02)
Margines joint limits
Distance to obstacles (SDF, moduł 03)
Posture cost (trzymaj się $q_{\text{nominal}}$ )

Referencje

Liegeois, „Automatic Supervisory Control of the Configuration and Behavior of Multibody Mechanisms" (IEEE SMC 1977) — oryginalna idea null-space optimization.
Nakamura & Hanafusa, „Inverse Kinematic Solutions With Singularity Robustness for Robot Manipulator Control" (ASME J. Dyn. Sys. 1986) — DLS.
Maciejewski & Klein, „Obstacle Avoidance for Kinematically Redundant Manipulators in Dynamically Varying Environments" (IJRR 1985).
Siciliano & Slotine, „A general framework for managing multiple tasks in highly redundant robotic systems" (ICAR 1991) — task priority.
Kanoun, Lamiraux & Wieber, „Kinematic control of redundant manipulators: Generalizing the task-priority framework to inequality task" (IEEE TRO 2011) — HQP.
Escande, Mansard & Wieber, „Hierarchical quadratic programming: Fast online humanoid-robot motion generation" (IJRR 2014) — iHQP.
Yoshikawa, „Manipulability of Robotic Mechanisms" (IJRR 1985) — wskaźnik manipulowalności.
TSID (LAAS/CNRS): stack-of-tasks/tsid.
OpenSoT (IIT): ADVRHumanoids/OpenSoT.