Contact status optimization of multibody dynamic systems using dual variable transformation

Generating the motion of redundant systems under general constraints within an optimization framework is a problem not yet solved, as there is, so far, a lack of completely predictive methods that concurrently solve for the optimal trajectory and the contact status induced by the given constraints. A novel approach for optimal motion planning of multibody systems with contacts is developed, based on a Sequential Quadratic Programming (SQP) algorithm for Nonlinear Programming (NLP). The objective is to detect and optimize the contact status and the relative contact force within the optimization sequential problem, while simultaneously optimizing a trajectory. The novelty is to seek for the contact information within the iterative solution of the SQP algorithm and use this information to sequentially update the resulting contact force in the system's dynamic model. This is possible by looking at the analytical relationship between the dual variables resulting from the constrained NLP and the Lagrange multipliers that represent the contact forces in the classical formulation of constrained dynamic systems. This approach will result in a fully predictive algorithm that doesn't require any a priori knowledge on the contact status (e.g., time of contact, point of contact, etc.) or contact force magnitude. A preliminary formulation is presented, as well as numerical experiments on simple planar manipulators, as demonstration of concepts.