Proximal and Sparse Resolution of Constrained Dynamic Equations

Abstract

Control of robots with kinematic constraints like loop-closure constraints or interactions with the environment requires solving the underlying constrained dynamics equations of motion. Several approaches have been proposed so far in the literature to solve these constrained optimization problems, for instance by either taking advantage in part of the sparsity of the kinematic tree or by considering an explicit formulation of the constraints in the problem resolution. Yet, not all the constraints allow an explicit formulation and in general, approaches of the state of the art suffer from singularity issues, especially in the context of redundant or singular constraints. In this paper, we propose a unified approach to solve forward dynamics equations involving constraints in an efficient, generic and robust manner. To this aim, we first (i) propose a proximal formulation of the constrained dynamics which converges to an optimal solution in the least-square sense even in the presence of singularities. Based on this proximal formulation, we introduce (ii) a sparse Cholesky factorization of the underlying Karush–Kuhn–Tucker matrix related to the constrained dynamics, which exploits at best the sparsity of the kinematic structure of the robot. We also show (iii) that it is possible to extract from this factorization the Cholesky decomposition associated to the so-called Operational Space Inertia Matrix, inherent to task-based control frameworks or physic simulations. These new formulation and factorization, implemented within the Pinocchio library, are benchmark on various robotic platforms, ranging from classic robotic arms or quadrupeds to humanoid robots with closed kinematic chains, and show how they significantly outperform alternative solutions of the state of the art by a factor 2 or more.