Smoothed Distance Functions for Direct Optimal Control of Contact-Rich Systems

This paper discusses a smoothing method for a signed distance function (SDF) between polytopes that is applicable in direct optimal control methods for contact-rich systems. It is well known that the Euclidean or similar SDF are nondifferentiable with respect to degrees of freedom for rigid bodies that do not have sufficiently smooth surfaces. We consider a growth distance for polytopes modeled as a parametric linear program and give a thorough proof that smoothing this SDF by solving perturbed optimality conditions results in well-defined functions. In previous works, exact optimality conditions of the SDF were embedded directly into an optimal control problem (OCP). In contrast, smoothing the SDF allows for external evaluation and thereby smaller instances of OCP, distributing the computational effort more equally among function evaluations and the load on numerical solvers. We discuss an implementation of the smooth SDF in the CasADi toolbox and compare the performance to the equivalent direct embedding into the OCP. A planar peg-in-hole task is considered for which an ensemble of trajectories that share the same control input is optimized simultaneously.