LieIPM: Lie Group Interior Point Method for Direct Trajectory Optimization of Rigid Bodies
Authors: Sangli Teng, Ruiqi Zhang, Tzu-Yuan Lin, William A Clark, Mark Mueller, Ram Vasudevan, Maani Ghaffari, Koushil Sreenath
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.
Optimal trajectory generation for rigid-body motions on Lie groups can be formulated as a variational problem that minimizes energy functionals defined by Riemannian metrics. While closed-form solutions exist for special cases such as product metrics and rest-to-rest boundary conditions, solving the general problem with arbitrary boundary states and coupled rotational-translational metrics often requires computationally expensive numerical boundary value solvers. These limitations restrict the use of geometrically consistent trajectory generation in real-time robotic planning and control. This paper presents a learning-based framework for approximating higher-order smooth trajectories on SE(3) under general left-invariant Riemannian metrics. The method parameterizes body-twist trajectories using high-order polynomials and relies on a neural network to learn a subset of the polynomial coefficients and the trajectory duration. The remaining coefficients are analytically determined to enforce the boundary conditions. The training of the network is guided by losses derived from Euler-Lagrange optimality conditions, metric-weighted smoothness objectives, and feasibility constraints. The metric-conditioned framework enables generalization across diverse metric structures and motion conditions. Extensive numerical experiments demonstrate that the proposed approach generates smooth trajectories that closely approximate solutions from numerical optimization while achieving millisecond-level inference times. We demonstrate two practical applications of the proposed framework: real-time generation of diverse motion primitives with waypoint traversal, and refinement for quadrotor flight under dynamic conditions. These results suggest that learning-based motions with geometric structure can provide an efficient alternative to conventional optimization-based methods for trajectory generation on SE(3).
Dynamically feasible trajectory optimization for rigid-body systems is naturally formulated on the special Euclidean group SE(3) but is challenging when dynamics are available only as black-box computations without derivatives. This paper formulates the Trajectory Bundle Method (TBM) for motion planning implicitly on SE(3). Bundles are constructed in the Lie algebra and propagated through nonlinear rigid-body dynamics using exponential and logarithmic maps, enabling derivative-free planning of non-Euclidean trajectories. We show that Euclidean TBM interpolation error is bounded quadratically by bundle diameter and extend this result to SE(3), where the bound additionally depends on a local Lipschitz constant of the Log map. Numerical experiments corroborate these bounds. Finally, we demonstrate SE(3) TBM by optimizing an acrobatic, collision-free fixed-wing maneuver through a rotated aperture without explicit models or derivatives of the vehicle dynamics, aerodynamics, or collision model.
Matthew D. Osburn, Cameron K. Peterson, John L. Salmon
The dynamics of a holonomically constrained rigid body can be modeled by Newton-Euler equations subjected to geometric constraints. This is frequently formulated as a differential-algebraic equation (DAE) system of index 1. Inmultibody system (MBS) dynamics it is common (1) to numerically solve this system by means of integration schemes for ordinary differential equations, and (2) to treat the rigid body motion on the direct product Lie group SO (3)R3, although rigid body motions form the semidirect product Lie group SE (3). It is has been observed that the constraint satisfaction depends on which Lie group is used as configuration space (c-space). In this paper the problem is considered from a geometric perspective. It is shown that the constraints are exactly satisfied by a numerical integration scheme if they define a subgroup of the c-space. The subgroups of SE (3) have a significance for modeling mechanical systems, including lower kinematic (Reuleaux) pairs and are implicitly used in MBS modeling. It is concluded that SE (3) is the appropriate cspace for numerical DAE modeling of a constrained rigid body. This result does not immediately apply to MBS, however.