Optimal motion planning of redundant manipulators with controlled task infeasibility

In optimal motion planning and control, the complex time-varying nature of redundant robots, environments, and task requirements causes complex domains and conflicting constraints. Since predicting or recovering infeasibility is not always possible, infeasibilities occur frequently and are not completely avoidable. We introduce a constrained nonlinear programming framework of controlled (as opposed to recovered) infeasibility for physically valid solutions while preserving the original problem and variable space. The constraint prioritization hierarchy includes a comprehensive classification of physical consistency, design requirements, and tasks. Priority weight functions having features of normalization and prioritization are incorporated into a sequential quadratic programming (SQP) algorithm to ensure generality and strict satisfaction of high-priority constraints, while lower-priority constraint violations are minimized. These are embedded in SQP through its merit function and composite cost function, in which general nonlinear functions including unilateral, time-dependent, and nonholonomic, can be incorporated in a unified approach. Also, the avoidance of the discontinuity problem with unilateral constraints is due to the time-dependent constraints strategy, which actively adapts to varying states. Numerical examples using multibody dynamic models of a redundant manipulator demonstrate these advantages.