PV-OSIMr: A Lowest Order Complexity Algorithm for Computing the Delassus Matrix

Autores: Ajay Suresha Sathya, Wilm Decré, Jan Swevers · IEEE Robotics and Automation Letters (RA-L) 2024

Licencias

Acceso abierto
Artículo: CC BY-SA 4.0

Todo lo que sigue se basa en el artículo completo bajo su licencia CC BY-SA 4.0; su figura se muestra con crédito a los autores.

En resumen

Reordena los cálculos de PV-OSIM para que las restricciones propagadas nunca superen seis dimensiones, lo que da un cálculo de la matriz de Delassus en O(n + m²), que los autores muestran que es asintóticamente óptimo.

Por qué importa

Los robots con muchos contactos, como los humanoides con pies y manos de varios dedos, necesitan esta matriz a alta frecuencia para simulación y control, así que menos operaciones significan solvers más rápidos. Como construir una matriz m × m y recorrer las n articulaciones es inevitable, los autores sostienen que O(n + m²) es el orden óptimo. El algoritmo se implementó con generación de código de CasADi y, según el artículo, también en C++ en la biblioteca Pinocchio.

Problema

La matriz de Delassus, o matriz de inercia inversa en el espacio operacional, relaciona las fuerzas de restricción con las aceleraciones de restricción y es necesaria para simular dinámica con restricciones y con contactos y para el control en el espacio operacional, donde a menudo es el paso más costoso en MPC y RL. Calcularla de forma ingenua cuesta O(n³ + m²n). El mejor algoritmo conocido, EFPA, alcanza O(n + md + m²), mientras que el anterior PV-OSIM de los autores es O(n + m²d) y escala mal cuando muchas restricciones deben propagarse a través de muchas articulaciones.

Solución propuesta

PV-OSIMr modifica el barrido hacia atrás de PV-OSIM para que, en lugar de propagar matrices del espacio de restricciones que crecen con el número de restricciones, propague propagadores de fuerza extendidos de tamaño fijo 6×6 e inercias inversas espaciales entre eslabones de ramificación consecutivos (el conjunto N). Un barrido hacia delante limitado sobre N calcula las inercias inversas, un barrido hacia atrás limitado calcula el propagador del espacio de restricciones de cada efector final y cada bloque de la matriz de Delassus se ensambla en tiempo constante. Esto da una complejidad O(n + m²).

Ideas clave

Cuando un eslabón soporta más de seis dimensiones de restricción, propagar magnitudes espaciales 6D y recuperar después los términos del espacio de restricciones es más barato que propagar directamente los términos del espacio de restricciones.
Las magnitudes costosas solo deben calcularse en los ancestros comunes más cercanos de los eslabones restringidos, por lo que los barridos adicionales recorren un conjunto reducido de eslabones.
Una menor complejidad asintótica no garantiza menos operaciones en robots pequeños: EFPA resultó más costoso que PV-OSIM y PV-OSIMr en casi todos los mecanismos probados, debido a su estructura de tres barridos y a las transformaciones de marco por eslabón.
LTL-OSIM, pese a su mayor complejidad, superó a EFPA en una sola mano Shadow porque la ramificación de la mano crea una dispersión que el algoritmo aprovecha.

Resultados principales

Complejidad: PV-OSIMr O(n + m²) frente a PV-OSIM O(n + m²d), EFPA O(n + md + m²), KJR O(n + m²d), LTL-OSIM O(nd² + m²d + md²) y el cálculo ingenuo O(n³ + m²n).
Atlas de pie con cuatro contactos puntuales 3D por pie (12D por pie): PV-OSIM necesitó un 19,9 % más de operaciones que PV-OSIMr; con una unión rígida (weld) 6D por pie, los dos algoritmos fueron idénticos.
Atlas con manos Shadow (restricción 3D por punta de dedo, 15D por mano): la sección IV-D informa de hasta aproximadamente un 34 % menos de operaciones que PV-OSIM, el siguiente algoritmo más eficiente, mientras que las conclusiones indican una mejora de hasta el 24 % en los ejemplos con Atlas.
En un mecanismo de prueba ramificado con uniones rígidas 6D en el extremo de cada rama, PV-OSIMr igualó a PV-OSIM con una sola rama y escaló de forma más suave a partir de dos ramas; EFPA acabó superando a PV-OSIM en mecanismos largos con diez ramas por lado.
En un caso PV-OSIM necesitó aproximadamente un 1,5 % menos de operaciones: Atlas con uniones rígidas 6D en ambas manos y ambos pies, donde las restricciones atraviesan solo tres articulaciones antes de la base flotante.
Todas las evaluaciones son recuentos de operaciones de la implementación en CasADi, no tiempos de reloj.

Limitaciones

PV-OSIM puede ser más barato cuando más de seis dimensiones de restricción atraviesan solo unas pocas (tres o menos) articulaciones de un único DoF, porque sus actualizaciones de rango 1 superan a los dos cálculos de rango 6 de PV-OSIMr.
Cuando las restricciones solo se encuentran en la base flotante, o el mecanismo es una única cadena, PV-OSIMr no aporta ninguna ganancia frente a PV-OSIM.
Los resultados se presentan como recuentos de operaciones en lugar de tiempos de ejecución medidos.
El algoritmo se presenta para árboles cinemáticos; la versión en C++ para Pinocchio solo se iba a publicar tras la aceptación.

Impacto en el campo

El artículo ganó el RA-L 2025 Best Paper Award y tiene 4 citas. Los autores publicaron su implementación en CasADi como código fuente en GitHub e indican que se implementó una versión en C++ en la biblioteca Pinocchio para publicarla tras la aceptación.

Resumen

Presentamos PV-OSIMr, un algoritmo eficiente para calcular la matriz de Delassus (también conocida como matriz de inercia inversa en el espacio operacional) para un árbol cinemático, con la complejidad computacional de menor orden conocida en la literatura. PV-OSIMr se obtiene optimizando los cálculos del solver de Popov-Vereshchagin (PV) mediante la composicionalidad de los propagadores de fuerza y de movimiento. Tiene una complejidad computacional de O(n + m²), frente a O(n + m²d) del algoritmo PV-OSIM original y O(n+md+m²) del algoritmo de propagador de fuerza extendido (EFPA), donde n es el número de articulaciones, m es el número de restricciones y d es la profundidad del árbol cinemático. Dado que el cálculo de la matriz de Delassus requiere construir una matriz de tamaño m × m y debe considerar todas las n articulaciones al menos una vez, la complejidad computacional asintótica de PV-OSIMr es óptima. Además, evaluamos nuestro algoritmo y comprobamos que en la práctica suele ser más eficiente que PV-OSIM y EFPA.

Enlaces

arXiv 2310.03676: https://arxiv.org/abs/2310.03676
DOI 10.1109/LRA.2024.3469829: https://doi.org/10.1109/LRA.2024.3469829
PDF: https://arxiv.org/pdf/2310.03676v1
Código: https://github.com/AjSat/spatial_V2