DMOC-based robot trajectory optimization with analytical first-order information

Discrete mechanics and optimal control (DMOC) is a numerical optimal control framework capable of solving robot trajectory optimization problems. This framework has advantages over other direct collocation and multiple-shooting schemes. In particular, it works with a reduced number of decision variables due to the use of the forced discrete Euler–Lagrange (DEL) equation. Also, the transcription mechanism inherits the numerical benefits of variational integrators (i.e., momentum and energy conservation over a long time horizon with large time steps). We extend the benefits of DMOC to solve trajectory optimization problems for highly articulated robotic systems. We provide analytical evaluations of the forced DEL equation and its partial differentiation with respect to decision variables. The Lie group formulation of rigid-body motion and the use of multilinear algebra allow us to efficiently handle sparse tensor computations. The arithmetic complexity of the proposed strategy is analyzed, and it is validated by solving humanoid motion problems.