1 Introduction

Robots with high Degrees of Freedom (DoFs) such as mobile manipulators and humanoids are designed for human-centered environments. To control these robots, whole-body control frameworks [13, 15] have been used to perform various prioritized tasks to consider motion distribution with kinematic redundancy. However, in practice, depending on the scenarios (e.g., locomotion, manipulation, and loco-manipulation), it is difficult to generate natural whole-body behavior only with whole-body controllers. This is because typical whole-body controllers produce joint motion minimizing the same metric for all prioritized tasks. For example, when the mobile manipulator tracks the desired trajectory by using a whole-body task, it often comes into singular configuration or reaches the boundary of the workspace due to dynamic and kinematic difference of mobile base and manipulator [1, 17]. Also, during locomotion phase of the humanoid, the movement of the upper body by whole-body control may adversely affect walking performance [26].

Fig. 1
figure 1

An example application of the proposed method with individual weighting matrix for each task priority: a box-taping scenario of humanoid

Full size image

In this paper, we propose a novel Weighted Hierarchical Quadratic Programming (WHQP) framework to characterize joint movement for each task. By combining two concepts of HQP [6] and weighted least-squares norm [2], it can handle various inequality and equality tasks with differently weighted joint motion for each priority effectively. Consequently, the proposed controller can generate natural whole-body behavior without additional subtasks which restrict undesirable movements, as shown in Fig. 1.

1.1 Related works

HQP has been actively studied in that it can handle both equality and inequality tasks while ensuring strict priorities of tasks. Kim et al. [12] proposed an HQP-based task transition method that can insert, remove, swap the priorities of the tasks while ensuring continuous control inputs. Tassi et al. [24] extended HQP in order to produce an impedance-like motion under external disturbance by augmenting the variable for Cartesian velocity. To enhance the computational efficiency, Lee et al. [14] utilizes operational space formulation [19] so that the size of the decision variable in QP is reduced when controlling the whole-body of the humanoid.

On the other hand, the weighted least-squares norm which is based on the weighted psuedo-inverse can treat the redundancy of the robots without additional constraints. Dariush et al. [4] penalized the motion of joints by designing a weighting matrix that determines contribution to the main task according to the extent of the proximity to collision in order to avoid self-collision or obstacle. Similarly, Farelo et al. [8] generated optimized joint motion for wheelchair-mounted arm by using weighted pseudo-inverse that considers not only joint limit of arm but also motion limit of wheelchair. Tsuichihara et al. [25] utilized weighted pseudo-inverse that restricts chest motion of humanoid to improve stability in manipulation. Park et al. [21] and Choi et al. [3] combined the task-priority method and the weighted pseudo-inverse method in order to generate the joint motion minimizing the residual error caused by singularity-robust framework. However, since these methods treat the same joint weights for all tasks, they cannot assign individual joint weights for each task priority.

To tackle this issue, Wu et al. [27] recently proposed a two-level prioritized whole-body Cartesian impedance controller with individual weighting matrices. In contrast to aforementioned methods [3, 4, 8, 21, 25] with the same weighted joint distribution for all tasks, the proposed controller used individual weighting matrices to generate different motion patterns for each task. However, it cannot calculate the solution when extending to multiple tasks since the main task and its null-space are only considered. Also, [27] cannot deal with inequality constraints.

1.2 Overview of the Paper

The proposed WHQP can efficiently compute an analytic solution in a weighted least-squares norm manner for prioritized equality and inequality tasks. By reformulating HQP with Complete Orthogonal Decomposition (COD) [6], the WHQP assigns the weighting matrix to each task hierarchy and derives the first-order optimality conditions. Based on these conditions, the active-set method [6, 16] is exploited to handle inequality tasks.

Therefore, the main contributions of this paper are as follows. First, our algorithm calculates the optimal solution given the joint weights for each priority level. Thus, the proposed algorithm helps to generate the preferred whole-body motion patterns according to various scenarios such as locomotion, manipulation, and loco-manipulation. Second, unlike previous works [3, 4, 8, 21, 25, 27] with the weighted optimization problem, the proposed method can handle inequality constraints as well as equality constraints. Finally, by virtue of symmetric null-space projection matrices, the proposed solver is computationally efficient. We show that our formulation can control various redundant robots including a nonholonomic mobile manipulator and humanoid in real-time.

The remainder of the paper is organized as follows. In Sect. 2, mathematical problem statements are explained. Section 3 describes how to formulate the WHQP subject to equality tasks and calculate a solution in a weighted least-squares norm sense. Based on the formulation of the WHQP, inequality tasks are handled by using active-set method in Sect. 4. Next, Sects. 5 and 6 describe the experimental validations and discussion of the proposed WHQP, respectively. Finally, the paper is concluded in Sect. 7.

2 Problem statement

This section provides the modification of the weighted pseudo-inverse to handle individual weights of joints for each task hierarchy and its limitations.

2.1 Pseudo-inverse with weighted least-squares norm for each task

Let us assume that there are p prioritized tasks and p weighting matrices for each task of n-DoFs redundant robot:

$$\begin{aligned} \dot{x}_{k} = J_{k}\dot{q}, \quad (k = 1,\cdots ,p) \end{aligned}$$
(1)

where \(\dot{x}_{k} \in \mathbb {R}^{m_{k}}\), \(J_{k} \in \mathbb {R}^{m_{k} \times n}\), and \(q \in \mathbb {R}^{n}\) are the task space velocity, the Jacobian matrix, and the joint position of the robot, respectively. The priorities are indicated by the number of subscripts: the smaller the subscript number, the higher the task priority. Also, the weighting matrix \(W_{k} \in \mathbb {R}^{n \times n}\) is assumed to be symmetric and positive definite.

To execute the first task \(\dot{x}_{1}\) while assigning dominant joints or optimizing a performance criterion through the weighting matrix \(W_{1}\), an optimization problem is formulated as

$$\begin{aligned} \begin{aligned}&\underset{\dot{q}_{1}}{\text {min}}&\frac{1}{2}\dot{q}_{1}^{T}W_{1}\dot{q}_{1}, \\&\text {s. t.}&J_{1}\dot{q}_{1} = \dot{x}_{1}. \end{aligned} \end{aligned}$$
(2)

The optimal solution \(\dot{q}_{1}^{*} \in \mathbb {R}^{n}\) is calculated analytically as

$$\begin{aligned} \dot{q}_{1}^{*} = J_{1}^{W_{1}+}\dot{x}_{1} = W_{1}^{-1}J_{1}^{T}(J_{1}W_{1}^{-1}J_{1}^{T})^{-1}\dot{x}_{1}, \end{aligned}$$
(3)

where \(J_{1}^{W_{1}+} \in \mathbb {R}^{n \times m_{1}}\) is the weighted pseudo-inverse of \(J_{1}\).

