Efficient Algorithms for Whole-Body and Second-Order Robot Trajectory Optimization
摘要
This chapter develops an approach for achieving fast whole-bodyOptimal control optimalOptimal control incorporating exact Lagrangian HessianHessian information. To this end, we concisely derive the three key state-of-the-art algorithmic ingredients of our approach, namely, (1) a linear solverSolver exploiting theOptimal control optimalOptimal control problem (OCP) structure, (2) a constrained forward dynamics algorithmAlgorithm, and (3) efficientEfficient differentiation of the OCP up to second order. The OCP’s linear solverSolver and the constrained dynamicsConstrained dynamics algorithm are derived in a unified manner since they are both based onRiccati recursion RiccatiRiccati recursion. For OCP differentiation, we combine the adjointAdjoint method and automatic differentiation (AD) to obtain significant efficiency improvements compared to vanilla AD. Our approach’s potential is demonstrated on a trajectoryTrajectory optimizationOptimization problem of a seven DoF Kuka LBR iiwa robot arm with full-order dynamics and a horizonHorizon length of 50, where we achieve 0.5 ms per OCP iteration with exact Lagrangian HessianHessian compared to 0.3 ms for Gauss–Newton approximation. Fewer OCP iterations for the exact HessianHessian approach result in overall faster computation time, challenging the conventional wisdom that exact HessianHessian methods are prohibitively expensive for fast OCP solvers.