Collision Detection with Analytical Derivatives of Contact Kinematics

Résumé Technique : iDCOL – Détection de Collision Différentiable Implicite avec Dérivées Analytiques

Énoncé du Problème
Les méthodes basées sur le gradient en robotique (planification, contrôle, apprentissage) nécessitent le calcul rapide et précis des dérivées des équations directrices par rapport aux états du robot et aux entrées de commande. Un prérequis critique pour la physique de contact différentiable est une application de la cinématique de contact bien définie et différentiable qui relie les états du robot à la distance de contact, à la localisation et à la normale.

Les approches existantes font face à deux limitations principales :

1. Non-lissé dans les configurations dégénérées : Les méthodes standard de détection de collision (par exemple, GJK/EPA) reposent sur une sélection discrète de caractéristiques. Lorsque les caractéristiques de contact changent (par exemple, face-à-face, arête-à-arête), les quantités de contact résultantes deviennent définies par morceaux et non différentiables.
2. Dégénérescence Géométrique : La différentiabilité est fondamentalement limitée par la géométrie des corps en contact. Les géométries non strictement convexes (par exemple, les polytopes, les cylindres) possèdent une courbure nulle ou indéfinie dans certaines directions, entraînant des localisations ou des normales de contact non uniques. En revanche, les géométries strictement convexes (par exemple, les sphères, les ellipsoïdes) admettent des points et des normales de contact uniques, produisant une cinématique différentiable.

Méthodologie
Le présent article introduit iDCOL (implicit differentiable collision detection), un cadre qui traite ces limitations en régularisant les géométries non strictement convexes en représentations implicites strictement convexes.

1. Régularisation Géométrique via des Surfaces Implicites :
   Au lieu d'utiliser des représentations polyédriques ou coniques exactes, iDCOL modélise les corps en collision à l'aide de surfaces implicites strictement convexes définies par des fonctions scalaires $\phi: \mathbb{R}^3 \to \mathbb{R}$.

• Polytopes Lisses : Approchés en utilisant l'opérateur LogSumExp (smooth-max) pour mélanger les contraintes d'hyperplans.
• Superquadriques et Cylindres Superelliptiques : Utilisés pour approximer des boîtes, des cônes et des cylindres avec des exposants de forme ($n$) et des paramètres de netteté ($\beta$) ajustables.
  Ces représentations garantissent une courbure finie positive partout, garantissant des points et des normales de contact uniques.

2. Formulation d'Optimisation Basée sur le Changement d'Échelle :
   S'appuyant sur le cadre DCOL, iDCOL formule la détection de collision comme la recherche du facteur de mise à l'échelle uniforme minimal $\alpha$ tel que deux corps mis à l'échelle partagent un point de contact. Le problème est posé comme :
   $$\min_{x, \alpha} \alpha \quad \text{sous contrainte de} \quad \phi_1(x/\alpha) \le 0, \quad \phi_2(R^T(x-r)/\alpha) \le 0$$
   Les conditions de Karush–Kuhn–Tucker (KKT) de ce problème se réduisent à un système de taille fixe de six équations non linéaires avec six inconnues ($x \in \mathbb{R}^3, \alpha, \lambda_1, \lambda_2$).

3. Stratégie de Résolution Numérique :

• Reparamétrage : Le facteur d'échelle est reparamétré comme $\alpha = e^s$ pour garantir la positivité et améliorer le conditionnement.
• Formulation de Substitution : Pour gérer les configurations extrêmes (corps quasi-coïncidents ou largement séparés), un problème de substitution redimensionne la translation relative afin d'assurer des conditions initiales sans pénétration.
• Solveur : Un solveur de type Newton personnalisé et sécurisé avec recherche linéaire par retour arrière est employé. Il utilise des Jacobiennes calculées analytiquement et supporte le réamorçage (warm-starting) à partir des pas de temps précédents.

