# Motion Planning for a Mobile Humanoid Manipulator Working in an Industrial Environment
## 3 Results
\begin{array}{cc}(C^{i}_{Or}(\mathbf{q})- \delta)^{2}&for\ C^{i}_{Or}(\mathbf{q})\leq\delta\\ 0&otherwise\end{array} \right.\]  
the penalty function \(D_{Or}(\mathbf{q})\) in the criteria (35) can be taken as follows:  
\[D_{Or}(\mathbf{q})=\rho_{O}\sum_{i=1}^{L_{Or}}d^{i}_{Or}(\mathbf{q}),\]  
where \(\rho_{O}\) is a constant positive coefficient determining the strength of the penalty. Let us note that the significant advantage of the above form of the penalty function is its local impact on the robot. Such an approach minimizes the influence of the secondary objective on the the primary task, and as a consequence, the primary task is not disturbed when the robot moves far away from the obstacles. A certain drawback of this collision-avoidance method is the possibility of the uneven motion of the robot in the neighborhood of the obstacles resulting from changing repulsive force. This effect can be reduced by the suitable choice of coefficient \(\rho_{O}\); however, it is difficult to eliminate it completely. For collision-free trajectory planning of the object, the penalty function \(D_{Oo}\) in the dependency (37) can be taken, using an approach similar to the one described above. As is the case with other motions planners, the presented algorithm required appropriate tuning of the parameters responsible for influencing the secondary objectives introduced in the dependency (35). The parameters \(\rho_{M}\) and \(\rho_{O}\), determining the strength of the penalty connected to mechanical limits and the conditions for the avoidance of collisions, are constant values and were chosen, for the following simulations, as:  
\[\rho_{M}=1.0,\quad\rho_{O}=10.0.\]  
Coefficient \(\rho_{V}\) introduced in (36) is connected to the term leading to the minimization of the robot kinetic energy and reducing its velocity. It seems that the influence of this component should depend on the stage of the accomplishment of the task. If \(\|\mathbf{e}\|\gg\mathbf{0}\) or \(\|\mathbf{e}\|\gg\mathbf{0}\), then any reduction in velocity is undesirable because it causes an increase in the time required to complete the task. After completing the task, the robotic system should stop, so the influence of the term damping its kinetic energy ought to be considerable. For this purpose, the \(\rho_{V}\) coefficient dependent on \(\mathbf{e}\), \(\mathbf{e}\) was used:  
\[\rho(\mathbf{e},\mathbf{e})=\frac{\|(\|\mathbf{e}\|,\,\|\mathbf{e}\|)^{T}\|}{ 1+\|(\|\mathbf{e}\|,\,\|\mathbf{e}\|)^{T}\|}\.\]  
At the initial moment, the robot is in the initial configuration described by the vector of generalized coordinates as follows:  
\[\mathbf{q}(0)=\left[ \begin{array}{cc}0,0,0,\\ \mathbf{q}_{P(0)}\end{array} \right.\ \ \ \underbrace{ \begin{array}{cc}0,\, \frac{1}{3}\pi,\,\frac{1}{4}\pi,\\ \mathbf{q}_{T}(0)\end{array} }_{\mathbf{q}_{T}(0)},\ \ \underbrace{ \begin{array}{cc}0,\, \frac{1}{2}\pi,0,\,-\frac{1}{4}\pi,\\ \mathbf{q}_{L}(0)\end{array} }_{\mathbf{q}_{L}(0)},\ \ \underbrace{ \begin{array}{cc}0,\, \frac{1}{2}\pi,0,\,-\frac{1}{4}\pi\\ \mathbf{q}_{R}(0)\end{array} }_{\mathbf{q}_{R}(0)}\ \right]^{T}\]and the position and orientation of the transported object is given as:  
\[\mathbf{o}(0)=\big{[}4.0,\,2.5,\,0.0,\,0.0,\,0.0,\,0.0\big{]}^{T}. \tag{38}\]  
The position of the robot and transported object corresponding to vectors \(\mathbf{q}(0)\), \(\mathbf{o}(0)\) and the state of the workspace at the initial moment is shown in Figure 3. As can be seen, in the workspace, there is a container, a CNC machine with a control panel and a roller conveyor transporting the workpieces. In the initial moment, the humanoid manipulator is standing behind the control panel, and its task is to move the object (beam), lying in front of the container, to the conveyor.