Exploiting sparsity in robot trajectory optimization with direct collocation and geometric algorithms

This paper presents a robot trajectory optimization formulation that builds upon numerical optimal control and Lie group methods. In particular, the inherent sparsity of direct collocation is carefully analyzed to dramatically reduce the number of floating-point operations to get first-order information of the problem. We describe how sparsity exploitation is employed with both numerical and analytical differentiation. Furthermore, the use of geometric algorithms based on Lie groups and their associated Lie algebras allow to analytically evaluate the state equations and their derivatives with efficient recursive algorithms. We demonstrate the scalability of the proposed formulation with three different articulated robots, such as a finger, a mobile manipulator and a humanoid composed of five, eight and more than twenty degrees of freedom, respectively. The performance of our implementation in C++ is also validated and compared against a state-of-the-art general purpose numerical optimal control solver.