Reprezentacja problemu i przestrzeni

Co z tego wynika dla planowania w wysokich wymiarach

Konstrukcja $\mathcal{Q}_{\text{obs}}$ w sposób deklaratywny (jak w powyższym demo — siatka, każda komórka, collision check) jest niewykonalna w wysokowymiarowych C-space:

• Dla 2-link arm (2D C-space, 80×80 grid): 6 400 collision checks.
• Dla Pandy (7D C-space, 80⁷ grid): 2.1 × 10¹³ checks — 1000 razy więcej niż atomów w komórce.

Dlatego planowanie w wysokowymiarowych C-space (Panda, humanoidy) opiera się na metodach próbkowych (PRM, RRT — moduł 06) lub optymalizacyjnych (CHOMP, TrajOpt — moduł 07) — żadna z nich nie konstruuje $\mathcal{Q}_{\text{obs}}$ jawnie, tylko zapytuje o kolizję dla konkretnych $q$.

Binarna informacja „jest kolizja / nie ma" jest za uboga dla optymalizacji. Planery CHOMP/TrajOpt i kontrolery CBF-QP potrzebują gradientu: kierunku, w którym przesunąć stan żeby oddalić się od przeszkód. SDF tę informację daje natywnie.

Konwencja znaków: $\Phi(p) > 0$ dla wolnej przestrzeni (im większe, tym dalej od przeszkód), $\Phi(p) = 0$ na brzegu, $\Phi(p) < 0$ wewnątrz przeszkody (im bardziej ujemne, tym głębiej).

Co to jest $\nabla \Phi$

