Wykrywanie kolizji

Idea: dwa wypukłe zbiory $A, B$ kolidują wtedy i tylko wtedy, gdy ich różnica Minkowskiego $A \ominus B = \{a - b : a \in A, b \in B\}$ zawiera origin:

GJK nie buduje $A \ominus B$ jawnie (drogie). Zamiast tego iteracyjnie konstruuje simplex (1-, 2- lub 3-punktowy w 2D; do 4 w 3D) wewnątrz $A \ominus B$, próbując „objąć" origin. W każdej iteracji dodaje support point $s(d)$ w aktualnym kierunku $d$ ku origin.

Kluczowy lemmat: jeśli istnieje plane separujący $A$ i $B$ (czyli $A \cap B = \emptyset$), to w pewnej iteracji wystąpi:

GJK zwraca odpowiedź w średnio 4-8 iteracjach dla 3D wielokątów. Złożoność $O(m + n)$ per iteracja gdzie $m, n$ = liczby wierzchołków $A, B$.

Pokrewne i rozszerzenia

• EPA (Expanding Polytope Algorithm) — gdy GJK wykryje kolizję, EPA znajduje penetration depth i kierunek separacji (potrzebne dla rozwiązania kontaktu).
• MPR (Minkowski Portal Refinement) — alternatywa GJK + EPA z prostszym kodem, podobny performance.
• SAT(Separating Axis Theorem) — dla wielokątów: sprawdza projekcje na osie krawędzi. Szybsze dla małej liczby wierzchołków (boxes), wolniejsze dla wielokątów > 100 vertices.
• libccd, FCL, Bullet — biblioteki C++ implementujące GJK+EPA z optymalizacjami (caching simplex między klatkami).

GJK testuje jedną parę w ms. Ale w scenie z setkami obiektów (manipulator + półki + obiekty) liczba par to$O(n^2)$ — niedopuszczalne. Stąd potrzebny broad-phase: szybkie odrzucanie par, które na pewno nie kolidują.

BVH organizuje obiekt w drzewo bounding volumes (zwykle AABB lub OBB). Korzeń = AABB całego obiektu. Każdy węzeł wewnętrzny ma dwoje dzieci z mniejszymi AABB. Liście = pojedyncze trójkąty.

Test kolizji dwóch obiektów = jednoczesny obchód obu drzew:

function intersect(nodeA, nodeB):
  if not AABB_intersect(nodeA.aabb, nodeB.aabb):
    return false
  if both are leaves:
    return triangle_test(nodeA.tri, nodeB.tri)
  recursively test children

W typowych scenach koszt zapytania spada z $O(n_1 n_2)$ do $O(\log n_1 + \log n_2)$ + linear cost na liście.

Warianty bounding volumes

• AABB (Axis-Aligned BB) — 6 floats (xMin..zMax). Najtańszy test, ale luźne dla obrotowych obiektów.
• OBB (Oriented BB) — z 3D rotacją. Ciasne, ale test droższy (15 osi separacji per parze).
• k-DOP (Discrete Oriented Polytope) — k par normal-aligned half-spaces. Kompromis AABB/OBB; popularne k = 14, 18, 26.
• Sphere — 4 floats. Mega tani test, bardzo luźny (zwykle ostatni resort).

Convex decomposition: dla nie-wypukłych mesh'y (np. linków Pandy) BVH + GJK nie wystarcza — narzędzia jak V-HACD dekomponują geometrię na sumę wypukłych części, każda z osobnym narrow-phase.

Dyskretny collision check sprawdza kolizję tylko w chwilach $t_k = k \cdot \Delta t$. Jeśli obiekt porusza się szybko, między $t_k$ a $t_{k+1}$ może przeskoczyć przez przeszkodę — kolizja niewykryta.

CCD wykrywa time-of-impact $t^* \in [t_k, t_{k+1}]$ — pierwszą chwilę, w której obiekty stykają się. Najprostsza forma: swept volume — obejmujemy całą trajektorię obiektu między klatkami jednym objętym AABB, testujemy z geometrią statycznej sceny.

