## 3 Motion Planning Method  
The motion planning problem for task space trajectory tracking consists of finding the input velocities \(\mathbf{u}\), given the desired end-effector's position/pose \(\mathbf{r_{d}}(t)\in\mathbb{R}^{m}\) for \(t\in[t_{0},t_{f}]\), such that \(\mathbf{r}(t)\) follows \(\mathbf{r_{d}}(t)\). If the task space velocity of the end-effector is set as  
\[\dot{\mathbf{r}}=\dot{\mathbf{r}}_{\mathbf{d}}+K(\mathbf{r_{d}}-\mathbf{r}), \tag{6}\]  
where \(K\in\mathbb{R}^{m\times m}\) is a positive definite matrix, then the convergence of \(\mathbf{r}(t)\) to \(\mathbf{r_{d}}(t)\) is guaranteed. Consequently, the motion planning problem is turned into solving the input velocities \(\mathbf{u}\) from the underdetermined linear equations (5) where \(\dot{\mathbf{r}}\) is set according to (6). During the execution of the trajectory, the movement of the joints that get closer to a position constraint (joint limit or self-collision) must be penalized (slowed down). This can be achieved by using a weighting matrix. In this work, we define the weighted input velocity and the reduced weighted Jacobian as follows:  
\[\bar{J}_{W}=\bar{J}W^{\frac{1}{2}}(\mathbf{q})\ \ \ \text{and}\ \ \mathbf{u}=W^{\frac{1}{2}}(\mathbf{q})\mathbf{u}_{W}, \tag{7}\]  
where \(W(\mathbf{q})\in\mathbb{R}^{\delta_{m}\times\delta_{m}}\) is a configuration-dependent positive semidefinite matrix. It is constructed so that its elements penalize the motion of the joints based upon the system constraints. We choose \(W(\mathbf{q})\) as a diagonal matrix in this paper; furthermore, we drop the notational dependency of \(W\) on \(\mathbf{q}\) to simplify the expression in the subsequent derivation. Using (7), the system (5) can be rewritten as  
\[\dot{\mathbf{r}}=\bar{J}_{W}\mathbf{u}_{W}. \tag{8}\]Because of the kinematic redundancy, i.e., \(m<\delta_{m}\), infinite solutions to \(\mathbf{u}_{W}\) exist. The solution to (8) can be decomposed as a sum of a particular solution and a non-zero homogeneous solution. The particular solution satisfies the end-effector's task, while the homogeneous solution changes the manipulator configuration without changing the end-effector's position/pose. One way to solve the redundant system (8) is to formulate the problem as a constrained optimization problem [1], where the goal is to minimize a cost function and satisfy the constraint (8). We propose the minimization of the quadratic cost function:  
\[g(\mathbf{u}_{W})=\frac{1}{2}(\mathbf{u}_{W}-W^{\frac{1}{2}}\mathbf{u}_{0})^{T}(\mathbf{u}_{W} -W^{\frac{1}{2}}\mathbf{u}_{0});\]  
this choice is aimed to find a vector of velocities \(\mathbf{u}_{W}\) that is as close as possible to \(W^{\frac{1}{2}}\mathbf{u}_{0}\), where \(\mathbf{u}_{0}\in\mathbb{R}^{\delta_{m}}\) is a velocity vector that is used to satisfy an additional task such as maximizing the manipulability. The choice of \(\mathbf{u}_{0}\) will be presented shortly. Notice that the physical constraints are also imposed to \(\mathbf{u}_{0}\) by weighting it similarly to how \(\bar{J}_{W}\) is obtained in (7). By using the method of Lagrange multipliers to minimize \(g(\mathbf{u}_{W})\) with the equality constraint (8), the following solution is obtained:  
\[\mathbf{u}_{W}{}^{*}=\bar{J}_{W}^{\dagger}\mathbf{\dot{r}}+(I-\bar{J}_{W}^{\dagger} \bar{J}_{W})W^{\frac{1}{2}}\mathbf{u}_{0}, \tag{9}\]  
where \(\bar{J}_{W}^{\dagger}\) is the Moore-Penrose pseudoinverse of the matrix \(\bar{J}_{W}\) such that \(\bar{J}_{W}\bar{J}_{W}^{\dagger}=I\), \(I-\bar{J}_{W}^{\dagger}\bar{J}_{W}\) is the orthogonal projection into the null space of \(\bar{J}_{W}\). The first term of (9) is the particular solution, and the second term is the homogeneous solution. It is trivial to show that \(I-\bar{J}_{W}^{\dagger}\bar{J}_{W}\) projects \(\mathbf{u}_{0}\) onto the null space of \(\bar{J}_{W}\) by multiplying both sides of (9) by \(\bar{J}_{W}\).