By utilizing the weighted pseudo-inverse solution in (3), a solution that executes multiple tasks with differently weighted joint solution for each task priority can be computed. Considering the secondary task \(\dot{x}_{2}\) with the corresponding weighting matrix \(W_{2}\), the total solution \(\dot{q}_{2}^{*}\) for two prioritized tasks would be

$$\begin{aligned} \dot{q}_{2}^{*} = \dot{q}_{1}^{*} + \dot{\tilde{q}}_{2}^{*}, \end{aligned}$$
(4)

where \(\dot{\tilde{q}}_{2}^{*}\) denotes the contribution to the secondary task without modifying the first task. This can be obtained by solving the following optimization problem as

$$\begin{aligned} \begin{aligned}&\underset{\dot{\tilde{q}}_{2}}{\text {min}}&\frac{1}{2}\dot{\tilde{q}}_{2}^{T}W_{2}\dot{\tilde{q}}_{2},\\&\text {s. t.}&J_{1}\dot{\tilde{q}}_{2} = \dot{x}_{1} - J_{1}\dot{q}_{1}^{*} = 0, \\&&J_{2}\dot{\tilde{q}}_{2} = \dot{x}_{2} - J_{2}\dot{q}_{1}^{*}, \end{aligned} \end{aligned}$$
(5)

The solution for (5) is calculated as

$$\begin{aligned} \dot{\tilde{q}}_{2}^{*} = (J_{2}N_{1}^{W_{2}})^{W_{2}+}(\dot{x}_{2} - J_{2}\dot{q}_{1}^{*}). \end{aligned}$$
(6)

where \(N_{1}^{W_{2}} = I - W_{2}^{-1}J_{1}^{T}(J_{1}W_{2}^{-1}J_{1}^{T})^{-1}J_{1} \in \mathbb {R}^{n \times n}\) is the projector onto the null-space of \(J_{1}\) weighted by \(W_{2}\). Note that the projector \(N_{1}^{W_{2}}\) is idempotent, but not symmetric.

Thus, it is straightforward to obtain a general solution for p prioritized tasks:

$$\begin{aligned} \dot{q}_{p}^{*} = \sum _{k=1}^{p} (J_{k}N_{k-1}^{W_{k}})^{W_{k}+}(\dot{x}_{k}-J_{k}\dot{q}_{k-1}^{*}), \end{aligned}$$
(7)

where \(N_{0}^{W_{1}}=I\), \(\dot{q}_{0}^{*}=0\), and \(N_{k-1}^{W_{k}}\) is the projector onto the null-space of the augmented Jacobian \(\underline{{J}}_{k-1} = \begin{bmatrix} J_{1}^{T}, J_{2}^{T},&\ldots&, J_{k-1}^{T} \end{bmatrix}^T \in \mathbb {R}^{\sum _{i=1}^{k-1}m_{i} \times n}\) weighted by \(W_{k}\). At this point, all tasks are assumed to be full rank. The projector \(N_{k-1}^{W_{k}}\) can be computed in two ways as

$$\begin{aligned}&N_{k-1}^{W_{k}} = N_{[1]}^{W_{k}}N_{[2]}^{W_{k}}\cdots N_{[k-1]}^{W_{k}} = \prod _{j=1}^{k-1} N_{[j]}^{W_{k}}, \end{aligned}$$
(8a)
$$\begin{aligned}&N_{k-1}^{W_{k}} = I - \underline{J}_{k-1}^{W_{k}+}\underline{J}_{k-1}, \end{aligned}$$
(8b)

where \(N_{[j]}^{W_{k}} = I - (J_{j}\prod _{i=0}^{j-1}N_{[i]}^{W_{k}})^{W_{k}+}(J_{j}\prod _{i=0}^{j-1}N_{[i]}^{W_{k}})\), \(N_{[0]}^{W_{k}}=I\), and \(N_{[1]}^{W_{k}}=I-W_{k}^{-1}J_{1}^{T}(J_{1}W_{k}^{-1}J_{1}^{T})^{-1}J_{1}\).

2.2 Problem statement

When computing the null-space projection matrix in (8), the matrix is asymmetric and its size remains constant. Owing to these properties, the computational cost increases exponentially when propagating to multiple tasks. To improve computational efficiency, [5] proposed a scheme to accelerate the computation by decomposing a symmetric null-space projection matrix. Therefore, our goal is to efficiently compute weighted least-squares norm solution by transforming the asymmetric null-space projection matrix in (8) into a symmetric matrix. Furthermore, we extend the pseudo-inverse with weighted least-squares norm for each task priority of Sect. 2.1 to deal with inequality as well as equality tasks.

3 WHQP with equality tasks

This section formulates the WHQP that minimizes the violation of the equality task in a weighted least-squares norm manner. When formulating the WHQP, Jacobian matrix and joint velocity are transformed into a weighted Jacobian matrix and joint velocity. This makes the null-space projection matrix symmetric, which enables the proposed solution to be computed more efficiently than the traditional method using weighted pseudo-inverse.

For the first task \(\dot{x}_{1}\), the weighted Jacobian matrix \(J_{1,W_{1}}\) and weighted joint velocity \(\dot{q}_{W_{1}}\) are defined as

$$\begin{aligned} \begin{aligned} J_{1,W_{1}}&= J_{1}W_{1}^{-\frac{1}{2}}, \\ \dot{q}_{W_{1}}&= W_{1}^{\frac{1}{2}}\dot{q}. \end{aligned} \end{aligned}$$
(9)

Then, the WHQP is formulated as

(10)

where \(s_{1} \in \mathbb {R}^{m_{1}}\) is the slack variable that alleviates the task \(\dot{x}_{1}\). Note that the variable to optimize is not \(\dot{q}\), but \(\dot{q}_{W_{1}}\).

To solve this problem, the Lagrangian \(\mathcal {L}_{1}\) is computed as

$$\begin{aligned} \mathcal {L}_{1} = \frac{1}{2}s_{1}^{T}s_{1} + \lambda _{1}^{T}(J_{1,W_{1}}\dot{q}_{W_{1}} - \dot{x}_{1} - s_{1}), \end{aligned}$$
(11)

where \(\lambda _{1} \in \mathbb {R}^{m_{1}}\) is the Lagrange multiplier. From the first-order optimality conditions, the Lagrangian differentiated by \( \lambda _{1}, s_{1},\) and \(\dot{q}_{W_{1}}\) should be zero:

$$\begin{aligned} \begin{aligned} \frac{\partial \mathcal {L}_{1}}{\partial \lambda _{1}}&= J_{1,W_{1}}\dot{q}_{W_{1}} - \dot{x}_{1} - s_{1} = 0, \\ \frac{\partial \mathcal {L}_{1}}{\partial s_{1}}&= s_{1} - \lambda _{1} = 0,\\ \frac{\partial \mathcal {L}_{1}}{\partial \dot{q}_{W_{1}}}&= J_{1,W_{1}}^{T}\lambda _{1} = 0. \end{aligned} \end{aligned}$$
(12)

