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 novel approach
for trajectory optimization on matrix Lie groups using an augmented
Lagrangian-based constrained discrete Differential Dynamic Programming. 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
also demonstrate the effectiveness of the method in handling external
disturbances through its application as a Lie-algebraic feedback control policy
on SE(3). Experiments show that the approach is able to effectively handle
configuration, velocity and input constraints and maintain stability in the
presence of external disturbances.Comment: 10 pages, 7 figure