Moduł 13 · Rozszerzenia

Planowanie z kontaktem

Complementarity, soft contact, contact-implicit trajopt, mode sequence MIQP, through-contact manipulation, compliant control, kontakt jako resource.

TL;DR

Klasyczne planowanie zakłada brak kontaktu — robot ma się poruszać w wolnej przestrzeni, kolizja = niepowodzenie. Ale wiele zadań manipulacyjnych wymaga kontaktu: pushing, chwytanie, montaż, in-hand manipulation. Tu kontakt jest narzędziem, nie błędem.

Modele kontaktu: rigid (LCP/complementarity), soft (penalty), spring-damper, hydroelastic.
Contact-implicit trajectory optimization: solver znajduje jednocześnie trajektorię i sekwencję kontaktów.
Mode sequence planning + MIQP: jawnie wybór które kontakty są aktywne w którym momencie.
Through-contact manipulation: push, slide, pivot, regrasp — sterowanie obiektem przez kontakt.
Compliant control: impedance/admittance — sterowanie z dynamiką elastyczną dla bezpiecznego kontaktu.
Kontakt jako resource: in-hand finger-gaiting używa chwilowych kontaktów do reorientacji obiektu.

Friction cone — fundament modelowania kontaktu

Wzór Coulomba (1781): siła kontaktu między dwiema powierzchniami dzieli się na normalną $F_n$ i styczną $F_t$ . Kontakt jest stick (powierzchnie się nie ślizgają) wtedy i tylko wtedy gdy:

|F_t| \leq \mu \cdot |F_n|

gdzie $\mu$ to coefficient of friction (typowo 0.2-1.5 dla różnych materiałów). Graficznie: wektor siły musi leżeć wewnątrz friction cone — stożka o półkącie $\arctan(\mu)$ wokół normalnej.

Stick vs slip jest fundamentalne dla manipulacji:

Stick: obiekt porusza się jak rigid body z ramieniem — predykcja deterministyczna.
Slip: relatywny ruch między palcem a obiektem; dynamika nieliniowa, trudniejsza predykcja.
Granica między stick a slip jest niegładka — stąd kontakt z LCP/complementarity ma niegładkie równania.

Friction cone — stick vs slip

✓ STICK (rigid push)

kąt θ (od normalnej)20°

\mu

(tarcie)0.40

|F|

0.15

Cone half-angle: 21.8° (arctan μ). Twoja siła jest 20° od normalnej. Wewnątrz cone'a — kontakt utrzymany, obiekt przesuwa się jak rigid body.

Wzór Coulomba: siła kontaktu

F = F_n \, \hat n + F_t \, \hat t

. Kontakt utrzymany (stick) wtedy i tylko wtedy gdy

|F_t| \leq \mu |F_n|

. Cone'a tworzą wszystkie kierunki spełniające ten warunek — półkąt

\arctan(\mu)

. Dla mu=0.4: ~21.8°. Dla suchego kontaktu metal-metal: μ ≈ 0.2-0.5; gumy: 0.7-1.5; lód: ≈ 0.05. Dlaczego ważne dla planera:— jeśli planuje przez through-contact (np. pushing), musi per kroksprawdzić czy zadana siła jest w cone'ie. Inaczej kontakt rozłącza się i model dynamiczny przestaje obowiązywać.

Friction cone w 3D — pyramid approximation

W 3D true friction cone to stożek (continuum kierunków). Numerycznie aproksymujemy go polyhedral pyramid — 4 lub 8 wektorów krawędziowych. Wtedy constraint $F_t \in \text{cone}$ staje się $F = \lambda_1 e_1 + \ldots + \lambda_k e_k$ z $\lambda_i \geq 0$ — sumą nieujemnych ważonych wektorów brzegowych. Reprezentacja LP-friendly.

Modele kontaktu — przegląd

Rigid contact (LCP — Linear Complementarity Problem)

Najściślejszy: kontakt traktowany jako twarda nierówność. Per krok solver rozwiązuje:

0 \leq F_n \perp d(q) \geq 0

(komplementarność: albo distance = 0 i siła > 0, albo distance > 0 i siła = 0). Plus friction Coulomb. To LCP — class problemu z dedykowanymi solverami (Lemke, PATH). MuJoCo używa wariantu (soft constraint) dla differentiability.

Soft contact (penalty)