To obtain the weighted solution \(\dot{q}_{W_{1}}^{*}\), the pseudo-inverse of the weighted Jacobian matrix \(J_{1,W_{1}}\) is computed using COD [6, 9] which is cheaper than computing the singular value decomposition as

$$\begin{aligned} \begin{aligned} J_{1,W_{1}}&= \begin{bmatrix}V_{1,W_1}&U_{1,W_1} \end{bmatrix}\begin{bmatrix}0 &{} 0 \\ L_{1,W_1} &{} 0 \end{bmatrix} \begin{bmatrix}Y_{1,W_1}&Z_{1,W_1} \end{bmatrix}^{T}\\&=U_{1,W_1}L_{1,W_1}Y_{1,W_1}^{T}, \end{aligned} \end{aligned}$$
(13)

where \(U_{1,W_1} \in \mathbb {R}^{m_{1} \times r_{1}}\) and \(V_{1,W_1} \in \mathbb {R}^{m_{1} \times (m_{1}-r_{1})}\) are the orthonormal bases for the column space of \(J_{1,W_1}\). \(r_{1}\) is the rank of \(J_{1,W_1}\). \(Y_{1,W_1} \in \mathbb {R}^{n \times r_{1}}\) and \(Z_{1,W_1} \in \mathbb {R}^{n \times (n-r_{1})}\) are the orthonormal bases for the row space of \(J_{1,W_1}\). \(L_{1,W_1} \in \mathbb {R}^{r_{1} \times r_{1}}\) is a lower triangular matrix. With this decomposition, the weighted solution and slack variable satisfying (12) are obtained as

$$\begin{aligned}&\dot{q}_{W_{1}}^{*} = J_{1,W_1}^{+}\dot{x}_{1} = Y_{1,W_1}L_{1,W_1}^{-1}U_{1,W_1}^{T}\dot{x}_{1}, \end{aligned}$$
(14)
$$\begin{aligned}&s_{1}^{*} = U_{1,W_1}U_{1,W_1}^{T}\dot{x}_{1}-\dot{x}_{1}=-V_{1,W_1}V_{1,W_1}^{T}\dot{x}_{1}, \end{aligned}$$
(15)

where \(J_{1,W_1}^{+}\) denotes the pseudo-inverse of \(J_{1,W_1}\). Based on (13) and (15), the last optimality condition of (12) is satisfied. In the end, the complete solution \(\dot{q}_{1}^{*}\) for the first task is obtained by transforming the weighted solution to the original joint space as

$$\begin{aligned} \dot{q}_{1}^{*} = W_{1}^{-\frac{1}{2}}\dot{q}_{W_{1}}^{*}. \end{aligned}$$
(16)

Similarly, the WHQP for the secondary task \(\dot{x}_{2}\) is formulated as

(17)

Similar to (6), the weighted solution \(\dot{q}_{W_{2}}^{*}\) lies in the null-space of \(J_{1,W_2}\) and at the same time executes the residual task after subtracting the effect of \(\dot{q}_{1}^{*}\) on the secondary task space. It is obtained as

$$\begin{aligned} \dot{q}_{W_{2}}^{*} = (J_{2,W_2}N_{1,W_2})^{+}(\dot{x}_{2} - J_{2}\dot{q}_{1}^{*}), \end{aligned}$$
(18)

where \(N_{1,W_2} = I - {J}_{1,W_2}^{+}{J}_{1,W_2}\) is the null-space projection matrix of \({J}_{1,W_2}\).

Unlike (6), \(N_{1,W_2}\) is both idempotent and symmetric. Thus, \(N_{1,W_2}\) can be represented as

$$\begin{aligned} N_{1,W_2} = Z_{1,W_2}Z_{1,W_2}^{T}, \end{aligned}$$
(19)

where \(Z_{1,W_2} \in \mathbb {R}^{n \times (n-r_{1})}\) is the null-space bases of \(J_{1,W_2}\) and obtained by decomposing \(J_{1,W_2}\). Then, the solution in (18) can be represented in a more efficient form [5] as

$$\begin{aligned} \begin{aligned} \dot{q}_{W_{2}}^{*} \!&\! = \! Z_{1,W_2}(J_{2,W_2}Z_{1,W_2})^{+}(\dot{x}_{2} - J_{2}\dot{q}_{1}^{*}) \\ \!&\! = \! Y_{2,W_2}L_{2,W_2}^{-1}U_{2,W_2}^{T}(\dot{x}_{2} - J_{2}\dot{q}_{1}^{*}), \end{aligned} \end{aligned}$$
(20)

where \(Y_{2,W_2} = Z_{1,W_2}\tilde{Y}_{2,W_2} \in \mathbb {R}^{n \times r_{2}}\). \(\tilde{Y}_{2,W_2} \in \mathbb {R}^{(n-r_{1}) \times r_{2}}\), \(L_{2,W_2} \in \mathbb {R}^{r_{2} \times r_{2}}\), and \(U_{2,W_2} \in \mathbb {R}^{m_{2} \times r_{2}}\) are obtained by decomposing \(J_{2,W_2}Z_{1,W_2} \in \mathbb {R}^{m_{2} \times (n-r_{1})}\) as

$$\begin{aligned} J_{2,W_2}Z_{1,W_2} \! = \! \begin{bmatrix}V_{2,W_2}&U_{2,W_2} \end{bmatrix} \! \begin{bmatrix}0 \! &{} \! 0 \\ L_{2,W_2} \! &{} \! 0 \end{bmatrix} \! \begin{bmatrix}\tilde{Y}_{2,W_2} \!&\! \tilde{Z}_{2,W_2} \end{bmatrix}^{\!T\!} \!. \end{aligned}$$
(21)

Using (20), the optimal slack variable \(s_{2}^{*}\) is computed as

$$\begin{aligned} \begin{aligned} s_{2}^{*}&= J_{2,W_2}\dot{q}_{W_{2}}^{*} - \dot{x}_{2} + J_{2}\dot{q}_{1}^{*}, \\&= U_{2,W_2}U_{2,W_2}^{T}(\dot{x}_{2} - J_{2}\dot{q}_{1}^{*}) - \dot{x}_{2} + J_{2}\dot{q}_{1}^{*}\\&= V_{2,W_2}V_{2,W_2}^{T}(J_{2}\dot{q}_{1}^{*} - \dot{x}_{2}). \end{aligned} \end{aligned}$$
(22)

Therefore, the total solution for two prioritized tasks \(\dot{x}_{1}\), \(\dot{x}_{2}\) is

$$\begin{aligned} \dot{q}_{2}^{*} = W_{1}^{-\frac{1}{2}}\dot{q}_{W_{1}}^{*} + W_{2}^{-\frac{1}{2}}\dot{q}_{W_{2}}^{*}. \end{aligned}$$
(23)

