Resumo Técnico: ContactIPM

Declaração do Problema

A otimização de trajetórias com contato implícito (CITO) busca otimizar estados de robôs, entradas, forças de contato e modos de contato simultaneamente, sem prescrever uma sequência de contato. Essa flexibilidade introduz Programas Matemáticos com Restrições de Complementaridade (MPCCs), onde o contato unilateral e o atrito são modelados via condições de complementaridade (ex: $0 \le a \perp b \ge 0$).

Esses problemas apresentam um desafio numérico fundamental: em pontos de MPCC factíveis, os gradientes das restrições de não-negatividade ativa e de produto são estruturalmente dependentes, fazendo com que as qualificações de restrição padrão (como LICQ) falhem. Essa degenerescência torna os solvers convencionais de programação não linear primal–dual pouco confiáveis, resultando frequentemente em sistemas KKT singulares ou mal condicionados.

Abordagens existentes tratam esse problema de robustez abandonando o passo de Newton primal–dual totalmente acoplado. Métodos como o CRISP otimizam apenas a trajetória primal através de subproblemas convexos, enquanto o IMPACT utiliza um loop externo de Lagrangiana aumentada com descida de coordenação em blocos. Por outro lado, solvers de controle ótimo que exploram a estrutura (ex: acados) utilizam recursões de Riccati para eficiência, mas frequentemente falham em gerar contato útil se a complementaridade exata for imposta sem tratamento especial. Permanece uma lacuna entre a robustez específica de MPCC e a eficiência dos solvers de controle ótimo (OCP) que exploram a estrutura.

Metodologia: ContactIPM

O ContactIPM propõe um método de ponto interior primal–dual unificado que mantém a estrutura de bloco-bandada de OCP necessária para a recursão de Riccati, enquanto lida com restrições de complementaridade de forma robusta.

1. Relaxação Interior Elástica Acoplada à Barreira

Em vez de usar pesos de penalidade ou Lagrangianas aumentadas, o ContactIPM introduz uma relaxação interior elástica estágio-local para cada par de complementaridade marcado $(a, b)$:

• Variáveis de Folga (Slacks): Folgas do lado positivo ($s_a, s_b$) e uma folga de produto ($s_c$) são introduzidas.
• Elevação Elástica (Elastic Lift): A condição de complementaridade é transformada em um sistema de igualdades:
  $$g_a + s_a = 0, \quad g_b + s_b = 0, \quad g_a g_b - \theta(\mu) + s_c = 0$$
  onde $\theta(\mu) = \gamma_{mpcc}\mu$ vincula o estreitamento da complementaridade diretamente ao parâmetro de barreira $\mu$.
• Regularidade: Esta construção garante que o Jacobiano da restrição elevada contenha um bloco identidade em relação às folgas. Consequentemente, o problema relaxado satisfaz a Qualificação de Independência Linear de Restrições (LICQ) em cada ponto interior, garantindo multiplicadores únicos e um sistema primal–dual regular.
• Sem Penalidade Independente: Diferente dos métodos de penalidade, o estreitamento da complementaridade é controlado pelo mesmo parâmetro de barreira $\mu$ que define o caminho central, eliminando a necessidade de coeficientes de penalidade ajustados independentemente.

2. Eliminação Primal–Dual por Estágio e Recursão de Riccati

O solver mantém a consistência primal–dual, atualizando a trajetória, multiplicadores de dinâmica, multiplicadores de desigualdade e folgas como componentes de um único sistema KKT perturbado.

• Eliminação Local: Como os pares de complementaridade e suas variáveis elásticas são locais a cada estágio individual, o solver elimina as variáveis de folga ($\Delta s_k$) e duais ($\Delta \lambda_k$) por estágio.
• Sistema Reduzido: Esta eliminação modifica apenas o Hessiano local do estágio ($\tilde{H}_k$) e o gradiente ($\tilde{q}_k$). A dinâmica permanece como o único acoplamento entre estágios adjacentes, preservando a estrutura de bloco-bandada.
• Resolução Eficiente: O sistema reduzido é resolvido usando uma recursão de Riccati para trás seguida de substituição direta, alcançando complexidade linear $O(N)$ em relação ao comprimento do horizonte.

3. Recuperação de Múltiplas Fases de MPCC

Para lidar com os múltiplos ramos estacionários associados a diferentes modos de contato, o ContactIPM emprega um cronograma de recuperação fixo envolvendo:

