Optimal redundancy resolution with task scaling under hard bounds in the robot joint space

For robots that are redundant with respect to a given task, we present an optimal differential kinematic inversion method in the presence of hard bounds on joint range, joint velocity, and joint acceleration. These hard bounds specify the robot motion capabilities that cannot be exceeded at any time. On the other hand, scaling of the desired task trajectory is allowed whenever the robot capabilities are insufficient to execute the original task. For a problem formulated in this way, we have recently presented the Saturation in the Null Space (SNS) algorithm that produces an efficient solution, based on Jacobian pseudoinversion and recovery in the null space of the saturation effects of a reduced number of joint velocity commands. To investigate the optimality properties of the SNS algorithm, we recast the problem as a constrained quadratic programming (QP) problem, in which the joint velocity norm as well as the task scaling are to be minimized. Its solution leads to a variant of the original algorithm, the Optimal Saturation in the Null Space (Opt-SNS). The Opt-SNS guarantees an optimal solution also when the basic SNS fails to do so and improves the numerical performance over the state-of-the-art QP solver. The possible existence of discontinuous solutions for the formulated problem is avoided by the introduction of a task scaling margin. The extension to the multi-task case is also presented. Simulation results for the 7R lightweight KUKA LWR IV robot illustrate the properties and computational efficiency of the new algorithm.