More generally, WHQP for the kth task \(\dot{x}_{k}\) is formulated as

(24)

where \(\underline{J}_{k-1,W_k} = \begin{bmatrix} J_{1,W_k}^{T}, J_{2,W_k}^{T}, \cdots , J_{k-1,W_k}^{T} \end{bmatrix}^T\) is the augmented Jacobian matrix weighted by \(W_k^{-\frac{1}{2}}\).

To solve (24), the Lagrangian \(\mathcal {L}_{k}\) is constructed as

$$\begin{aligned} \begin{aligned} \mathcal {L}_{k}&= \frac{1}{2}s_{k}^{T}s_{k} + \lambda _{k}^{T}(J_{k,W_k}\dot{q}_{W_{k}} - \dot{x}_{k} + J_{k}\dot{q}_{k-1}^{*} - s_{k}) \\&+ \underline{\lambda }_{k-1}^{T}(\underline{J}_{k-1,W_k}\dot{q}_{W_{k}}). \end{aligned} \end{aligned}$$
(25)

Then, the optimality conditions for the kth task are directly obtained as

$$\begin{aligned} \begin{aligned} \frac{\partial \mathcal {L}_{k}}{\partial \lambda _{k}}&= J_{k,W_k}\dot{q}_{W_{k}} - \dot{x}_{k} + J_{k}\dot{q}_{k-1}^{*} - s_{k} = 0, \\ \frac{\partial \mathcal {L}_{k}}{\partial \underline{\lambda }_{k-1}}&= \underline{J}_{k-1,W_k}\dot{q}_{W_{k}} = 0,\\ \frac{\partial \mathcal {L}_{k}}{\partial s_{k}}&= s_{k} - \lambda _{k} = 0, \\ \frac{\partial \mathcal {L}_{k}}{\partial \dot{q}_{W_{k}}}&= J_{k,W_k}^{T}\lambda _{k} + \underline{J}_{k-1,W_k}^{T}\underline{\lambda }_{k-1} = 0. \\ \end{aligned} \end{aligned}$$
(26)

To calculate the solution, the Jacobian matrices \(J_{k,W_k}\), \(\underline{J}_{k-1,W_k}\) are decomposed as in [6]:

$$\begin{aligned} \begin{aligned} J_{k,W_k} \!&\! = \! \begin{bmatrix}V_{k,W_k} \!&\! U_{k,W_k} \end{bmatrix} \!\begin{bmatrix}N_{k,W_k} \! &{} \! 0 \\ M_{k,W_k} \! &{} \! L_{k,W_k} \end{bmatrix} \! \begin{bmatrix}\underline{Y}_{k-1,W_k} \!&\! Y_{k,W_k} \end{bmatrix}^{\!T\!} \\ \!&\!= \! E_{k,W_k}H_{k,W_k}\underline{Y}_{k,W_k}^{\!T\!}, \end{aligned} \end{aligned}$$
(27)

where

$$\begin{aligned} \begin{aligned} N_{k,W_k}&=V_{k,W_k}^{T}J_{k,W_k}\underline{Y}_{k-1,W_k}\in \mathbb {R}^{(m_{k}-r_{k}) \times \sum _{i=1}^{k-1} r_{i}}, \\ M_{k,W_k}&=U_{k,W_k}^{T}J_{k,W_k}\underline{Y}_{k-1,W_k}\in \mathbb {R}^{r_{k} \times \sum _{i=1}^{k-1} r_{i}}, \\ \underline{Y}_{k-1,W_k}&=\begin{bmatrix}{Y}_{1,W_k},\cdots ,{Y}_{k-1,W_k}\end{bmatrix}\in \mathbb {R}^{n \times \sum _{i=1}^{k-1} r_{i}}, \\ Y_{k,W_k}&=Z_{k-1,W_k}\tilde{Y}_{k,W_k}\in \mathbb {R}^{n \times r_{k}}. \end{aligned} \end{aligned}$$
(28)

\(V_{k,W_k}\), \(U_{k,W_k}\), \(L_{k,W_k}\), and \(\tilde{Y}_{k,W_k}\) are obtained from decomposing \(J_{k,W_k}Z_{k-1,W_k}\). The basis \(\underline{Y}_{k-1,W_k}\) is obtained by recursively decomposing \(k-1\) times from \({J}_{1,W_k}\) to \({J}_{k-1,W_k}\). Using the representation of (27), the stacked matrix \(\underline{J}_{k-1,W_k}\) is described as

$$\begin{aligned} \underline{J}_{k-1,W_{k}}= & {} \begin{bmatrix}E_{1\!,\!W_{\!k\!}\!} &{} \cdots \! &{} \! 0 \\ \vdots \! &{} \! \ddots \! &{} \! \vdots \! \\ \! 0 \! &{} \! \cdots &{} E_{k\!-\!1\!,\!W_{\!k\!}\!} \end{bmatrix} \! \! \begin{bmatrix} \! H_{1\!,\!W_{\!k\!}\!} \! &{} \! 0 \\ \! \vdots \! &{} \! \vdots \! \\ \! N_{k\!-\!1\!,\!W_k\!} \! &{} \! 0 \! \\ \! M_{k\!-\!1\!,\!W_{\!k\!}\!} &{} L_{k\!-\!1\!,\!W_{\!k\!}\!} \end{bmatrix} \! \! \begin{bmatrix}{Y}_{\!1\!,\!W_k\!}^{T} \\ \vdots \\ Y_{\!k\!-\!1\!,\!W_k\!}^{T} \end{bmatrix} \nonumber \\= & {} \underline{E}_{k-1,W_k}\underline{H}_{k-1,W_k}\underline{Y}_{k-1,W_k}^{T}. \end{aligned}$$
(29)

Then, the weighted solution \(\dot{q}_{W_{k}}^{*}\) satisfying (26) is computed as

$$\begin{aligned} \dot{q}_{W_{k}}^{*} = Y_{k,W_k}L_{k,W_k}^{-1}U_{k,W_k}^{T}(\dot{x}_{k} - J_{k}\dot{q}_{k-1}^{*}). \end{aligned}$$
(30)

Next, the optimal slack variable \(s_{k}^{*}\) is obtained as

$$\begin{aligned} s_{k}^{*} = V_{k,W_k}V_{k,W_k}^{T}(J_{k}\dot{q}_{k-1}^{*} - \dot{x}_{k}). \end{aligned}$$
(31)

The Lagrange multipliers, \(\lambda _{k}^{*}\) and \(\underline{\lambda }_{k-1}^{*}\), satisfying (26) are directly computed as

$$\begin{aligned}&\lambda _{k}^{*} = s_{k}^{*}, \end{aligned}$$
(32)
$$\begin{aligned}&\underline{\lambda }_{k-1}^{*} = - \underline{J}_{k-1,W_k}^{\ddag T}J_{k,W_k}^{T}s_{k}^{*}, \end{aligned}$$
(33)

