# Motion Planning for a Mobile Humanoid Manipulator Working in an Industrial Environment
## 3 Results  
In order to verify the effectiveness of the proposed algorithm, computer simulations involving the humanoid mobile manipulator, presented in Figure 1, were considered. The kinematic parameters of the robotic system corresponding to the DH parameters, collected in Table 1, are given as (all physical values are expressed in the SI system):  
* Circular platform with radius \(r=0.3\);
* Torso: \(l_{T_{1}}=0.5\), \(l_{T_{2}}=l_{T_{3}}=0.4\);
* Arms: \(l_{L_{1}}=l_{R_{1}}=0.3\), \(l_{L_{2}}=l_{L_{3}}=l_{R_{2}}=l_{R_{3}}=0.6\);
* Base frame of the arm angle \(\beta=\pi/3\). To preserve the mechanical constraints, the coordinates of the humanoid torso should not exceed lower and upper limits \(q_{T}^{min}\), \(q_{T}^{max}\) taken as:  
\[q_{T}^{min}=\left[-\pi,\,\tfrac{1}{4}\pi,\,0\right]^{T},\quad q_{T}^{max}= \left[\pi,\,\tfrac{3}{4}\pi,\,\tfrac{3}{4}\pi\right]^{T},\]  
while the coordinates of the lower \(q_{L}^{min}\), \(q_{R}^{min}\) and upper \(q_{L}^{max}\), \(q_{R}^{min}\) limits of the arms are:  
\[q_{L}^{min}=q_{R}^{min}=\left[-\tfrac{1}{4}\pi,\,\tfrac{1}{6}\pi,\,-\tfrac{1} {4}\pi,\,-\tfrac{1}{2}\pi\right]^{T},\quad q_{L}^{max}=q_{R}^{max}=\left[ \tfrac{3}{4}\pi,\,\tfrac{5}{8}\pi,\,\pi,\,\tfrac{1}{4}\pi\right]^{T}.\]  
In order to take into account the mechanical constraints while planning the trajectory for the robot, the approach based on the original concept of Liegeois and presented in the work [31], which leads to keeping the configuration angles as close as possible to the middle of the ranges permitted, was proposed. Therefore, the penalty function \(D_{M}(\mathbf{q})\) in the criteria (35) was taken as follows:  
\[D_{M}(\mathbf{q})=\rho_{M}\sum_{i=4}^{n}\left(\frac{q^{i}-q^{i}}{q_{max}^{i}- q_{min}^{i}}\right)^{2},\]  
where \(\rho_{M}\) is the constant positive coefficient determining the strength of the penalty. In the following simulations, it was assumed that there were obstacles in the workspace. For the purpose of avoiding collisions, the distances between the robot, the transported object, and obstacles should be determined. In general, checking whether two objects will collide, or not, is equivalent to a nonlinear programming problem and engenders a large computational burden. In order to improve the effectiveness of the calculations in the presented paper, the method of enlarging obstacles, while simultaneously discretizing both the robot and the object, was proposed. In this method, it was assumed that the whole robotic system was described by two-dimensional parameterized surfaces and each \(j\)-th obstacle was approximated by set \(\mathbf{S}_{j}\), whose surface is defined by smooth function \(S_{j}(\mathbf{p})\) as follows:  
\[\mathbf{S}_{j}(\mathbf{p})=\big{\{}\mathbf{p}:\ S_{j}(\mathbf{p})\leq 0\big{\}}.\]  
In order to obtain a finite number of constraints, describing the conditions for the avoidance of collisions, each obstacle was enlarged by a certain positive value \(\sigma\). For the assumed value \(\sigma\), it was possible to determine the discretization of the surface describing the components of the mobile manipulator and the transported object, in order to ensure the conditions for the avoidance of collisions. In this way, the whole robotic system was reduced to a discrete set of points, so the collision test led to checking a finite number of inequalities (12) and (13). Finally, scalar functions \(C^{i}_{Or}(\cdot)\), \(C^{i}_{Oo}(\cdot)\) can be expressed as:  
\[C^{i}_{Or}(\mathbf{q}(t)) =S_{j}(\mathbf{p}_{k})-\sigma,\] \[C^{i}_{Oo}(\mathbf{o}(t)) =S_{j}(\mathbf{p}_{k})-\sigma,\]  
where \(\mathbf{p}_{k}\) denotes the \(k\)-th point from a discrete set of points approximating the mobile manipulator or transported object. In order to plan a collision-free trajectory, each obstacle was surrounded by a \(\delta\)-sized safety zone in which the collision avoidance conditions (12) were active. In this case, using \(C^{i}_{Or}(\cdot)\) in the above forms and introducing the functions \(d^{i}_{Or}(\mathbf{q})\):  
\[d^{i}_{Or}(\mathbf{q})=\left\{