G. Alcan, F. Abu-Dakka, V. Kyrki
Robotics: Science and Systems Workshop "Frontiers of optimization for robotics"
Publication date: July 2024

Matrix Lie groups are an important class of manifolds commonly used in control and robotics, and the optimization of control policies on these manifolds is a fundamental problem. In this work, we propose a new approach for trajectory optimization on matrix Lie groups using an augmented Lagrangian-based constrained Differential Dynamic Programming (DDP). The method involves lifting the optimization problem to the Lie algebra in the backward pass and retracting back to the manifold in the forward pass. In contrast to previous approaches which only addressed constraint handling for specific classes of matrix Lie groups, the proposed method provides a general approach for nonlinear constraint handling for generic matrix Lie groups. We evaluate the effectiveness of the proposed DDP method in handling constraints in a simple mechanical system characterized by rigid body dynamics in SE(3), assessing computational efficiency compared to existing optimization solvers, and optimizing trajectory performance in a realistic quadrotor scenario.