A Numerically Robust LCP Solver for Simulating Articulated Rigid Bodies in Contact

This paper presents a numerically robust algorithm for solving linear complementarity problems (LCPs), and applies it to simulation of frictional contacts of articulated rigid bodies each modeled as a general polygonal object. We first point out two problems of the popular pivot-based LCP solver called Lemke Algorithm and its extension with lexicographic ordering, due to numerical errors especially for ill-conditioned LCPs. Our new algorithm solves these problems by storing all pivot candidates and searching for a sequence of pivots that leads to a solution. An LCP-based contact dynamics formulation is combined with a forward dynamics algorithm for articulated rigid bodies to perform the whole simulation using a dynamic programming approach. Simulation examples using a humanoid robot show that the Lemke Algorithm (with or without lexicographic ordering) cannot solve complex contact problems, while our algorithm can successfully simulate such situations. We also demonstrate that the simulation results are qualitatively similar to those of hardware experiments.