• Continuação: Estreitamento gradual do parâmetro de barreira enquanto tenta permanecer no atual bacia de atração do modo de contato.
• Reinício (Restart): Se a continuação falhar (ex: devido à perda de condicionamento), o solver restaura o palpite primal original e reinicia com um parâmetro de barreira maior e curvatura de Gauss–Newton para explorar diferentes bacias.
• Terminação: O solver termina apenas quando o resíduo de complementaridade física não relaxado ($r_{phys} = \max |a_k b_k|$) cai abaixo de uma tolerância especificada, garantindo que a trajetória final satisfaça o modelo de contato físico, não apenas a formulação relaxada.

4. Escalonamento e Globalização

O método aplica escalonamento de coordenadas diagonal consistente com o horizonte, derivado das magnitudes das derivadas por estágio. A globalização é alcançada via uma busca de linha com filtro que equilibra a redução do objetivo e a violação de restrições, com uma regra de fração para a fronteira para manter as folgas positivas.

Principais Contribuições

1. Formulação Unificada: Uma formulação por estágio que converte pares de complementaridade marcados em linhas de produto elásticas acopladas à barreira dentro de uma estrutura de dinâmica, custo e desigualdade não linear.
2. Método Newton Primal–Dual Consciente de MPCC: Um método que preserva a estrutura KKT perturbada acoplada enquanto utiliza eliminação local de folga/dual e recursão de Riccati. Inclui pré-condicionamento de escala de trajetória e uma política de recuperação de múltiplas fases.
3. Avaliação Reproduzível: Um conjunto experimental abrangente comparando o ContactIPM contra CRISP, IMPACT e uma linha de base acados estruturada em OCP (com complementaridade exata), incluindo 50 execções (rollouts) em malha fechada sob várias perturbações.

Resultados Experimentais

Desempenho vs. CRISP

• Velocidade: Em quatro casos de teste fixos (Cartpole, Push Box, Transport, Push T), o ContactIPM é de 2,17× a 8,87× mais rápido que o CRISP em 20 repetições de tempo pareadas.
• Robustez: O ContactIPM iguala o CRISP em Cartpole e Transport, mas tem sucesso em 5 casos adicionais de Push Box (24/25 vs. 19/25). No conjunto Push T, o ContactIPM tem sucesso em todos os 50 casos, enquanto o CRISP tem sucesso em apenas 27/50 quando a complementaridade física estrita é imposta.

Desempenho vs. IMPACT

• Velocidade: O ContactIPM é 2,96× mais rápido no Push T e 4,91× mais rápido no Cart Transport. No entanto, o IMPACT é 4,46× mais rápido no Push Box.
• Robustez: O ContactIPM tem sucesso em 150/150 casos em três conjuntos de 50 casos (Push Box, Push T, Cart Transport), comparado aos 147/150 do IMPACT.

Comparação com Linha de Base Estruturada em OCP (acados)

• Quando o acados é configurado com complementaridade exata (sem relaxação elástica), ele frequentemente falha em gerar contato útil. Por exemplo, no benchmark Push Box, o acados passa nos testes de viabilidade física em todos os 25 casos, mas 0/25 atingem o objetivo terminal, muitas vezes terminando em uma trajetória de contato zero.
• Em 165 casos ricos em contato (CRISP Transport, CRISP Push T, IMPACT Push T e IMPACT Cart Transport), o acados produziu apenas 1 trajetória aceita, enquanto o ContactIPM teve sucesso em 158. Isso demonstra que explorar a estrutura de OCP sozinha é insuficiente para superar a degenerescência de MPCC.

Validação em Malha Fechada

• Em 50 execuções de malha fechada de Push Box envolvendo erro de modelo, ruído e resets de estado, o ContactIPM teve sucesso em todos os 50 casos.
• Latência: O tempo de solução mediana foi de 2,08 ms, com 99,37% das soluções cumprindo o prazo de controle de 100 ms.
• Viabilidade: O feedback de malha fechada manteu a viabilidade primal e a complementaridade física, com os produtos de complementaridade física máxima permanecendo abaixo de $1,01 \times 10^{-6}$.

Significância e Alegações

O artigo alega que a robustez de MPCC, a consistência primal–dual e a estrutura de controle ótimo podem coexistir em um único solver. O ContactIPM demonstra que a separação do tratamento de complementaridade do passo de Newton primal–dual (como visto no CRISP e IMPACT) não é um custo inevitável da complementaridade de contato. Ao combinar uma relaxação interior elástica com eliminação por estágio e recursão de Riccati, o solver alcança tanto a robustez numérica necessária para problemas de contato implícito quanto a eficiência computacional do controle ótimo que explora a estrutura. Os resultados sugerem que o tratamento especializado de MPCC é necessário mesmo quando se utilizam técnicas avançadas de álgebra linear de OCP.