Zamiast hard constraint, kara w funkcji potencjalnej: $V(d) = k_p \max(0, -d)^2 + c_d \max(0, -\dot d)^2$ (Hunt-Crossley). Łatwiejsze numerycznie, ale wymaga małych kroków czasu (sztywność $k_p$ potrzebuje stability). Standard w PyBullet/Bullet.

Spring-damper

Najprostsze: $F_n = k_p \cdot \delta + c_d \cdot \dot \delta$ gdzie $\delta$ = penetration depth. Łatwo wdrożyć, ale realne tylko dla małych prędkości. Stosowane w grach.

Hydroelastic contact (Drake)

Najbardziej realistyczne: obiekt ma scalar pressure field wewnątrz; integracja po obszarze kontaktu daje siłę i moment. Lepsze dla deformowalnych powierzchni (gripper palec z gumą). Implementacja w Drake (Castro et al. 2020).

Contact-implicit trajectory optimization

Klasyczne TrajOpt (moduł 07) zakłada że sekwencja kontaktów jest dana (np. „w fazie 1 lewy palec, w fazie 2 prawy"). Co jeśli chcemy ją wyznaczyć automatycznie przez optymalizator?

Posa, Cantu, Tedrake 2014

Pierwsze contact-implicit TrajOpt. Constraints kontaktu wrzucone jako complementarityw NLP:

d(q_k) \cdot \lambda_n = 0, \quad \lambda_n \geq 0

Solver IPOPT/SNOPT znajdują trajektorię + sekwencję kontaktów jednocześnie. Wada: NLP jest nielinearny i niegładki przy aktywacji kontaktu (gradient skacze). Praktycznie konwerguje dla 5-10 kontaktów.

Manchester, Kuindersma 2017 — Variational Integrators

Dyskretyzacja zachowująca energię + impulsy momentum przy kontakcie. Stabilniejsze numerycznie niż prosty Euler.

Tassa, Erez, Todorov 2012 — Smooth complementarity

MPC z miękkim modelem kontaktu (smooth approximation LCP) → gradient dostępny → iLQR/DDP. Skaluje się znacznie lepiej. Standard w mujoco_mpc.

Mode sequence planning + MIQP

Dla zadań ze znaną strukturą (np. chodzenie z 4 nogami): dyskretyzuj wybór kontaktu — w każdej chwili wybierz binarnie który kontakt jest aktywny.

MIQP (Mixed Integer Quadratic Program): optymalizuj trajektorię (continuous variables) + sekwencję trybów (integer variables) jednocześnie. Solvery: Gurobi, Mosek.

Zastosowania:

Footstep planning dla humanoidów (Deits 2014) — wybierz stopy do każdej kroku.
In-hand manipulation (Hogan, Rodriguez 2020) — sekwencja pinch grasps dla rotation obiektu.
Pushing przez wielu agentów — kto pcha kiedy.

Ograniczenie: MIQP skaluje się wykładniczo z liczbą trybów. Dla N = 20 kroków i 4 trybów per krok: 4²⁰ = 10¹² kombinacji. Heurystyki + branch-and-bound do tego rzędu wielkości.

Through-contact manipulation

Through-contact: kontakt jest zamierzonym narzędziem do osiągnięcia celu manipulacyjnego.

Pushing

Robot pcha obiekt po stole. Quasi-static (małe prędkości, brak inercji):

\dot p_{\text{obj}} = J_{\text{push}}(p_{\text{contact}}, F) \cdot F

Lynch & Mason 1996 wyprowadzili motion cone dla pushed objects: zbiór kierunków ruchu osiągalnych przez dane kontakt + cone tarcia. Klasyczna teoria push planning.

Sliding

Obiekt ślizga się po powierzchni. Tarcie ślizgowe (zwykle $\mu_{\text{slide}} < \mu_{\text{stick}}$ ). Modele: limit surface (Goyal 1991) — wszystkie pary force-twist konsystentne z tarciem.

Pivoting (in-hand)

Obracanie obiektu wokół jednego punktu kontaktu (palec jako pivot). Wymaga koordynacji palcy + kierunkowa siła.

Regrasping

Reorientacja obiektu przez sekwencję pinch grasps. Klasyczna implementacja: finger gaiting (Han, Trinkle 1998). Każdy palec puszcza obiekt i przemieszcza się, pozostałe utrzymują.

Compliant control — sterowanie z dynamiką elastyczną

Przy kontakcie sterowanie sztywne (PD z dużym wzmocnieniem) powoduje skoki sił → uszkodzenie. Lepsze: compliant control.

Impedance control (Hogan 1985)

Reguluj impedancję ramienia — wirtualną masa-sprężyna- tłumik:

F_{\text{ext}} = M \ddot{\tilde x} + D \dot{\tilde x} + K \tilde x

gdzie $\tilde x = x - x_d$ = błąd pozycji. Robot zachowuje się jak miękka sprężyna. Klasyczny dla peg-in-hole, polishing.

Admittance control

Odwrotnie: zmierz siłę kontaktu, oblicz żądaną pozycję (z modelu impedance), pozycyjny PD śledzi. Stabilniejsze przy kontakcie z miękkim sensorem siły, ale wolniejsze.

Hybrid force/position

Dla niektórych kierunków sterujemy pozycją (np. trzymanie tablicy), dla innych siłą (np. nacisk na ścianę). Stosowane w polerowaniu, spawaniu.

Variable impedance

Zmieniaj $K, D$ w trakcie ruchu — sztywno gdy potrzebne, miękko przy kontakcie z człowiekiem. Często uczone (Calinon, Kormushev) z demonstracji.

Kontakt jako resource

Zaawansowane manipulatory celowo używają kontaktów do osiągnięcia celów, których nie da się osiągnąć przez ruch w wolnej przestrzeni:

Caging — obiekt zamknięty kinematycznie przez kontakty (nie chwycony siłowo).
Form closure — geometria kontaktów uniemożliwia ruch obiektu (bardzo stabilny chwyt).
Force closure — siły tarcia mogą balansować dowolną zewnętrzną siłę.
Funnel manipulation (Mason 2001) — sekwencja ruchów zbiega obiekt do żądanego stanu mimo niepewności początkowej.

Ściąga

Friction cone (Coulomb)

|F_t| \leq \mu |F_n| \;\Leftrightarrow\; F \in \text{friction cone}

Cone half-angle: arctan(μ). μ: metal-metal ≈ 0.3, guma ≈ 1.0

Modele kontaktu

Rigid + LCP/complementarity (MuJoCo, Drake) — fizyka
Soft penalty (PyBullet, Bullet) — łatwe, ale stiff
Spring-damper — gry
Hydroelastic (Drake) — realistic gripper

Contact-implicit trajopt

Trajektoria + sekwencja kontaktów w jednym NLP. Posa 2014, Manchester 2017. Tassa smooth-LCP dla iLQR/DDP.

Mode sequence + MIQP

Binarne wybory which-contact-active per krok. Footstep planning, in-hand. Gurobi/Mosek. Wykładnicze skalowanie.

Through-contact

Pushing — Lynch-Mason motion cone
Sliding — Goyal limit surface
Pivoting — palec jako oś
Regrasping — finger gaiting (Han-Trinkle)

Compliant control

Impedance (Hogan 1985): F = Mẍ + Dẋ + Kx
Admittance: F → x_d (odwrotne)
Hybrid force/position (Raibert-Craig)
Variable impedance (learned)

Referencje

Stewart & Trinkle, „An implicit time-stepping scheme for rigid body dynamics with inelastic collisions and Coulomb friction" (Int. J. Numerical Methods in Engineering 1996) — LCP foundation.
Hogan, „Impedance Control: An Approach to Manipulation" (J. Dyn. Syst. Meas. Control 1985).
Lynch & Mason, „Stable Pushing: Mechanics, Controllability, and Planning" (IJRR 1996).
Goyal, Ruina, Papadopoulos, „Limit surface and moment function descriptions of planar sliding" (ICRA 1989).
Posa, Cantu & Tedrake, „A direct method for trajectory optimization of rigid bodies through contact" (IJRR 2014) — contact-implicit TrajOpt.
Manchester & Kuindersma, „Variational Contact- Implicit Trajectory Optimization" (ISRR 2017).
Tassa, Erez & Todorov, „Synthesis and Stabilization of Complex Behaviors through Online Trajectory Optimization" (IROS 2012) — smooth-LCP DDP.
Deits & Tedrake, „Footstep planning on uneven terrain with mixed-integer convex optimization" (Humanoids 2014) — MIQP for legged.
Hogan & Rodriguez, „Reactive Planar Non-prehensile Manipulation with Hybrid Model Predictive Control" (IJRR 2020).
Castro, Qu, Kuppuswamy, Alspach, Sherman, „A Transition-Aware Method for the Simulation of Compliant Contact with Regularized Friction" (IEEE RA-L 2020) — hydroelastic w Drake.