## 4 Manipulability Maximization and Constraints Satisfaction
### Joint Position Constraints  
Joint position constraints are common requirements in the motion planning of robotic systems. In this work, joint limits and self-collision avoidance are considered. These constraints are included in the proposed solution (12) using matrix \(W\), which we define as the product of three terms as follows:  
\[W=W_{Jlim}W_{Col}T_{q}^{-1},\]  
where \(W_{Jlim}\) is the weighting matrix for joint limits constraints, \(W_{Col}\) is the weighting matrix for self-collision avoidance, and the matrix \(T_{q}\) (known as the Jacobian normalization matrix), is used to normalize the velocity commands \(\mathbf{u}\) as follows [12]:  
\[T_{q}=\text{diag}\left(\frac{1}{u_{1_{max}}},\frac{1}{u_{2_{max}}},\dots,\frac{ 1}{u_{\delta_{max}}}\right),\]  
where \(u_{i_{max}}\) is the \(i\)th maximum velocity command. The weighting of this matrix handles the differences in units and scales among all the joints.  
For mobile manipulators, matrices \(W_{Jlim}\) and \(W_{Col}\) should be constructed only considering the arm. This is because there is no limit to the movement of the mobile platform along each degree of freedom. Likewise, the movement of the mobile platform cannot be used to avoid self-collisions because the platform moves the base of the arm. Therefore, in this work, the structure of both weighting matrices was designed to take this into account. To simplify the presentation, let \(W_{g}\) represent either \(W_{Jlim}\) or \(W_{Col}\). \(W_{g}\) is a diagonal \(\delta_{m}\times\delta_{m}\) matrix with the following form:  
\[W_{g}=\begin{bmatrix}I_{2}&0\\ 0&W_{a}\end{bmatrix},\]  
where \(I_{2}\) is an identity matrix of size 2 (for the case of the differential drive) and \(W_{a}\) is a diagonal \(n_{a}\times n_{a}\) matrix given by  
\[W_{a}=\begin{bmatrix}w_{1}&0&0&\dots&0\\ 0&w_{2}&0&\dots&0\\ \vdots&\vdots&\vdots&\ddots&\vdots\\ 0&0&0&\dots&w_{n_{a}}\end{bmatrix},\]  
where each element of the diagonal matrix \(W_{a}\) is defined using a performance criterion \(H(\mathbf{q})\).  
In the next two subsections, two separate criterion functions for joint limits and self-collision avoidance are defined. These two criterion functions have common properties. The values of \(H(\mathbf{q})\) and \(|\partial H(\mathbf{q})/\partial q_{i}|\) become very large as the constraints are violated. When constructing matrix \(W_{a}\) using these criterion functions, the joint movement towards or away from a constraint must be contemplated [11; 16]. Under this consideration, the elements of \(W_{a}\) are given by  
\[\begin{split} w_{i}=\begin{cases}\frac{1}{1+\left|\frac{ \partial H(\mathbf{q})}{\partial q_{i}}\right|},&\text{if }\Delta\Big{|}\frac{ \partial H(\mathbf{q})}{\partial q_{i}}\Big{|}>0\\ 1,&\text{if }\Delta\Big{|}\frac{\partial H(\mathbf{q})}{\partial q_{i}} \Big{|}\leq 0,\end{cases}\\ \text{with }i=1,2,\dots,n_{a},\end{split} \tag{17}\]  
where \(\Delta|\partial H(\mathbf{q})/\partial q_{i}|\) is the change rate of \(|\partial H(\mathbf{q})/\partial q_{i}|\) with respect to time, and is numerically calculated during implementation. With this choice, a value of one is assigned to \(w_{i}\), indicating that no penalty is imposed on the \(i\)th joint, if the \(i\)th joint is not moving (\(\Delta|\partial H(\mathbf{q})/\partial q_{i}|=0\)), or it moves away from a constraint (\(\Delta|\partial H(\mathbf{q})/\partial q_{i}|<0\)). On the other hand, \(w_{i}\) tends towards zero if the movement of the \(i\)th joint gets closer to a constraint.  
Hence, the element \(w_{i}\) penalizes the movement of the \(i\)th joint by means of (12) if it moves towards a constraint and stops the joint if it is too close to it.