A Simple Formulation for Fast Prioritized Optimal Control of Robots using Weighted Exact Penalty Functions

Prioritization of tasks is a common approach to resolve conflicts in instantaneous control of redundant robots. However, the idea of prioritization has not yet been satisfactorily extended to model predictive control (MPC) to allow for real-time robot control. The standard sequential approach for prioritization is unsuitable because of the computational burden involved in solving a nonlinear problem (NLP) at every priority level. We introduce an alternate promising approach of using weighted exact penalties for the MPC stage costs, where a correctly tuned set of weights can introduce strict prioritization. We prove the existence of a set of equivalent weights that provides the same solution as the sequential approach for a local convex approximation of the original NLP and use this insight to design an algorithm to adaptively tune the weights. The weighted method is validated on a dual arm robot task in simulations and also implemented on a physical robot. We report computational times that are fast enough for prioritized MPC of robot manipulators for the first time, to the best of our knowledge.