# Motion Planning for a Mobile Humanoid Manipulator Working in an Industrial Environment
## 2 Materials and Methods
### Formulation of the Problem  
From the perspective of Industry 4.0, the use of information from the integral sensors of the robot, as well as from the sensors located in its surroundings allows tasks that, formerly, had to be planned in detail to now be completed autonomously. The basic idea of a task-motion-planning system for the task of autonomous robot considered in this work is presented in Figure 2. Such a system consists of the Task Planner, which specifies the main goals for the robot based on information from the sensors, and the Motion Planner, which plans the robot motion, according to the goals from the Task Planner. Trajectories generated by the Motion Planner are transmitted to the Robotic System. In the presented work, it was assumed that the information from the Task Planner was known and only the subsequent phases of the transportation problem of the lengthy object, as represented by blue boxes in the diagram, remained to be considered. According to the idea of the Motion Planner, presented in Figure 2, the considered task of a humanoid mobile manipulator transporting an object in an industrial environment is divided into three phases (subtasks), solved separately:  
* Moving the end-effectors to the given location \(\mathbf{p}_{f}\) starting from the current configuration \(\mathbf{q}_{0}\) in order to approach the object to be moved;
* Moving the object to the specified location \(\mathbf{o}_{f}\) in the workspace;
* Moving the object along a prescribed path \(\varphi(s)\) in order to complete a precise operation, such as an assembly task. The first task was to move the end-effectors of the humanoid manipulator to the grip points specified by the Task Planner. This is a common, point-to-point task, in this case, to be undertaken by two end-effectors simultaneously. It can be expressed as:  
\[\mathbf{f}(\mathbf{q}(T))=\left[ \begin{array}{c}\mathbf{f}_{L}(\mathbf{q}( T))\\ \mathbf{f}_{R}(\mathbf{q}(T))\end{array} \right]=\left[ \begin{array}{c} \mathbf{p}_{L,f}\\ \mathbf{p}_{R,f}\end{array} \right]=\mathbf{p}_{f}, \tag{6}\]  
where \(T\) is the final moment of the motion and \(p_{f}\) is the \(2m\)-element vector containing the final position of the left \(\mathbf{p}_{L,f}\) and the right \(\mathbf{p}_{R,f}\) end-effectors of the robot. Figure 2: Basic idea of the task-motion-planning system. In the second and third task, it is necessary to take into account the object being transported by the robot. In the presented work, such tasks were formulated as the movement of the object in the workspace, where the motion of the robot was forced by the set of constraints resulting from the closed kinematic chain established by the robot arms and the object being held by them. In both cases, the robot began the movement from the initial configuration \(\mathbf{q}_{0}\) holding the object; however, in the second task, achieving the desired final location was the only goal of any importance, while in the third task, the movement of the object was determined by the prescribed path \(\varphi(s)\). In the second task, starting from the configuration achieved in the first one, the robot moved the object being transported to the position specified by the Task Planner. According to the above considerations, this task can be described as:  
\[\mathbf{o}(T)=\mathbf{o}_{f}. \tag{7}\]  
Finally, the humanoid manipulator started from the final configuration of the previous task and carried out the precise manipulation of the object, according to the path determined by the Task Planner. Such a task can be written as follows:  
\[\mathbf{o}(t)-\varphi(s)=\mathbf{0}, \tag{8}\]  
where \(s\) is the scaling parameter, which depends on time \(t\), i.e., \(s=s(t)\), \(s\in[0,1]\). In both of the above cases, additional constraints resulting from grasping should be considered. The humanoid manipulator held the object at two contact points, and the hard finger model [25] of contact was assumed in this paper.