LieIPM: Lie Group Interior Point Method for Direct Trajectory Optimization of Rigid Bodies

2026-06-09

AI Summary

This work addresses the singularities and ill-conditioning arising in rigid-body trajectory optimization when Euclidean-space approaches neglect the underlying manifold structure. To overcome these issues, we propose a structure-aware constrained optimization framework formulated directly on matrix Lie groups. Built upon a second-order rigid-body dynamics model, our method uniquely embeds an interior-point algorithm into the Lie group manifold, integrating a line-search strategy with a Lie group variational integrator to preserve rotational topology while avoiding singularities. By exploiting group symmetries, we derive closed-form intrinsic derivatives and employ intrinsic Newton-type updates for efficient solution computation. Experimental results demonstrate that the proposed framework significantly outperforms both general-purpose solvers and existing structure-aware methods in terms of convergence speed and robustness.

Abstract

Designing dynamically feasible trajectories for rigid bodies is a fundamental problem in robotics. While direct methods are widely used, the existing constrained optimizers typically operate in Euclidean space and ignore the manifold structure of rigid body motions. This mismatch may introduce singularities or lead to poorly conditioned optimization problems. To bridge this gap, we develop a structure-aware framework for constrained trajectory optimization directly on matrix Lie groups. Our approach is based on the second-order rigid body models utilizing Lie group structures, which enables efficient Newton-type updates while preserving the underlying geometry. Building on this model, we propose a line-search Lie Group Interior Point Method (LieIPM) to handle constraints on the manifolds. We instantiate the framework for rigid body motion planning using Lie group variational integrators and derive closed-form intrinsic derivatives that exploit group symmetries. The LieIPM preserves the topology of rotation motions by construction and avoids singularities. Numerical results demonstrate superior robustness and faster convergence compared to general-purpose solvers and structure-exploiting optimal control methods.

Problem

• trajectory optimization
• rigid body
• Lie group
• manifold structure
• singularities

Innovation

• Lie group optimization
• Interior Point Method
• Trajectory optimization
• Rigid body dynamics
• Geometric numerical integration