Collision Detection with Analytical Derivatives of Contact Kinematics

Resumen Técnico: iDCOL – Detección de Colisiones Diferenciable Implícita con Derivadas Analíticas

Declaración del Problema
Los métodos basados en gradientes en robótica (planificación, control, aprendizaje) requieren el cálculo rápido y preciso de las derivadas de las ecuaciones gobernantes con respecto a los estados del robot y las entradas de control. Un prerrequisito crítico para la física de contacto diferenciable es un mapa de cinemática de contacto bien definido y diferenciable que relacione los estados del robot con la distancia de contacto, la ubicación y la normal.

Los enfoques existentes enfrentan dos limitaciones principales:

1. Falta de suavidad en configuraciones degeneradas: Los métodos estándar de detección de colisiones (por ejemplo, GJK/EPA) dependen de la selección discreta de características. Cuando las características de contacto cambian (por ejemplo, cara contra cara, arista contra arista), las cantidades de contacto resultantes se vuelven definidas por partes y no diferenciables.
2. Degeneración geométrica: La diferenciabilidad está fundamentalmente limitada por la geometría de los cuerpos en contacto. Las geometrías no estrictamente convexas (por ejemplo, politopos, cilindros) poseen curvatura cero o indefinida en ciertas direcciones, lo que conduce a ubicaciones o normales de contacto no únicas. En contraste, las geometrías estrictamente convexas (por ejemplo, esferas, elipsoides) admiten puntos y normales de contacto únicos, lo que genera una cinemática diferenciable.

Metodología
El artículo presenta iDCOL (detección de colisiones diferenciable implícita), un marco que aborda estas limitaciones regularizando las geometrías no estrictamente convexas en representaciones implícitas estrictamente convexas.

1. Regularización Geométrica mediante Superficies Implícitas:
   En lugar de utilizar representaciones poliédricas o cónicas exactas, iDCOL modela los cuerpos en colisión utilizando superficies implícitas estrictamente convexas definidas por funciones escalares $\phi: \mathbb{R}^3 \to \mathbb{R}$.

• Politopos Suaves: Aproximados utilizando el operador LogSumExp (smooth-max) para mezclar las restricciones de semiespacio.
• Supercuádricas y Cilindros Superelípticos: Utilizados para aproximar cajas, conos y cilindros con exponentes de forma ($n$) y parámetros de nitidez ($\beta$) ajustables.
  Estos representaciones aseguran una curvatura finita positiva en todas partes, garantizando puntos y normales de contacto únicos.

2. Formulación de Optimización Basada en Escalamiento:
   Basándose en el marco DCOL, iDCOL formula la detección de colisiones como la búsqueda del factor de escalamiento uniforme mínimo $\alpha$ tal que dos cuerpos escalados compartan un punto de contacto. El problema se plantea como:
   $$\min_{x, \alpha} \alpha \quad \text{sujeto a} \quad \phi_1(x/\alpha) \le 0, \quad \phi_2(R^T(x-r)/\alpha) \le 0$$
   Las condiciones de Karush–Kuhn–Tucker (KKT) de este problema se reducen a un sistema de tamaño fijo de seis ecuaciones no lineales con seis incógnitas ($x \in \mathbb{R}^3, \alpha, \lambda_1, \lambda_2$).

3. Estrategia de Solución Numérica:

• Reparametrización: El factor de escalamiento se reparametriza como $\alpha = e^s$ para asegurar la positividad y mejorar el condicionamiento.
• Formulación de Sustitución: Para manejar configuraciones extremas (cuerpos casi coincidentes o ampliamente separados), un problema de sustitución reescala la traslación relativa para asegurar condiciones iniciales de no penetración.
• Solucionador: Se emplea un solucionador de tipo Newton personalizado con búsqueda de línea de retroceso (backtracking line search). Este utiliza Jacobianos calculados analíticamente y admite el uso de valores iniciales (warm-starting) de pasos de tiempo anteriores.