where \(\underline{J}_{k-1,W_k}^{\ddag }\) is the pseudo-inverse matrix of \(\underline{J}_{k-1,W_k}\) which fulfills Moore-Penrose conditions except that \(\underline{J}_{k-1,W_k}\) \(\underline{J}_{k-1,W_k}^{\ddag }\) is symmetric. Each component of the Lagrange multipliers \(\underline{\lambda }_{k-1}^{*}\) can be obtained recursively in a descending order as follows:

$$\begin{aligned} \! \underline{\lambda }_{k-1}^{*} \! = \! \begin{bmatrix} \! \underline{\lambda }_{k-2}^{*} \\ \! \lambda _{k-1}^{*} \! \end{bmatrix} \! = \! \begin{bmatrix} -\underline{J}_{k-2,W_k}^{\ddag T}(J_{k,W_k}^{T}s_{k}^{*} + J_{k-1,W_k}^{T}\lambda _{k-1}^{*}) \\ -U_{k-1,W_k}L_{k-1,W_k}^{-T}Y_{k-1,W_k}^{T}J_{k,W_k}^{T}s_{k}^{*} \end{bmatrix} \end{aligned}$$
(34)

The obtained Lagrange multipliers are used as an indicator to determine which task to be deactivated depending on whether they are negative or not in the following section.

Finally, the total solution for k prioritized tasks is

$$\begin{aligned} \begin{aligned} \dot{q}_{k}^{*}&= \dot{q}_{k-1}^{*} + W_{k}^{-\frac{1}{2}}\dot{q}_{W_{k}}^{*} \\&= \dot{q}_{k-1}^{*} + W_{k}^{-\frac{1}{2}}Y_{k,W_k}L_{k,W_k}^{-1}U_{k,W_k}^{T}(\dot{x}_{k} - J_{k}\dot{q}_{k-1}^{*}). \end{aligned} \end{aligned}$$
(35)

Additionally, the solution can be computed in a recursive form as follows:

$$\begin{aligned} \dot{q}_{k}^{*} = \underline{J}_{k,W_k}^{\ddag }\underline{\dot{x}}_{k}, \end{aligned}$$
(36)

where

$$\begin{aligned} \begin{aligned} \underline{J}_{k,W_{k}}^{ \ddag }&= \left[ \begin{matrix}(I - W_{k}^{-\frac{1}{2}}\!&\!Y_{k,W_k}L_{k,W_k}^{-1}U_{k,W_k}^{T}\!&\!J_{k})\!&\!J_{k-1,W_{k-1}}^{\ddag },&\end{matrix}\right. \\&\left. \begin{matrix}W_{k}^{-\frac{1}{2}}\!&\!Y_{k,W_k}\!&\!L_{k,W_k}^{-1}\!&\!U_{k,W_k}^{T}\end{matrix} \right] \\ \underline{\dot{x}}_{k}&=\begin{bmatrix} \underline{\dot{x}}_{k-1}^{T},&\dot{x}_{k}^{T} \end{bmatrix}^{T}. \end{aligned} \end{aligned}$$
(37)

Since the solution minimizing weighted least-squares norm is the generalized solution to cover Euclidean least-squares norm solution, the formulas (36) and (37) can be utilized to compute the solution of the HQP if the weighting matrix is set to the identity matrix.

4 WHQP with inequality tasks

To obtain the optimal solution for p inequality tasks, the WHQP for the kth inequality task is formulated by rewriting (24) as

$$\begin{aligned} \begin{aligned}&\underset{\dot{q}_{W_{k}}, {s_{k}}}{\text {min}}&\frac{1}{2}\Vert s_{k} \Vert ^{2}_{2}, \\&\text {s. t.}&J_{k,W_k}\dot{q}_{W_{k}} \le \dot{x}_{k} - J_{k}\dot{q}_{k-1}^{*} + s_{k}, \\&&\underline{J}_{k-1,W_k}\dot{q}_{W_{k}} \le \underline{\dot{x}}_{k-1} - \underline{J}_{k-1}\dot{q}_{k-1}^{*}. \\ \end{aligned} \end{aligned}$$
(38)

The optimality conditions for (38) are additionally considered in (26) as follows:

$$\begin{aligned} \begin{aligned} \lambda _k^{T}(J_{k,W_k}\dot{q}_{W_{k}} - \dot{x}_{k} + J_{k}\dot{q}_{k-1}^{*} - s_{k})&= 0, \\ \underline{\lambda }_{k-1}^{T}(\underline{J}_{k-1,W_k}\dot{q}_{W_{k}}- \underline{\dot{x}}_{k-1} + \underline{J}_{k-1}\dot{q}_{k-1}^{*})&= 0,\\ \lambda _k&\ge 0, \\ \underline{\lambda }_{k-1}&\ge 0. \\ \end{aligned} \end{aligned}$$
(39)

The conditions of (39) denote complementary conditions that if an inequality task becomes active, that is, it becomes an equality task, its corresponding Lagrange multiplier must be greater than or equal to zero.

figure a

Based on the above conditions, the active-set method [16] is adopted. The active-set method iterates a loop until the optimal solution and optimal working set are determined. The working set denotes a set of equality tasks. The optimal solution indicates that it does not activate or violate any other task except the working set. The optimal working set indicates that the corresponding Lagrangian multipliers are non-negative.

The detailed procedure is described in Algorithm 1. At first, the algorithm estimates an initial working set \(\mathcal {W}_{0}\) for warm-start. Typically, \(\mathcal {W}_{0}\) contains only equality tasks if there is no initial guess for the inequality tasks. Given the working set \(\mathcal {W}\), \(\textit{WHQP\_equality}\) computes the optimal solution by using (36) (see Line 3).

Then, \(\textit{ComputeStepLength}\) finds a step length \(\alpha \) in order for the current solution to step toward the optimal solution without violating the tasks as (see Line 4)

$$\begin{aligned} \alpha = \min (1, \underset{k,r}{\min }(\alpha _{k,r})), \end{aligned}$$
(40)

where