4. Dérivées Analytiques :
   En appliquant le Théorème de la Fonction Implicite (TFI) au système résiduel KKT, le cadre dérive les dérivées analytiques de la solution de contact ($z^*$) par rapport à la configuration $q$. Cela permet le calcul exact des Jacobiennes des quantités de cinématique de contact (écart, normale, points témoins) sans différenciation numérique ou surcharge de la différenciation automatique.

Contributions Clés

• Cadre iDCOL : Un cadre de détection de collision et de cinématique de contact différentiable implicite qui restaure la différentiabilité dans les configurations de contact dégénérées grâce à la régularisation géométrique.
• Système Non Linéaire de Taille Fixe : Contrairement aux approches d'optimisation précédentes qui peuvent varier en dimension, iDCOL réduit la détection de collision à un système déterministe de $6 \times 6$, permettant une résolution et une différenciation efficaces.
• Dérivées Analytiques : La dérivation de dérivées analytiques exactes pour la cinématique de contact via le TFI, évitant le coût de calcul et les erreurs d'approximation de la différenciation numérique ou automatique.
• Implémentation Open-Source : Un solveur C++ rapide, à l'échelle de la microseconde, supportant une bibliothèque de primitives implicites lisses (polytopes, cônes tronqués, superellipsoïdes, cylindres superelliptiques).

Résultats et Évaluation
L'approche a été évaluée par des simulations approfondies et des tests de performance :

• Robustesse et Temps d'Exécution : Le solveur converge de manière fiable pour diverses paires de formes. Sans réamorçage, les temps d'exécution moyens varient d'environ 2 µs (formes simples) à 19 µs (superellipsoïdes complexes). Avec le réamorçage, les temps d'exécution tombent systématiquement à environ 2 µs.
• Comparaison avec DCOL :
  • Pour les contacts polytope-polytope, iDCOL est initialement plus lent que DCOL (qui utilise une géométrie polyédrique exacte), mais devient comparable avec le réamorçage.
  • Pour les paires de formes lisses (ex: ellipsoïde-ellipsoïde), iDCOL est plus rapide que DCOL.
  • Crucialement, iDCOL fournit une dépendance de configuration plus lisse, ce qui est avantageux pour l'optimisation basée sur le gradient, tandis que DCOL présente un comportement lissé par morceaux dû aux changements d'ensemble actif.
• Applications :
  • Planification de Trajectoire de Quadrotor : L'optimisation par gradient d'une trajectoire sans collision a réduit le temps de calcul de près de deux ordres de grandeur par rapport aux méthodes sans dérivées analytiques.
  • Collisions de Corps Rigides Multi-corps : Une simulation de 10 corps rigides interagissant a été complétée en environ 1,94 s ; sans dérivées analytiques, la simulation a pris environ 6,5 fois plus longtemps et présentait une robustesse réduite.
  • Interaction Souple-Rigide : Un manipulateur souple interagissant avec des obstacles rigides a été simulé avec succès. Sans dérivées analytiques, le solveur stagnait, entraînant des temps d'exécution jusqu'à deux ordres de grandeur plus longs.

Signification et Revendications
L'article pose que iDCOL répond à une tension fondamentale entre l'exactitude géométrique et la différentiabilité analytique dans la robotique riche en contacts. En échangeant explicitement la fidélité géométrique exacte contre une convexité stricte (via des paramètres de lissage ajustables), le cadre atténue les dégénérescences géométriques qui compromettent la différentiabilité.

Les auteurs affirment que iDCOL permet des requêtes de collision efficaces, à l'échelle de la microseconde, avec des dérivées analytiques complètes, ce qui le rend adapté à l'intégration dans des cadres de simulation, de contrôle et d'optimisation de trajectoire basés sur le gradient. Le cadre est conçu pour être modulaire, permettant l'introduction de nouvelles primitives convexes simplement en définissant leurs fonctions implicites et leurs dérivées, tandis que l'algorithme central reste indépendant de la forme. Ce travail vise à faciliter le développement d'algorithmes évolutifs capables de coupler étroitement la géométrie, la dynamique et l'optimisation.