Symbol $\nabla$ (czytaj „nabla") to operator gradientu. Dla funkcji skalarnej $\Phi(p) = \Phi(x, y)$:

To wektor w każdym punkcie $p$, wskazujący kierunek największego wzrostu $\Phi$. W kontekście SDF: skoro $\Phi$ rośnie gdy oddalamy się od przeszkód, $\nabla \Phi$ wskazuje kierunek „uciekania" od najbliższej przeszkody.

Dlaczego wszystkie strzałki mają mniej-więcej tę samą długość? To kluczowa właściwość SDF:

Intuicja: gdy posunę się o 1 cm w kierunku $\nabla \Phi$, $\Phi$ wzrasta o 1 cm — bo to właśnie odległość od przeszkody. Stąd „jednostka zmiany pozycji = jednostka zmiany Φ", czyli $\|\nabla \Phi\| = 1$. Gradient SDF niesie informację tylko o kierunku, nie o magnitudzie. W demo strzałki są rysowane ze stałą długością — to odzwierciedla ten fakt.

Wyjątki: gradient nieokreślony na „szkielecie" wolnej przestrzeni (zbiór punktów równo odległych od ≥2 przeszkód) i na brzegu (Φ=0). W praktyce uciszamy te przypadki numerycznie.

Co robi suwak „kontur $\Phi = c$"

Suwak ustawia wartość $c$ — odległość bezpieczeństwa. Niebieska/fioletowa linia rysowana na heatmapie to level set (zbiór poziomicowy):

czyli wszystkie punkty oddalone od najbliższej przeszkody dokładnie o $c$ jednostek:

• $c = 0$ → kontur pokrywa brzeg przeszkód (czarny)
• $c = 0.05$ → linia 0.05 (5%) jednostek na zewnątrz przeszkód — taką ścieżkę musi trzymać robot żeby zachować margines bezpieczeństwa 5%
• $c = -0.02$ → linia 2% jednostek wewnątrz przeszkody (głębokość penetracji)

Planery CBF (moduł 12) używają stałej $c = \varepsilon_{\text{safe}}$ jako twardego ograniczenia: trajektoria musi spełniać $\Phi(q(t)) \geq \varepsilon_{\text{safe}}$ w każdej chwili. CHOMP (moduł 07) używa miękkiego kosztu $c_{\text{obs}}(q) = \max(0, \varepsilon - \Phi(q))^2$ — kara rośnie kwadratowo gdy trajektoria wpada w pas bezpieczeństwa.

W demo poniżej: przesuwając suwak obserwuj jak linia konturu „pęcznieje" wokół przeszkód (większe $c$) lub „obkurcza się" w ich wnętrze (ujemne $c$).

Warianty w praktyce

• TSDF (Truncated SDF) — KinectFusion / Voxblox: wartości $\Phi$ obcinane do $[-d_{\max}, d_{\max}]$ dla oszczędności pamięci.
• ESDF (Euclidean SDF) — FIESTA: rozszerzenie TSDF z dokładnym gradientem aż do dużych odległości; preferowane dla planowania.
• Neural SDF — DeepSDF, NeRF-Nav: sieć$f_\theta(p) \approx \Phi(p)$; bardzo zwarte, ale inference ~ms.

Praktyczne kombinacje

W praktyce wybór reprezentacji wynika z typu planera i źródła danych. Pięć typowych kombinacji (skróty rozwijam na bieżąco):

• Planery próbkowe (np. PRM — Probabilistic Roadmap, RRT* — Rapidly-exploring Random Tree*; pełnio omówione w module 06): pytają tylko „czy konfiguracja koliduje?" — wystarczy siatka trójkątów (mesh) obiektu zorganizowana w BVH (Bounding Volume Hierarchy — drzewo obejmujących prostopadłościanów, patrz moduł 04). Standardowa biblioteka: FCL (Flexible Collision Library, Pan et al. 2012) lub jej C++17 fork HPP-FCL (HPP = Humanoid Path Planner, Pinocchio Lab). Wydajność: ~O(log n) per query.
• Planery optymalizacyjne (np. CHOMP — Covariant Hamiltonian Optimization for Motion Planning, TrajOpt — Trajectory Optimization; moduł 07): potrzebują gradientu $\nabla \Phi$ w każdym punkcie wzdłuż trajektorii. Preferują ESDF (Euclidean Signed Distance Field — pole SDF z dokładnymi wartościami i gradientem aż do dużych odległości). Implementacje: FIESTA (Fast Incremental ESDF, Han et al. 2019), Voxblox.
• Online z czujnika (mobilny manipulator z kamerą RGB-D / LiDAR): surowe dane to point cloud (chmura punktów ze współrzędnymi XYZ + kolor). Konwersja do Octomap (Hornung et al. 2013 — octree-based voxel grid; oktanowe drzewo, log-kompresja zajętości przestrzeni), potem standardowe collision queries w czasie planowania.
• GPU collision dla sterowania predykcyjnego:cuRobo (NVIDIA, Sundaralingam et al. 2023) i Warp (NVIDIA Python ↔ CUDA framework dla symulacji fizyki) używają SDF w voxel grid (siatka wokseli 3D) + batched queries (sprawdzenie tysięcy konfiguracji równolegle). Niezbędne dla planerów takich jak MPPI (Model Predictive Path Integral; moduł 10) czy CEM(Cross-Entropy Method) — sample'ują K = 1000+ trajektorii w pętli sterowania > 50 Hz.
• Self-collision Pandy (sprawdzanie, czy robot sam się nie ze sobą zderzy — moduł 04): zamiast pełnego mesha (~10 000 trójkątów per ogniwo, kosztowne) używamy aproksymacji prymitywami — każde ogniwo opakowane w kilka sfer i capsules (kapsuły = cylinder zakończony dwiema półsferami; analityczne SDF). Wystarcza ~21 par par sprawdzeń per konfiguracja (Pandy ma 8 ogniw, sąsiednie pary skipped — patrz Allowed Collision Matrix w module 04).

Każdy planer (A*, RRT, CHOMP) korzysta z funkcji odległości $d: \mathcal{Q} \times \mathcal{Q} \to \mathbb{R}_{\geq 0}$ — przyjmuje dwa punkty $(q_a, q_b)$ z C-space i zwraca nieujemną liczbę „jak daleko od siebie są". Wybór tej metryki zmienia charakter rozwiązań — RRT z metryką L2 i RRT z metryką ważoną dadzą różne drzewa na tej samej scenie.

L2 (euklidesowa)

Po prostu pitagorasowa odległość między dwoma wektorami w $\mathbb{R}^n$: dla każdej z n współrzędnych weź różnicę, podnieś do kwadratu, zsumuj, pierwiastek. Najprostsza, domyślna w większości planerów. Problem: traktuje wszystkie joints jednakowo, mimo że joint 1 Pandy rusza całą masą ramienia, a joint 7 tylko gripperem. Mały błąd w joint 1 = duży błąd w pozycji TCP; mały błąd w joint 7 = niewielki.

Ważona

To samo co L2, ale każda współrzędna mnożona przez wagę $w_i$. $W = \mathrm{diag}(w_1, \ldots, w_n)$oznacza macierz diagonalną z wagami na przekątnej; w rozwinięciu po prostu $d_W^2 = \sum_i w_i (q_a^i - q_b^i)^2$. Wagi $w_i$ typowo odzwierciedlają mass/inertia poszczególnych joints — joint 1 dostaje większą wagę niż joint 7. Dla Pandy popularne wagi (z URDF inertias): mniej-więcej $(2, 2, 1.5, 1.5, 1, 0.5, 0.5)$.

Riemannowska / geodezyjna

L2 dobrze działa, gdy C-space „leży na płasko" jak $\mathbb{R}^n$. Ale gdy C-space jest manifoldem zakrzywionym (np. obejmuje $SO(3)$ — rotacje 3D, lub $SE(3)$ — pełne pozy), prosta odległość euklidesowa nie ma sensu. Wyobraź sobie pytanie „jak daleko jest z Warszawy do Tokio" — odpowiedź „linia prosta przez wnętrze Ziemi" jest matematycznie poprawna, ale nieużyteczna; chcemy długości po powierzchni. Analogicznie w C-space liczymy długość najkrótszej krzywej (zwanej geodezją) na manifoldzie. Dla rotacji 3D:

Tu $R_a, R_b \in SO(3)$ to dwie macierze rotacji (a nie wektory współrzędnych). Operator $\log: SO(3) \to \mathfrak{so}(3)$ („logarytm na manifoldzie") jest odwrotnością formuły Rodriguesa — przekształca macierz rotacji w wektor kąta-osi (axis-angle, moduł 02). W praktyce: odległość ta równa się kątowi rotacji, jaki przejść z orientacji $R_a$ do $R_b$ obracając wokół jednej osi. Tę metrykę wybierają planery operujące na pełnych pozach $SE(3)$, np. planowanie zadań manipulacyjnych w operational space (moduł 09, 11).

Task-space metric (w przestrzeni operacyjnej)

$p(q)$ to funkcja forward kinematics (moduł 02) — przyjmuje konfigurację $q$i zwraca pozę narzędzia (TCP) w $SE(3)$. Ta metryka mierzy „jak daleko jest TCP od TCP", nie „jak daleko są konfiguracje od siebie". Dla 7-DOF redundantnego robota dwie różne $q$ mogą mieć tę samąTCP — w task-space ich odległość = 0, a w L2/W bywa duża. Używana przy planowaniu zadań geometrycznych (rysowanie konturu, malowanie), gdzie liczy się pozycja narzędzia, a nie konkretne ułożenie ramienia.

Wszystko poniżej można zapisać jako standardowe zadanie:

Składniki:

• Stan $q \in \mathcal{Q}$ — konfiguracja (path planning) lub stan dynamiczny $(q, \dot q)$ (kinodynamic).
• Akcja / sterowanie $u \in \mathcal{U}$ — dla kinodynamic $u = \ddot q$, dla dynamic $u = \tau$ (moment napędowy).
• Constraints geometryczne: brak kolizji $\sigma(t) \in \mathcal{Q}_{\text{free}}$.
• Constraints różniczkowe: limity prędkości/ przyspieszeń/jerk $|\dot q_i| \leq \dot q_{\max,i}$, dynamika $M\ddot q + C\dot q + g = \tau$.
• Funkcja kosztu $J$ — czas ($L = 1$), energia ($L = \|\tau\|^2$), jerk ($L = \|\dddot q\|^2$), clearance ($L = -\Phi(q)$), albo kombinacja.
• Solution criterion — kompletność (algorytm zwraca rozwiązanie gdy istnieje), optymalność (zwraca najlepsze), probabilistic-completeness (gwarancja w probabilistycznym sensie — RRT, PRM).

W konkretnych modułach kursu spotkasz różne realizacje:

• Moduł 05 (grafowe): $\mathcal{Q}$ dyskretyzowana, $J$ jako suma wag krawędzi.
• Moduł 06 (próbkowe): $\mathcal{Q}$ ciągła, kolizja zapytania (collision query), nie funkcja kosztu.
• Moduł 07 (optymalizacyjne): pełen NLP z $J$, constraints jako penalty lub równania KKT.
• Moduł 10 (MPC): skrócony horyzont, online QP / rolloutowanie.