High Accuracy Numerical Optimal Control for Rigid Bodies with Patch Contacts through Equivalent Contact Points -- Extended Version

This paper extends the Finite Elements with Switch Detection and Jumps (FESD-J) [1] method to problems of rigid body dynamics involving patch contacts. The FESD-J method is a high accuracy discretization scheme suitable for use in direct optimal control of nonsmooth mechanical systems. It detects dynamic switches exactly in time and, thereby, maintains the integration order of the underlying Runge- Kutta (RK) method. This is in contrast to commonly used time-stepping methods which only achieve first-order accuracy. Considering rigid bodies with possible patch contacts results in nondifferentiable signed distance functions (SDF), which introduces additional nonsmoothness into the dynamical system. In this work, we utilize so-called equivalent contact points (ECP), which parameterize force and impulse distributions on contact patches by evaluation at single points. We embed a nondifferentiable SDF into a complementarity Lagrangian system (CLS) and show that the determined ECP are well-defined. We then extend the FESD-J discretization to the considered CLS such that its integration accuracy is maintained. The functionality of the method is illustrated for both a simulation and an optimal control example.