Fast redundancy resolution for high-dimensional robots executing prioritized tasks under hard bounds in the joint space

A kinematically redundant robot with limited motion capabilities, expressed by inequality constraints of the box type on joint variables and commands, needs to perform a set of tasks, expressed by linear equality constraints on robot commands, possibly organized with priorities. Robot motion capabilities cannot be exceeded at any time, and the resulting constraints are to be considered as hard bounds. Instead, robot tasks can be relaxed by velocity scaling if no feasible solution exists. To address this redundancy resolution problem, we developed a method in which joint space commands are successively saturated and their effect compensated in the null space of a suitable task Jacobian (SNS, Saturation in the Null Space). Computationally efficient versions of the basic and optimal SNS algorithms are proposed here, based on a task augmentation reformulation, a QR factorization of the main matrices involved, and a so-called warm start procedure. The obtained performance allows to control in real time robots with high-dimensional configuration spaces executing a large number of prioritized tasks, and with an associated high number of hard bounds that saturate during motion.