# Motion Planning for a Mobile Humanoid Manipulator Working in an Industrial Environment
## 3 Results
According to the approach described above, the objects in the workspace, for the purposes of avoiding collisions, were approximated by solids with smooth surfaces described by superellipsoids [32] as:  
\[S_{j}(\mathbf{p})=\Bigg{(}\bigg{(}\frac{p_{x}}{a_{x}}\bigg{)}^{\frac{2}{a_{2} }}+\bigg{(}\frac{p_{y}}{a_{y}}\bigg{)}^{\frac{2}{a_{2}}}\bigg{)}^{\frac{a_{2} }{a_{1}}}+\bigg{(}\frac{p_{z}}{a_{z}}\bigg{)}^{\frac{2}{a_{1}}}.\]  
where: \(a_{x},a_{y},a_{z}\) are semidiameters and \(\omega_{1},\omega_{2}\) are parameters determining the shape of the superellipsoid, \(\mathbf{p}=[p_{x},p_{y},p_{z}]^{T}\). Each obstacle was surrounded by a safety zone with \(\delta=0.3\). The model of the scene with superellipsoids approximating objects-obstacles and safety zones is presented in Figure 4. The parameters of the solids are shown in Table 2. \begin{table}
\begin{tabular}{l c c c c c c c c} \hline \hline
**Obstacle** & \(\mathbf{p_{x}}\) & \(\mathbf{p_{y}}\) & \(\mathbf{p_{z}}\) & \(\mathbf{a_{x}}\) & \(\mathbf{a_{y}}\) & \(\mathbf{a_{z}}\) & \(\mathbf{\omega_{1}}\) & \(\mathbf{\omega_{2}}\) \\ \hline _CNC machine_ & 1.50 & \(-1.00\) & 0.75 & 1.00 & 0.50 & 0.75 & 0.1 & 0.1 \\ _Control panel_ & 1.00 & 0.50 & 0.50 & 0.25 & 0.50 & 0.50 & 0.1 & 0.1 \\ _Conveyor_ & 3.75 & \(-1.00\) & 0.25 & 1.25 & 0.25 & 0.25 & 0.1 & 0.1 \\ _Container_ & 4.50 & 2.50 & 0.30 & 0.25 & 1.00 & 0.30 & 0.1 & 0.1 \\ \hline \hline \end{tabular}
\end{table} 
Table 2: Parameters of the superellipsoids approximating the obstacles. Figure 3: Mobile manipulator and the scene in the initial moment. According to the idea presented in Figure 2, the autonomous mobile manipulator completes its task in three phases, performing the following subtasks specified by the Task Planner:  
* Movement of the end-effectors to the grip points \(\mathbf{p}_{f}=[4.0,\,3.0,\,0.1,\,4.0,\,2.0,\,0.1]^{T}\);
* Movement of the object to the location \(\mathbf{o}_{f}=[4.0,\,-1.0,\,1.0,\,0.0,\,0.0,\,-\frac{1}{2}\pi]^{T}\);
* Movement of the object along the path \(\varphi(s)=[4.0,\,-1.0,\,1.0-\frac{1}{2}s,\,0.0,\,0.0,\,-\frac{1}{2}\pi]^{T}\). The basic versions of the presented algorithms were written and tested in a MATLAB R14 SP1 environment. In the simulations, whose results are shown below, fragments of codes were rewritten in C++ in order to speed up the calculations. The obtained results were visualized using the library (developed by the authors and available online) The Robot Toolbox for MATLAB 2.0 [33]. The manner in which the task was completed, presenting the robot and object at the beginning and at the end of each phase, is shown in Figure 5. The details of each subtask are shown in Figures 6-8. Moreover, the animation presenting the motions of the humanoid manipulator in the workspace was added as Supplementary Materials ([[https://www.mdpi.com/article/10.3390/app1136209/s1](https://www.mdpi.com/article/10.3390/app1136209/s1)]([https://www.mdpi.com/article/10.3390/app1136209/s1](https://www.mdpi.com/article/10.3390/app1136209/s1)) accessed on 17 April 2021) and, additionally, is available online at [[https://staff.uz.zgora.pl/gpajak/troolbox/masm21/](https://staff.uz.zgora.pl/gpajak/troolbox/masm21/)]([https://staff.uz.zgora.pl/gpajak/troolbox/masm21/](https://staff.uz.zgora.pl/gpajak/troolbox/masm21/)) accessed on 17 April 2021. Figure 8: The third phase of the task: precise manipulation. Figure 6: The first phase of the task: move-to-grip. Figure 7: The second phase of the task: transporting the object.