## 3 Motion Planning Method
The velocity input vector is then recovered using the second part of (7):  
\[\mathbf{u}=W^{\frac{1}{2}}\bar{J}_{W}^{\dagger}\mathbf{\dot{r}}+W^{\frac{1}{2}}(I-\bar {J}_{W}^{\dagger}\bar{J}_{W})W^{\frac{1}{2}}\mathbf{u}_{0}. \tag{10}\]  
The degree of kinematic redundancy at the velocity level is relevant for this solution to be feasible, since it defines the number of simultaneous constraints that can be satisfied in the differential kinematics inversion without \(\bar{J}_{W}\) losing its rank. The degree of redundancy for NMMs is calculated as \(R=\delta_{m}-m\). Whilst analyzing the matrix \(W^{\frac{1}{2}}\) for three different cases, it is possible to understand the expected behavior of solution (10). Let \(z\) represent the number of elements that are zero in the diagonal of \(W^{\frac{1}{2}}\), i.e., the number of joints that are forced to stop due to \(z\) simultaneously activated constraints. When \(z<R\), both the particular and homogeneous solutions exist, and therefore, the secondary task will still be considered. When \(z=R\), the system is not redundant anymore and only the particular solution exists. Finally, when \(z>R\), the system (8) has no solution. Therefore, for the solution (10) to exist, the condition \(z\leq R\) is necessary. Our proposed solution (10) has an advantage over the projected gradient solution used in [5; 6], because it includes the physical constraints and penalizes both the particular and homogeneous solutions. Furthermore, in contrast with the weighted least-norm method [11], this solution takes advantage of the redundant joints that have not been penalized, due to physical constraints, in attempt to satisfy the task defined by \(\mathbf{u}_{0}\). In this paper, the matrix \(W\) is constructed to satisfy joint position constraints (joint limits and self-collision avoidance), which is discussed in Section 4.2. The vector \(\mathbf{u}_{0}\) is designated to locally maximize a proposed new manipulability measure for mobile manipulators, as discussed in Section 4.1. To satisfy the joint velocity constraints, the particular and homogeneous solutions are analyzed. Because the particular solution is in charge of following the end-effector's task, it cannot be modified. On the other hand, the homogeneous solution can be varied to satisfy the joint velocity constraints. Furthermore, the homogeneous solution must also consider the admissible velocity commands with respect to the nonholonomic constraints [7]. Taking these two points into account, we define vector \(\mathbf{u_{0}}\) as  
\[\mathbf{u_{0}}=\pm\alpha(t)\beta(t)S^{\mathsf{T}}(\mathbf{q})\mathbf{\
abla_{q}}\mathbf{F}(\mathbf{q}), \tag{11}\]  
where \(\alpha(t)\) is a scalar coefficient that is adjusted online to satisfy joint velocity limits (Section 4.3.2), \(\beta(t)\) is a positive variation factor used to satisfy joint velocity boundary constraints (Section 4.3.1), \(S(\mathbf{q})\) is defined in (4), and \(\mathbf{\
abla_{q}}\mathbf{F}(\mathbf{q})\) is the gradient of the objective function \(F(\mathbf{q}):\mathbb{R}^{3+n_{a}}\to\mathbb{R}\), which is set to a manipulability measure. The product \(\alpha(t)\beta(t)\) is the step size of the gradient step ascent/descent, and the sign of \(\mathbf{u_{0}}\) defines whether the objective function \(F(\mathbf{q})\) will be maximized \((+)\) or minimized \((-)\). Replacing (11) in (10), the final form of the proposed solution for \(\mathbf{u}\) is:  
\[ \begin{split}&\mathbf{u}=\mathbf{u_{p}}+\alpha(t)\beta(t)\mathbf{u_{h}},\\ &\text{with}\\ &\mathbf{u_{p}}=W^{\frac{1}{2}}\mathbf{f}_{W}^{\dagger}\mathbf{\dot{r}},\\ &\mathbf{u_{h}}=W^{\frac{1}{2}}(I-\mathbf{f}_{W}^{\dagger}J_{W})W^{\frac {1}{2}}S^{\mathsf{T}}(\mathbf{q})\mathbf{\
abla_{q}}\mathbf{F}(\mathbf{q}),\end{split}  \tag{12}\]  
where the positive sign for the gradient step descent has been used because we are interested in maximizing the manipulability. Notice that when the sign of \(\alpha(t)\) is positive, the objective function \(F(\mathbf{q})\) is maximized.