# Motion Planning for a Mobile Humanoid Manipulator Working in an Industrial Environment
## 2 Materials and Methods
### Solution: Primary Tasks  
According to the analysis from the previous section, the transportation task, considered in this work, consisted of three steps: moving the robot to the object, moving the object to a specific location, and moving the object along a prescribed path. The first step is a point-to-point task described by the dependency (6). In the presented approach, in order to eliminate, known from the literature, the drift effect resulting in a trajectory error increasing in time, a feedback loop algorithm, correcting the robot movement on the basis of the information from the sensors, was proposed. For this purpose, the following end-effector error function \(\mathbf{e}(t)\) between the current and fixed final end-effectors positions was introduced:  
\[\mathbf{e}(t)=\mathbf{f}(\mathbf{q})-\mathbf{p}_{f}. \tag{15}\]  
Using this function, the task of the robot can be presented in a more general form as:  
\[\lim_{t\to\infty}\mathbf{e}(t)=\mathbf{0},\quad\lim_{t\to\infty}\dot{\mathbf{ e}}(t)=\mathbf{0}. \tag{16}\]  
Such a formulation of the task allows reaching the final positions of the end-effectors, in a finite time, with any accuracy defined by the needs of the realized process. Moreover, in the proposed solution, the convergence rate and resulting final time were dependent on the gain coefficients and these can be tuned by their appropriate selection. In order to solve the trajectory planning problem of the humanoid manipulator, the following system of differential equations was introduced:  
\[\ddot{\mathbf{e}}(\mathbf{q},\dot{\mathbf{q}},\ddot{\mathbf{q}})+\mathbf{K}_ {d}\,\dot{\mathbf{e}}(\mathbf{q},\dot{\mathbf{q}})+\mathbf{K}_{p}\,\mathbf{e} (\mathbf{q})=\mathbf{0}, \tag{17}\]  
where \(\mathbf{K}_{d}=\text{diag}(K_{d}^{1},\ldots,K_{d}^{2m})\) and \(\mathbf{K}_{p}=\text{diag}(K_{p}^{1},\ldots,K_{p}^{2m})\) are \((2m\times 2m)\) diagonal gain matrices with positive coefficients, which determine the convergence rate of the solution of Equation (17). The system (17) is a homogeneous differential equation with constant coefficients. In order to find the solution, \(4m\) consistent dependencies should be given. They can be determined from the error function (15) for \(t=0\), taking into account the initial condition (10). Moreover, using the Lyapunov stability theory and an approach similar to that presented in [28] for nonholonomic mobile manipulators, it is possible to show that the system is asymptotically stable, so the end-effector error \(\mathbf{e}(t)\) and its derivative \(\dot{\mathbf{e}}(t)\) converge to zero, guaranteeing fulfillment of the conditions (16). The convergence rate depends on the gain coefficients of matrices \(\mathbf{K}_{d}\) and \(\mathbf{K}_{p}\). Determining the components \(\bar{\mathbf{e}}(\mathbf{q},\dot{\mathbf{q}},\dot{\mathbf{q}})\), \(\dot{\mathbf{e}}(\mathbf{q},\dot{\mathbf{q}})\) as:  
\[ \begin{split}\ddot{\mathbf{e}}(\mathbf{q},\dot{\mathbf{q}},\dot{ \mathbf{q}})&=\mathbf{J}(\mathbf{q})\dot{\mathbf{q}}+\mathbf{J}( \mathbf{q})\ddot{\mathbf{q}},\\ \dot{\mathbf{e}}(\mathbf{q},\dot{\mathbf{q}})&=\mathbf{ J}(\mathbf{q})\dot{\mathbf{q}}\end{split}  \tag{18}\]  
and substituting them into System (17), the dependency that allows determining the trajectory of the humanoid can be written as:  
\[\mathbf{J}(\mathbf{q})\ddot{\mathbf{q}}+\boldsymbol{v}(\mathbf{q},\dot{ \mathbf{q}})=\mathbf{0}, \tag{19}\]  
where \(\boldsymbol{v}(\mathbf{q},\dot{\mathbf{q}})=\mathbf{J}(\mathbf{q})\dot{ \mathbf{q}}+\mathbf{K}_{d}\mathbf{J}(\mathbf{q})\dot{\mathbf{q}}+\mathbf{K}_{ p}(\mathbf{f}(\mathbf{q})-\mathbf{p}_{f})\). The dependency (19) formulates the primary task of the humanoid manipulator, which grasps the object in the locations specified by vector \(\mathbf{p}_{f}\). Due to the high redundancy of the humanoid \((n>2m)\), the Jacobian matrix \(\mathbf{J}(\mathbf{q})\) is rectangular (the number of its columns is greater than the number of its rows), and there is an infinite number of trajectories satisfying the condition (19).