4. Derivadas Analíticas:
   Al aplicar el Teorema de la Función Implícita (IFT) al sistema de residuo KKT, el marco deriva derivadas analíticas de la solución de contacto con respecto a la configuración $q$. Esto permite el cálculo exacto de los Jacobianos para las cantidades de la cinemática de contacto (brecha, normal, puntos testigo) sin necesidad de diferenciación numérica o la sobrecarga de la diferenciación automática.

Contribuciones Clave

• Marco iDCOL: Un marco de detección de colisiones y cinemática de contacto diferenciable implícita que restaura la diferenciabilidad en configuraciones de contacto degeneradas mediante la regularización geométrica.
• Sistema No Lineal de Tamaño Fijo: A diferencia de otros enfoques de optimización que pueden variar en dimensión, iDCOL reduce la detección de colisiones a un sistema determinista de $6 \times 6$, permitiendo una solución y diferenciación eficientes.
• Derivadas Analíticas: La derivación de derivadas analíticas exactas para la cinemática de contacto utilizando el IFT, evitando el costo computacional y los errores de aproximación de la diferenciación numérica o automática.
• Implementación de Código Abierto: Se proporciona un solucionador en C++ extremadamente rápido, de escala de microsegundos, que soporta una biblioteca de primitivas implícitas suaves (politopos, conos truncados, superelipsoides, cilindros superelípticos).

Resultados y Evaluación
El enfoque fue evaluado mediante simulaciones extensas y pruebas comparativas:

• Robustez y Tiempo de Ejecución: El solucionador converge de manera confiable a través de diversos pares de formas. Sin el uso de valores iniciales (warm-starting), los tiempos de ejecución promedio oscilan entre ~2 µs (formas simples) y ~19 µs (superelipsoides complejos). Con warm-starting, los tiempos de ejecución caen consistentemente a ~2 µs.
• Comparación con DCOL:
  • Para contactos de politopo-politopo, iDCOL es inicialmente más lento que DCOL (que utiliza geometría poliédrica exacta), pero se vuelve comparable con warm-starting.
  • Para pares de formas suaves (por ejemplo, elipsoide-elipsoide), iDCOL es más rápido que DCOL.
  • Crucialmente, iDCOL proporciona una dependencia de la configuración más suave, lo cual es ventajoso para la optimización basada en gradientes, mientras que DCOL exhibe un comportamiento suave por partes debido a los cambios en el conjunto activo.
• Aplicaciones:
  • Planificación de Trayectorias de Cuadricópteros: La optimización basada en gradientes de una trayectoria libre de colisiones redujo el tiempo de computación en casi dos órdenes de magnitud en comparación con métodos sin gradientes analíticos.
  • Colisiones Rígidas Multicuerpo: Una simulación de 10 cuerpos interactuantes completó su ejecución en ~1.94s; sin derivadas analíticas, la simulación tardó ~6.5 veces más y mostró una robustez reducida.
  • Interacción Suave-Rígida: Se simuló con éxito la interacción de un manipulador blando con obstáculos rígidos. Sin derivadas analíticas, el solucionador se estancaba, resultando en tiempos de ejecución hasta dos órdenes de magnitud más largos.

Significado y Reivindicaciones
El artículo postula que iDCOL aborda una tensión fundamental entre la exactitud geométrica y la diferenciabilidad analítica en la robótica rica en contactos. Al intercambiar explícitamente la fidelidad geométrica exacta por la convexidad estricta (mediante parámetros de suavizado ajustables), el marco mitiga las degeneraciones geométricas que socavan la diferenciabilidad.

Los autores afirman que iDCOL permite consultas de colisión eficientes a escala de microsegundos con derivadas analíticas completas, lo que lo hace adecuado para su integración en marcos de simulación, control y optimización de trayectorias basados en gradientes. El marco está diseñado para ser modular, permitiendo la introducción de nuevas primitivas convexas simplemente definiendo sus funciones implícitas y derivadas, mientras que el algoritmo central permanece independiente de la forma. El trabajo tiene como objetivo facilitar el desarrollo de algoritmos escalables que acoplen estrechamente la geometría, la dinámica y la optimización.