$$\begin{aligned} \alpha _{k,r} = {\left\{ \begin{array}{ll} \frac{\dot{x}_{k}[r]-J_{k}[r]\dot{q}^{(j)}}{J_{k}[r](\dot{q}^{*(j)}-\dot{q}^{(j)})} &{} if \ \ J_{k}[r]\dot{q}^{(j)} \le \dot{x}_{k}[r] \\ 1, &{} otherwise. \end{array}\right. } \end{aligned}$$
(41)

In (40) and (41), k and r denote the indices of the task priority and row of the Jacobian matrix and task vector, respectively. In addition, \(\dot{q}^{*(j)}\) and \(\dot{q}^{(j)}\) represent the optimal solution and current solution at iteration j, respectively. Calculation of (41) is performed for each row of \(\underline{J}_{p}\) that is not in the working set \(\mathcal {W}\).

Starting at the current solution \(\dot{q}^{(j)}\), the solution that is translated toward the optimal solution \(\dot{q}^{*(j)}\) with a step length \(\alpha \) is calculated as follows (see Line 5):

$$\begin{aligned} \dot{q}^{(j+1)} = \dot{q}^{(j)}+\alpha (\dot{q}^{*(j)}-\dot{q}^{(j)}) \end{aligned}$$
(42)

If the step length is less than 1, the corresponding row of the Jacobian matrix, \(J_{k}[r]\) and component of the task vector, \(\dot{x}_{k}[r]\), are added to the working set, and a new iteration begins with the new working set (see Line 6–8).

Once the obtained step length is equal to 1, which indicates that the optimal solution does not activate the remaining inequality tasks, \(\textit{ComputeLambda}\) computes the Lagrangian multipliers using (32) and (33) (see Line 11). Next, to ensure that the optimality conditions of (39) are satisfied, it is checked whether or not the minimum Lagrangian multiplier is negative (see Line 12–13). If there exist negative Lagrangian multipliers, the corresponding row of the Jacobian matrix and component of the task vector are removed from the working set, and the next iteration begins (see Line 14–15).

5 Experimental results

The WHQP was validated through various experiments using the differentially-driven mobile base with the 7-DoFs manipulator [13] and the humanoid, DYROS-JET [23], with 28-DoFs. The source code is available at http://github.com/ggory15/weightedhqp. Additionally, video clips of the experimental results are available in the supplementary material.

5.1 Simulation experiment with nonholonomic mobile manipulator

To maximize the dexterity and mobility of the mobile manipulator, the controller should provide different motion patterns depending on the locomotion, manipulation, and loco-manipulation phases. In particular, because the mobile manipulator comprises two independent systems (mobile base and manipulator), assigning dominant subsystem for each task priority is effective.

Fig. 2
figure 2

Experimental results with a nonholonomic mobile manipulator in simulation. A scenario composed of two phases is designed: The first phase is for the robot to approach a laptop. Then, the robot begins to inspect the laptop while avoiding the obstacle during the second phase. Snapshots and motion ratio of the mobile and manipulator are depicted for different combinations of the weighting matrices. For Phase 1, the resultant postures are different as the weighting matrix changes from the the identity matrix (Case 1) to the mobile-dominant matrix (Case 3). For Phase 2, compared to the same weighting matrix (Case 1) and identity matrix (Case 2), the robot succeeded to execute the task without reaching the posture of singularity or joint limit by assigning individual weighting matrix (Case 3)

Full size image

5.1.1 Scenario description

To validate this, we designed the following scenario with an 11-DoFs nonholonomic mobile manipulator on simulator, CoppeliaSim. The scenario has two phases and each phase is performed for 5 sec. In the first phase, the mobile manipulator reaches the target point above a laptop by tracking the desired trajectory of the end-effector. Then, the robot begins to keep the end-effector focused on the moving target point above the laptop while avoiding an obstacle in the second phase.

5.1.2 Task and weighting matrix description

During the first phase, the robot has a single task of tracking the end-effector position trajectory as

$$\begin{aligned} J_{1}\dot{q} = \dot{x}_{1} \end{aligned}$$
(43)

where \(J_{1} \in \mathbb {R}^{3 \times 11}\) is the whole-body translational Jacobian of the end-effector [13] and \(\dot{x}_{1} \in \mathbb {R}^{3}\) is the desired linear velocity of the end-effector.

In the second phase, the robot executes three prioritized tasks. As the highest priority task, an 11-DoFs joint limit avoidance task [22] is assigned as

$$\begin{aligned} \begin{aligned} \underline{\dot{x}}_{1}&\le J_1\dot{q} \le \overline{\dot{x}}_{1},\\ J_{1}&= I_{11}, \\ \underline{\dot{x}}_{1}&= \epsilon \frac{\underline{q}-q}{\Delta t}\ \ and \ \ \overline{\dot{x}}_{1}= \epsilon \frac{\overline{q}-q}{\Delta t}. \end{aligned} \end{aligned}$$
(44)

\(I_{11} \in \mathbb {R}^{11 \times 11}\) denotes an identity matrix and \(\underline{\dot{x}}_{1}\) and \(\overline{\dot{x}}_{1}\in \mathbb {R}^{11}\) are the lower and upper bound velocity, respectively. \(\underline{q}\) and \(\overline{q} \in \mathbb {R}^{11}\) are the lower and upper limit of joint position. \(q \in \mathbb {R}^{11}\) is the current joint position, \(\epsilon \) is the tuning parameter and \(\Delta t\) is the control loop period. Then, the obstacle avoidance is formed as an 1-DoF inequality task [7] with the second priority as

$$\begin{aligned} \begin{aligned} J_{2}\dot{q}&\le \dot{x}_{2},\\ J_{2}&= n^{T}J_{obs},\\ \dot{x}_{2}&=\epsilon \frac{d-d_{thre}}{\Delta t}, \end{aligned} \end{aligned}$$
(45)

where \(n \in \mathbb {R}^{3}\) is the direction vector from the closest point on the robot to the obstacle and \(J_{obs} \in \mathbb {R}^{3 \times 11}\) is the Jacobian matrix of the closest point. d is the distance between the robot and obstacle and \(d_{thre}\) denotes the threshold distance. Finally, the lowest priority task is designed as the 3-DoFs gaze task [11] which aligns the target point with the line of sight from the end-effector. The gaze task is defined as

$$\begin{aligned} \begin{aligned} J_3\dot{q}&= \dot{x}_{3}, \\ J_3&= p_{ee,t}\times (p_{ee,l} \times J_{w}) - p_{ee,l}\times J_{v}, \\ \dot{x}_{3}&= -\lambda (p_{ee,l}\times p_{ee,t}), \end{aligned} \end{aligned}$$
(46)

where \(p_{ee,l}=p_{l}-p_{ee}\) and \(p_{ee,t}=p_{t}-p_{ee}\). \(p_{l}\in \mathbb {R}^{3}\) is the position of arbitrary point on the line of sight, \(p_{ee}\in \mathbb {R}^{3}\) is the position of the end-effector, \(p_{t}\in \mathbb {R}^{3}\) is the position of the target point, \(J_{w}\in \mathbb {R}^{3\times 11}\) is the rotational Jacobian matrix, \(J_{v}\in \mathbb {R}^{3\times 11}\) is the translational Jacobian matrix, and \(\lambda \) is gain constant.

For analyzing the effectiveness of the WHQP, the combinations of the weighting matrices of the three comparative cases are presented in Fig. 2. For Phase 1, to check the contribution of the mobile base to the tracking task depending on the weighting matrix, the weighting matrix is set from the identity matrix (Case 1) to the mobile-dominant matrix (Case 3). In the case of Phase 2, to demonstrate the effectiveness of the individual weighting matrix, the weighting matrix of Case 1 is designed as the manipulator-dominant matrix for all tasks (one weighting matrix for all prioritized tasks). Case 2 assigns the identity matrix for all tasks (equivalent to pseudo-inverse method [6]). Case 3 uses individual weighting matrices for each task priority.

5.1.3 Results

Figure 2 illustrates the snapshots of the simulation results and the ratio of the joint velocity norm between the mobile base, \(\dot{q}_{b} \in \mathbb {R}^{4}\), and manipulator, \(\dot{q}_{m} \in \mathbb {R}^{7}\), over time. The ratio is calculated as \(\frac{\Vert \dot{q}_{b} \Vert _{2}^{2}}{\Vert \dot{q} \Vert _{2}^{2}}\) and \(\frac{\Vert \dot{q}_{m}\Vert _{2}^{2}}{\Vert \dot{q}\Vert _{2}^{2}}\), where \(\dot{q} = \begin{bmatrix} \dot{q}_{b}^{T}, \dot{q}_{m}^{T}\end{bmatrix}^{T}\).

In Phase 1 of Case 1, the mobile manipulator began to track the trajectory only by using the manipulator and used both the mobile robot and manipulator together after the arm was stretched around 1.5 sec, which comes into the singular configuration. In practice, this phenomenon occurs frequently due to the difference in dynamics and performance between the mobile base and the manipulator [10, 27]. However, since the robot tracked the trajectory with the mobile base moving relatively more than the manipulator in Case 2 and only the mobile base moving in Case 3, the robot did not reach the singular posture.

After the end of Phase 1, the robot began to align the line from the end-effector with the blue ball over the laptop with the desired trajectory of the target point while avoiding the obstacle around 6 sec. In Case 1, the resultant posture reached the joint limit and singularity because all the tasks were executed by the manipulator. In Case 2, the configuration of the manipulator reached the joint limit and singularity even though the mobile robot helped to avoid the obstacle. This implies that the weighting matrices given in Case 2 did not properly distribute the movement between the mobile base and manipulator. In contrast, the gaze task was executed by the manipulator and the obstacle was avoided by the mobile robot in Case 3. Since the whole-body motion was properly distributed by assigning individual weighting matrices for each task priority, the robot could execute the tasks successfully without additional consideration of the necessary constraints.

Therefore, setting individually proper weighting matrices for given tasks is very effective for performing various and complex scenarios for the redundant robot.

Fig. 3
figure 3

Experimental results with a nonholonomic mobile manipulator in delivery scenario. The mobile manipulator tracks the end-effector trajectory by using the whole-body, but generates various motion patterns by changing the weighting matrix over time. During Phase 1, the robot dominantly used the mobile robot to reach the target position. Then, the robot only exploited the manipulator to approach and pick up the box. For the last phase, the robot stacked the box on the other boxes by using the whole-body coordinately

Full size image

5.2 Real experiment with nonholonomic mobile manipulator

5.2.1 Scenario description

In this section, a typical delivery scenario consisting of locomotion, manipulation, and loco-manipulation phases was designed with a real robot. The scenario has three phases. The first phase is for the robot to execute locomotion task for 15 s. The second phase is to perform manipulation task of picking up a box from 15 to 42 s. In the last phase of loco-manipulation, the robot delivers the box in front of the door from 42 to 50 s.

5.2.2 Task and weighting matrix description

Each phase includes the same task hierarchies: a joint limit avoidance task (the first priority) and tracking task for the trajectory of the end-effector (the second priority). The joint limit avoidance task (\(J_{1} \in \mathbb {R}^{11\times 11}\) and \([\underline{\dot{x}}_{1}, \overline{\dot{x}}_{1}] \in \mathbb {R}^{11}\)) is same as (44). The second priority task is to track the desired trajectory of the end-effector as

$$\begin{aligned} \begin{aligned} J_2\dot{q}&= \dot{x}_{2}, \\ J_2&= \begin{bmatrix} J_{v} \\ J_{w} \end{bmatrix} \ \ and \ \ \dot{x}_{3} = \begin{bmatrix} \dot{p}_{ee,d} \\ \delta \Phi \end{bmatrix}, \end{aligned} \end{aligned}$$
(47)

where \(J_{2} \in \mathbb {R}^{6\times 11}\) is the whole-body Jacobian of the end-effector, \(\dot{p}_{ee,d} \in \mathbb {R}^{3}\) is the desired linear velocity of the end-effector, and \(\delta \Phi \in \mathbb {R}^{3}\) is the orientation error.

To generate various types of motion patterns, different weights are assigned to the tracking task (\(J_{2}\), \(\dot{x}_{2}\)) for each phase:

$$\begin{aligned} W_{2}=\begin{bmatrix} 0.001I_{4} &{} 0 \\ 0 &{} I_{7} \end{bmatrix}\rightarrow \begin{bmatrix} I_{4} &{} 0 \\ 0 &{} 0.001I_{7} \end{bmatrix}\rightarrow \begin{bmatrix} I_{4} &{} 0 \\ 0 &{} I_{7} \end{bmatrix} \end{aligned}$$
(48)

In the first phase, the weighting matrix is set such that the trajectory is dominantly tracked by the mobile robot. Then, the robot mainly exploits the manipulator to track the trajectory in the second phase. In the last phase, the weights are equally distributed to the manipulator and mobile base.

5.2.3 Results

As depicted in Fig. 3, from 0 to 15 s, the robot tracked the desired locomotion trajectory through mobile dominant behaviorFootnote 1. Then, our WHQP-based controller generated the motions of the manipulator to accurately pick up a box during the manipulation phase. Finally, the robot succeeded in placing a box on the stacked boxes using whole-body motion from 42 to 50 s.

5.3 Real experiment with humanoid

5.3.1 Scenario description

Similar to Sects. 5.1 and 5.2, we validated the performance of the WHQP by implementing the inverse kinematics controller for a humanoid. A box-taping scenario was designed by using the 16-DoFs upper body of the humanoid for 15 s.

Fig. 4
figure 4

Experimental results with a humanoid in box-taping scenario. The upper-body of the humanoid executes two prioritized tasks: Initial position of the left hand is maintained as the top priority. The relative motion between the right and left hand is generated as the lower priority. Since the two tasks share the joints of the waist and left arm, the weighting matrices are designed in order to execute the tasks effectively: The first task only uses the left arm without waist and the second task only exploits the waist and right arm without the left arm

Full size image

5.3.2 Task and weighting matrix description

The humanoid executes two prioritized tasks. The first task is to maintain the initial position of the left hand as

$$\begin{aligned} \begin{aligned} J_1\dot{q}&= \dot{x}_{1}, \\ J_1&= \begin{bmatrix} J_{left}, 0_{6\times 7}, J_{waist} \end{bmatrix} \ \ and \ \ \dot{x}_{1} = \begin{bmatrix} \dot{p}_{left,d} \\ \delta \Phi _{left} \end{bmatrix}, \end{aligned} \end{aligned}$$
(49)

where \(J_1 \in \mathbb {R}^{6\times 16}\) is the Jacobian matrix for the left hand, \(J_{left}\in \mathbb {R}^{6\times 7}\) is the Jacobian matrix from the left shoulder to the left hand, and \(J_{waist}\in \mathbb {R}^{6\times 2}\) is the Jacobian matrix for the waist roll and yaw joint. \(\dot{p}_{left,d}\in \mathbb {R}^{3}\) and \(\delta \Phi _{left}\in \mathbb {R}^{3}\) are the desired linear velocity and orientation error for the left hand, respectively. Then, the relative motion task [18] between the right and left hand is assigned as the second priority:

$$\begin{aligned} \begin{aligned} J_2\dot{q}&= \dot{x}_{2}, \\ J_2&= \begin{bmatrix} -J_{left}, J_{right}, J_{waist} \end{bmatrix} \ \ and \ \ \dot{x}_{1} = \begin{bmatrix} \dot{p}_{rel,d} \\ \delta \Phi _{rel} \end{bmatrix}, \end{aligned} \end{aligned}$$
(50)

where \(J_2 \in \mathbb {R}^{6\times 16}\) is the Jacobian matrix for the relative motion, \(J_{right}\in \mathbb {R}^{6\times 7}\) is the Jacobian matrix from the right shoulder to the right hand. \(\dot{p}_{rel,d}\in \mathbb {R}^{3}\) and \(\delta \Phi _{rel}\in \mathbb {R}^{3}\) are the desired relative linear velocity and relative orientation error, respectively.

Since the joints for executing two prioritized tasks share waist and left arm joints, individual weighting matrices are assigned for each task, as follows:

$$\begin{aligned} \begin{aligned} W_1 = \begin{bmatrix} 0.01I_{7} &{} &{} \\ &{} I_{7} &{} \\ &{} &{} I_{2} \end{bmatrix} and\ W_2 = \begin{bmatrix} I_{7} &{} &{} \\ &{} 0.01I_{7} &{} \\ &{} &{} 0.01I_{2}\end{bmatrix} \end{aligned} \end{aligned}$$
(51)

The weighting matrix is set such that the left arm is predominantly used for the first priority task (\(W_{1}\)) and the waist and right arm are predominantly used for the relative motion task of the second priority (\(W_{2}\)).

5.3.3 Results

Figure 4 depicts the snapshots of box-taping scenarios and a motion ratio for the left-arm, right-arm, and waist. Thanks to the WHQP formulation with weighting matrices, the humanoid robot achieved dexterous manipulation through whole-body behavior. Consequently, the WHQP for a nonholonomic mobile manipulator and a humanoid presented in Sects. 5.1 and 5.2, respectively, can deal with complex real-world scenarios without any additional constraints or planners.

6 Discussions and implementation details

6.1 Computation cost

To validate the computational efficiency of the WHQP, the computation time for the equality tasks was compared with those of the two methods in Sect. 2.1: weighted pseudo-inverse (7) with (8a) and (7) with (8b). The total number of DoFs was set to 30, and the number of task hierarchies p ranged from 1 to 10. Also, the task dimension \(m_k\) was uniformly distributed depending on the number of task hierarchies, \(\sum _{k=1}^{p} m_k \approx 30\). We randomly generated Jacobian matrices with full rank and task vectors and performed 500 calculations for each number of task priorities. Finally, an Intel Core i7 with a 16 GB RAM computer was used.

As shown in Fig. 5, the WHQP exhibits a low computation time even when the number of task hierarchies increases. This is because the null-space projection matrix is decomposed into the product of the orthogonal bases. Thus, the size of the inverse matrix is reduced when recursively propagating to compute the solution of a low priority task. In contrast, the other two approaches have a higher computation time than that of the WHQP because the null-space projection matrix of the weighted pseudo-inverse is inherently asymmetric. Therefore, the size of the inverse matrix cannot be reduced.

Fig. 5
figure 5

Computation time for the equality tasks with respect to the number of task hierarchies. Total DoFs were assumed as 30 and task dimension for each task hierarchy was uniformly distributed. The proposed WHQP maintains lower computation time than other weighting pseudo-inverse methods (7) with (8a) and (8b). Additionally, computation time by the HQP is plotted for fair comparison. By assigning weighting matrix, the WHQP has higher computation cost than the HQP

Full size image

On the other hand, the proposed WHQP has inherently higher computation cost than the HQP as shown in Fig. 5. When computing the total solution for k-th task in (35), the operation of the COD is performed \(k(k+1)/2\) times, whereas the HQP performs k operations of the COD. This is because the null-space matrix \(Z_{k-1,W_{k}}\) is obtained by recursively decomposing from \(J_{1,W_{k}}\) to \(J_{k-1,W_{k}}\).

6.2 Composite weighting matrix in same hierarchy

Since our formulation handles one weighting matrix at each level, a composite weighting matrix is required if there are two or more tasks at the same task level. In practice, we present a simple construction method using a linear combination [4].

$$\begin{aligned} W = a_{1}W_{1} + a_{2}W_{2} + \cdots + a_{n}W_{n}, \end{aligned}$$
(52)

where \(\sum _{i=1}^{n} a_{i} = 1\) and \(0 \le a_{i} \le 1\). The magnitude of \(a_{i}\) indicates which weighting matrix is more dominant at the same level.

6.3 Nullity of WHQP

If the remaining nullity of the WHQP is 0,Footnote 2 the result of the WHQP becomes the same as that of the original HQP solver. It is evident that a weighted Euclidean norm is equivalent to a normal Euclidean norm when the matrix is not redundant [20]. Therefore, to enhance the effectiveness of the weighting matrices of tasks in the WHQP, it is recommended to configure a set of the tasks where the nullity of the controller exists.

7 Conclusions

Whole-body controllers with hierarchical optimization have great advantages in controlling redundant robots, including mobile manipulators and humanoids. However, the motion generated by these controllers is often undesirable, without additional constraints. In this study, a novel weighted hierarchical quadratic programming method to assign joint weights for each task priority is proposed. The proposed method can be summarized as follows. First, because our algorithm treats a weighting matrix for each task, it can generate various whole-body behaviors depending on the scenarios, in comparison with previous studies. Next, our algorithm using the active-set method can efficiently handle inequality tasks as well as equality tasks. Finally, we demonstrated the effectiveness of the proposed controller through several experiments with a real mobile manipulator and humanoid. With these excellent results, we expect that our method can be applied to other highly redundant systems, such as aerial manipulators. Our future work will involve the extension of the proposed framework for automatically generating suitable weighing matrices for each task.