# Motion Planning for a Mobile Humanoid Manipulator Working in an Industrial Environment
## 2 Materials and Methods
### Solution: Secondary Tasks  
Due to its structure, the mobile humanoid manipulator has a high degree of redundancy, i.e., it has more degrees of freedom than are needed to perform the considered tasks. The existence of additional degrees of freedom makes it possible to introduce additional criteria that allow the manner in which the humanoid manipulator carries out the task to be determined. In such a case, a conflict between the primary task and additional limitations is possible. The problem can be solved by prioritizing individual tasks that will allow secondary goals to be achieved only if they do not violate the primary task. In a typical approach, the robot task was treated as the primary objective function, and additional conditions were included as secondary functions. The tasks of the robot and the requirements determining how they are carried out are defined by the constraints introduced in the problem formulation section. The constraints imposed on the end-effector movement (6)-(9) specify the primary tasks of the robot and were taken into account along with the initial condition (10) in the solution presented in the previous section. The remaining limitations must be considered as secondary objectives of the tasks. In particular, as was shown in [29], the zero velocity of the end-effectors in the destination location, assured by the solution of the primary task, does not guarantee that the robot motion will come to a stop, once the task has been accomplished, thus satisfying the condition (10). In the case of the Jacobian pseudoinverse methods, it is important to provide the high manipulability measure of the robot (14) to avoid the loss of the Jacobian matrix rank. Moreover, the high manipulability in the destination location makes it easier to complete the next task. Finally, in practical applications, it is essential to fulfill the constraints resulting from the mechanical properties of the robot (11), as well as to avoid collisions during the robot motion in an industrial environment (12). In order to fulfill the secondary objectives of the task, the following dependency, resulting from the \(\mathbf{J}^{\dagger}(\mathbf{q})\) properties \(\mathbf{J}\mathbf{J}^{\dagger}=\mathbf{I}\), where \(\mathbf{I}\) is the identity matrix, was used. The right multiplication of this dependency by \(\mathbf{J}(\mathbf{q})\) gives:  
\[\mathbf{J}(\mathbf{I}-\mathbf{J}\mathbf{J}^{\dagger})=\mathbf{0},\]  
where \(\mathbf{P}=\mathbf{I}-\mathbf{J}\mathbf{J}^{\dagger}\) is the projection matrix on the null space of \(\mathbf{J}^{\dagger}(\mathbf{q})\), and as a result, if \(\ddot{\mathbf{q}}^{*}\) is a solution of the primary task of the robot, then for any chosen vector \(\mathbf{v}\), the solution of this task is also a vector:  
\[\ddot{\mathbf{q}}=\ddot{\mathbf{q}}^{*}+\mathbf{P}\,\mathbf{v}. \tag{32}\]  
Finally, it may be concluded that vector \(\mathbf{v}\), projected by operator \(\mathbf{P}\), allows the configuration of the robot to be changed without changing the positions of its end-effectors, so it may be used to satisfy the secondary objectives of the task [30]. This approach leads to a solution of the mobile robot in the following form:  
\[\ddot{\mathbf{q}}=-\mathbf{J}^{\dagger}(\mathbf{q})\,\,\boldsymbol{v}(\mathbf{ q},\dot{\mathbf{q}})+\mathbf{P}\,\mathbf{v}. \tag{33}\]By choosing vector \(\mathbf{v}\) in such a way as to optimize some additional criterion, it is possible to take into account a secondary objective in the form \(H(\mathbf{q},\dot{\mathbf{q}})\). According to [30], mapping \(H(\mathbf{q},\dot{\mathbf{q}})\) is usually taken as a mixed objective function:  
\[H(\mathbf{q},\dot{\mathbf{q}})=\frac{1}{2}\dot{\mathbf{q}}^{T}\dot{\mathbf{q}}+ H^{\prime}(\mathbf{q}), \tag{34}\]  
whose minimization leads to a reduction in the velocities of the mobile robot and the fulfillment of the additional criteria described by mapping \(H^{\prime}(\mathbf{q})\). In this work, this additional term was used to maximize the manipulability measure and to satisfy the collision and mechanical constraints.