W manipulatorze CCD jest kluczowe podczas wykonania zaplanowanej trajektorii — bez niego szybkie ruchy (q̇ ~ 2 rad/s) mogą wjechać w przeszkodę między cyklami sterownika (1 kHz).

Algorytmy CCD w praktyce

• Swept-AABB vs static obstacle — najprostsze, wystarcza dla pocisków / mobile robotów.
• Continuous GJK (Bergen 2003) — sweep'owy GJK z parametryzacją czasem. Złożone, ale dokładne.
• Conservative advancement — iteracyjnie advance'uj symulację o czas mniejszy niż TOI, aż osiągniesz kontakt z toleracją.
• FCL implementuje conservative advancement z GJK podstawy.

Często potrzebujemy więcej niż „jest/brak kolizji" — chcemy odległość do najbliższej przeszkody i kierunek, w którym się oddalać. To podstawa:

• CHOMP / TrajOpt (moduł 07): gradient SDF wokół przeszkód działa jak siła wypychająca trajektorię.
• CBF-QP (moduł 12): bezpieczeństwo formalne — ograniczenie $\Phi(q(t)) \geq \epsilon_{\text{safe}}$.
• Reactive control (moduł 09): pola potencjałowe, fabrics.

GJK rozszerzony o zwracanie najbliższych punktów daje:

gdzie $(a^*, b^*)$ to najbliższe punkty na powierzchni każdego obiektu. Dla SDF (moduł 03) gradient jest natywny i ciągły.

Praktyczna kombinacja w cuRobo / NVIDIA Warp: SDF voxel grid przeszkód (na GPU) + analityczne SDF kapsuł dla ogniw Pandy (sferi + walce) → tysiące jednoczesnych queries dla batched MPC.

CPU collision check w pętli rolloutów MPPI (K = 1000 trajektorii × H = 30 kroków = 30 000 queries per iteracja, przy 50 Hz = 1.5M/s) szybko staje się wąskim gardłem. GPU pozwala na batched parallel queries:

• cuRobo(NVIDIA, 2023) — pełen kinodynamic stack na CUDA. Collision check oparty na sphere SDF dla ogniw + voxel SDF przeszkód. Tysiące rolloutów w < 10 ms.
• NVIDIA Warp — Python ↔ CUDA framework dla physics. Implementacja BVH + GJK na GPU dostępna w warp.sim.
• Isaac Sim / Isaac Lab — pełna fizyka GPU dla RL, używa PhysX-based collision.
• Flex / FleX — granular / soft body GPU collision.

Aplikacja webowa (ten projekt) używa CPU collision — JS nie ma dostępu do CUDA. Dla rzeczywistego deployment na manipulatorze z GPU rekomendacja: cuRobo (open-source, integrowany z ROS2).

Manipulator 7-DOF (i więcej) może sam się ze sobą zderzyć — przedramię z barkiem, gripper z bazą. Bez self-collision checking planer może zwrócić „idealną" trajektorię, która w praktyce zniszczy robota.

Naiwne podejście: dla wszystkich par ogniw $(i, j)$ wykonaj collision check. Dla Pandy: $\binom{8}{2} = 28$ par. Większość jest jednak trywialna:

• Sąsiednie ogniwa (dzielące staw) zawsze się stykają — bez sensu sprawdzać.
• Pary fizycznie nieosiągalne (baza ↔ link 1) — ignored.
• Pary o znikomym prawdopodobieństwie kolizji można też wyłączyć dla performance.

Allowed Collision Matrix (ACM)zapisuje, które pary są aktywnie sprawdzane. MoveIt2 generuje ACM automatycznie przez próbkowanie 10 000 random configs i wyłączenie par, które kolidują > 95% przypadków (zawsze stykające) lub < 0.1% (nigdy). Reszta to faktycznie ryzykowne kombinacje.