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

Licenças

Acesso aberto
Artigo: CC BY-SA 4.0

Tudo o que segue se baseia no artigo completo sob a licença CC BY-SA 4.0; sua figura é mostrada com crédito aos autores.

Em resumo

Reordena os cálculos do PV-OSIM para que as restrições propagadas nunca excedam seis dimensões, resultando em um cálculo da matriz de Delassus em O(n + m²), que os autores mostram ser assintoticamente ótimo.

Por que importa

Robôs com muitos contatos, como humanoides com pés e mãos de vários dedos, precisam dessa matriz em alta frequência para simulação e controle, então menos operações significam solvers mais rápidos. Como construir uma matriz m × m e percorrer todas as n juntas é inevitável, os autores argumentam que O(n + m²) é a ordem ótima. O algoritmo foi implementado com geração de código do CasADi e, segundo o artigo, também em C++ na biblioteca Pinocchio.

Problema

A matriz de Delassus, ou matriz de inércia inversa no espaço operacional, relaciona forças de restrição a acelerações de restrição e é necessária para a simulação de dinâmica com restrições e com contato e para o controle no espaço operacional, em que muitas vezes é a etapa mais custosa em MPC e RL. Calculá-la de forma ingênua custa O(n³ + m²n). O melhor algoritmo conhecido, o EFPA, chega a O(n + md + m²), enquanto o PV-OSIM anterior dos autores é O(n + m²d) e escala mal quando muitas restrições precisam ser propagadas por muitas juntas.

Solução proposta

O PV-OSIMr modifica a varredura para trás do PV-OSIM para que, em vez de propagar matrizes do espaço de restrições que crescem com o número de restrições, ele propague propagadores de força estendidos de tamanho fixo 6×6 e inércias inversas espaciais entre elos de ramificação consecutivos (o conjunto N). Uma varredura para frente limitada sobre N calcula as inércias inversas, uma varredura para trás limitada calcula o propagador do espaço de restrições de cada efetuador final, e cada bloco da matriz de Delassus é montado em tempo constante. Isso dá complexidade O(n + m²).

Principais ideias

Quando um elo suporta mais de seis dimensões de restrição, propagar grandezas espaciais 6D e recuperar depois os termos do espaço de restrições é mais barato que propagar diretamente os termos do espaço de restrições.
As grandezas custosas só precisam ser calculadas nos ancestrais comuns mais próximos dos elos restritos, de modo que as varreduras extras percorrem um conjunto reduzido de elos.
Uma complexidade assintótica menor não garante menos operações em robôs pequenos: o EFPA foi mais custoso que o PV-OSIM e o PV-OSIMr em quase todos os mecanismos testados, por causa de sua estrutura de três varreduras e das transformações de referencial por elo.
O LTL-OSIM, apesar da complexidade maior, superou o EFPA em uma única mão Shadow porque a ramificação da mão cria uma esparsidade que ele explora.

Principais resultados

Complexidade: PV-OSIMr O(n + m²) contra PV-OSIM O(n + m²d), EFPA O(n + md + m²), KJR O(n + m²d), LTL-OSIM O(nd² + m²d + md²) e o cálculo ingênuo O(n³ + m²n).
Atlas em pé com quatro contatos pontuais 3D por pé (12D por pé): o PV-OSIM precisou de 19,9 % mais operações que o PV-OSIMr; com uma união rígida (weld) 6D por pé, os dois algoritmos foram idênticos.
Atlas com mãos Shadow (restrição 3D por ponta de dedo, 15D por mão): a seção IV-D relata até cerca de 34 % menos operações que o PV-OSIM, o próximo algoritmo mais eficiente, enquanto as conclusões indicam melhoria de até 24 % nos exemplos com o Atlas.
Em um mecanismo de teste ramificado com uniões rígidas 6D na ponta de cada ramo, o PV-OSIMr igualou o PV-OSIM com um único ramo e escalou de forma mais suave a partir de dois ramos; o EFPA acabou superando o PV-OSIM em mecanismos longos com dez ramos por lado.
Em um caso, o PV-OSIM precisou de cerca de 1,5 % menos operações: Atlas com uniões rígidas 6D nas duas mãos e nos dois pés, em que as restrições passam por apenas três juntas antes da base flutuante.
Todas as avaliações são contagens de operações da implementação em CasADi, não tempos de relógio.

Limitações

O PV-OSIM pode ser mais barato quando mais de seis dimensões de restrição passam por apenas algumas (três ou menos) juntas de um único DoF, porque suas atualizações de posto 1 superam os dois cálculos de posto 6 do PV-OSIMr.
Quando as restrições só se encontram na base flutuante, ou o mecanismo é uma única cadeia, o PV-OSIMr não traz ganho em relação ao PV-OSIM.
Os resultados são apresentados como contagens de operações, e não como tempos de execução medidos.
O algoritmo é apresentado para árvores cinemáticas; a versão em C++ para o Pinocchio só seria disponibilizada após a aceitação.

Impacto na área

O artigo ganhou o RA-L 2025 Best Paper Award e tem 4 citações. Os autores disponibilizaram sua implementação em CasADi como código-fonte no GitHub e afirmam que uma versão em C++ foi implementada na biblioteca Pinocchio para ser disponibilizada após a aceitação.

Resumo

Apresentamos o PV-OSIMr, um algoritmo eficiente para calcular a matriz de Delassus (também conhecida como matriz de inércia inversa no espaço operacional) para uma árvore cinemática, com a complexidade computacional de menor ordem conhecida na literatura. O PV-OSIMr é obtido otimizando os cálculos do solver de Popov-Vereshchagin (PV) por meio da composicionalidade dos propagadores de força e de movimento. Ele tem complexidade computacional de O(n + m²), contra O(n + m²d) do algoritmo PV-OSIM original e O(n+md+m²) do algoritmo de propagador de força estendido (EFPA), em que n é o número de juntas, m é o número de restrições e d é a profundidade da árvore cinemática. Como o cálculo da matriz de Delassus exige construir uma matriz de tamanho m × m e precisa considerar todas as n juntas pelo menos uma vez, a complexidade computacional assintótica do PV-OSIMr é ótima. Além disso, avaliamos nosso algoritmo e constatamos que, na prática, ele costuma ser mais eficiente que o PV-OSIM e o EFPA.

Open Source Robotics