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

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

Licences

• Accès libre
• Article: CC BY-SA 4.0

Tout ce qui suit s'appuie sur l'article complet sous sa licence CC BY-SA 4.0 ; sa figure est montrée en citant les auteurs.

En bref

Réordonne les calculs de PV-OSIM pour que les contraintes propagées ne dépassent jamais six dimensions, ce qui donne un calcul de la matrice de Delassus en O(n + m²), que les auteurs montrent asymptotiquement optimal.

Pourquoi c'est important

Les robots avec de nombreux contacts, comme les humanoïdes dotés de pieds et de mains à plusieurs doigts, ont besoin de cette matrice à haute fréquence pour la simulation et la commande ; moins d'opérations signifie donc des solveurs plus rapides. Comme construire une matrice m × m et parcourir les n articulations est inévitable, les auteurs soutiennent que O(n + m²) est l'ordre optimal. L'algorithme a été implémenté avec la génération de code CasADi et, selon l'article, également en C++ dans la bibliothèque Pinocchio.

Problème

La matrice de Delassus, ou inverse de la matrice d'inertie dans l'espace opérationnel, relie les forces de contrainte aux accélérations de contrainte ; elle est nécessaire à la simulation de la dynamique contrainte et de contact ainsi qu'à la commande dans l'espace opérationnel, où elle constitue souvent l'étape la plus coûteuse en MPC et en RL. Son calcul naïf coûte O(n³ + m²n). Le meilleur algorithme connu, EFPA, atteint O(n + md + m²), tandis que le PV-OSIM antérieur des auteurs est en O(n + m²d) et passe mal à l'échelle lorsque de nombreuses contraintes doivent être propagées à travers de nombreuses articulations.

Solution proposée

PV-OSIMr modifie le balayage arrière de PV-OSIM de sorte qu'au lieu de propager des matrices de l'espace des contraintes qui grandissent avec le nombre de contraintes, il propage des propagateurs de force étendus de taille fixe 6×6 et des inerties inverses spatiales entre corps de ramification consécutifs (l'ensemble N). Un balayage avant limité sur N calcule les inerties inverses, un balayage arrière limité calcule le propagateur de l'espace des contraintes de chaque effecteur terminal, et chaque bloc de la matrice de Delassus est assemblé en temps constant. On obtient ainsi une complexité en O(n + m²).

Idées clés

• Lorsqu'un corps supporte plus de six dimensions de contrainte, propager des grandeurs spatiales 6D et retrouver ensuite les termes de l'espace des contraintes coûte moins cher que propager directement les termes de l'espace des contraintes.
• Les grandeurs coûteuses ne doivent être calculées qu'aux plus proches ancêtres communs des corps contraints, si bien que les balayages supplémentaires portent sur un ensemble réduit de corps.
• Une complexité asymptotique plus faible ne garantit pas moins d'opérations sur de petits robots : EFPA était plus coûteux que PV-OSIM et PV-OSIMr pour presque tous les mécanismes testés, en raison de sa structure à trois balayages et de ses changements de repère par corps.
• LTL-OSIM, malgré sa complexité plus élevée, a battu EFPA sur une seule main Shadow, car les ramifications de la main créent une parcimonie qu'il exploite.

Principaux résultats

• Complexité : PV-OSIMr O(n + m²) contre PV-OSIM O(n + m²d), EFPA O(n + md + m²), KJR O(n + m²d), LTL-OSIM O(nd² + m²d + md²) et calcul naïf O(n³ + m²n).
• Atlas debout avec quatre contacts ponctuels 3D par pied (12D par pied) : PV-OSIM a nécessité 19,9 % d'opérations de plus que PV-OSIMr ; avec une soudure 6D par pied, les deux algorithmes étaient identiques.
• Atlas avec mains Shadow (contrainte 3D par bout de doigt, 15D par main) : la section IV-D rapporte jusqu'à environ 34 % d'opérations en moins que PV-OSIM, l'algorithme le plus efficace suivant, tandis que les conclusions indiquent jusqu'à 24 % d'amélioration sur les exemples Atlas.
• Sur un mécanisme de test ramifié avec des soudures 6D à l'extrémité de chaque branche, PV-OSIMr a égalé PV-OSIM pour une seule branche et a crû plus modérément à partir de deux branches ; EFPA a fini par battre PV-OSIM pour de longs mécanismes à dix branches par côté.
• Dans un cas, PV-OSIM a nécessité environ 1,5 % d'opérations de moins : Atlas avec des soudures 6D sur les deux mains et les deux pieds, où les contraintes ne traversent que trois articulations avant la base flottante.
• Tous les bancs d'essai sont des décomptes d'opérations issus de l'implémentation CasADi, et non des temps d'exécution mesurés.

Limites

• PV-OSIM peut être moins coûteux lorsque plus de six dimensions de contrainte ne traversent que quelques (trois ou moins) articulations à un seul DoF, car ses mises à jour de rang 1 l'emportent sur les deux calculs de rang 6 de PV-OSIMr.
• Lorsque les contraintes ne se rejoignent qu'à la base flottante, ou que le mécanisme est une chaîne simple, PV-OSIMr n'apporte aucun gain par rapport à PV-OSIM.
• Les résultats sont rapportés en décomptes d'opérations plutôt qu'en temps d'exécution mesurés.
• L'algorithme est présenté pour des arbres cinématiques ; la version C++ dans Pinocchio ne devait être publiée qu'après acceptation.

Impact sur le domaine

L'article a reçu le RA-L 2025 Best Paper Award et compte 4 citations. Les auteurs ont publié leur implémentation CasADi en code source sur GitHub et indiquent qu'une version C++ a été implémentée dans la bibliothèque Pinocchio en vue d'une publication à l'acceptation.

Résumé

Nous présentons PV-OSIMr, un algorithme efficace de calcul de la matrice de Delassus (aussi appelée inverse de la matrice d'inertie dans l'espace opérationnel) pour un arbre cinématique, avec la complexité de calcul d'ordre le plus bas connue dans la littérature. PV-OSIMr est obtenu en optimisant les calculs du solveur de Popov-Vereshchagin (PV) grâce à la compositionnalité des propagateurs de force et de mouvement. Sa complexité de calcul est O(n + m²), contre O(n + m²d) pour l'algorithme PV-OSIM d'origine et O(n+md+m²) pour l'algorithme du propagateur de force étendu (EFPA), où n est le nombre d'articulations, m le nombre de contraintes et d la profondeur de l'arbre cinématique. Comme le calcul de la matrice de Delassus exige de construire une matrice de taille m × m et doit prendre en compte les n articulations au moins une fois, la complexité de calcul asymptotique de PV-OSIMr est optimale. Nous évaluons en outre notre algorithme et constatons qu'il est souvent plus efficace que PV-OSIM et EFPA en pratique.

Liens

• 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
• Code: https://github.com/AjSat/spatial_V2