SQP-Based Mobile Manipulator Motion Planning With Controlled Infeasibility for Physically Valid Task Failure

Since anticipating or recovering infeasibility in optimal motion planning is not always possible, infeasibilities occur frequently and are not completely avoidable. We introduce an enhanced sequential quadratic programming (SQP) based framework of controlled infeasibility for physically valid solutions, based on our previous study. A priority weight function is incorporated into an SQP algorithm combined with constraints and objective function normalization to ensure strict satisfaction of high-priority constraints. These are embedded in the SQP algorithm through its merit function and composite cost function, in which general nonlinear functions can be incorporated in a unified approach. Several simple mobile manipulator examples demonstrate the advantages of the proposed method.