## 3 Decision Algorithm  
The decision algorithm is the component that turns the static hybrid position/force control method into a dynamic control by changing the Raibert and Craig [15] static configuration of the S-matrix, and updating its values depending on a deterministic state machine. The diagram of the overall state machine is presented in Figure 3. Here, one can see multiple system states followed by one or more transitions. Table 1 presents the state description and Table 2 presents the description of the transition. Figure 2: The mobile robot 3D model with the incorporated 5 DOF manipulator [40; 41]. To keep the state machine clear and easy to understand, several components were detailed in additional diagrams presented in Figure 4a-c. These are the homing state \begin{table}
\begin{tabular}{c c c} \hline \hline
**State** & **Diagram** & **Description** \\ \hline S\({}_{\text{i1}}\) & Main & The initial state of the entire system. \\ \hline S\({}_{\text{i2}}\) & Main/Homing & Initialization complete. \\ \hline S\({}_{\text{i2,1}}\) & Homing & Homing process started. \\ \hline S\({}_{\text{i3}}\) & All & Homing done or motion complete. \\ \hline S\({}_{\text{C1}}\) & Main & Positioning in XOY plane and rotate gripper. \\ \hline S\({}_{\text{C2}}\) & Main/Force Control & Positioning on OZ axis (move gripper up or down) and rotate around OZ for gripper orientation. \\ \hline S\({}_{\text{C3}}\) & Main/Force Control & Ready to start force control (grip object). \\ \hline S\({}_{\text{C3,1}}\) & Force Control & Doing force control for selected reference. \\ \hline S\({}_{\text{C3,2}}\) & Force Control & Gripping force is stable while holding the object. \\ \hline S\({}_{\text{C4}}\) & Main/Position & Stop force control (done). Switching to position control and open gripper. \\ \hline S\({}_{\text{C5}}\) & Position Control & Doing position control for selected reference. \\ \hline S\({}_{\text{ES}}\) & Position & Emergency stop. \\ \hline \hline \end{tabular}
\end{table} 
Table 1: State machine states list and descriptions. Figure 3: The main 5 DOF robot state machine for waste selection. machine, the position control state machine, and the force control state machine. Their state and transition descriptions can be also found in Table 1.  
The homing state machine is very simple. The homing process is started sequentially on all five DOFs (state S\({}_{12.1}\)). When the homing process is complete, the 5 DOF system transitions to the stable state S\({}_{3}\). This state is the starting point for all following control sequences. When a new task is received (transition T\({}_{\text{C1}}\)), the system begins to complete it, starting with the first two translation joints for positioning on XOY plane. Then, the vertical motion and orientation begin (S\({}_{\text{C2}}\)) and on its completion, the force control takes over for the gripper joint by changing the control type for this degree of freedom within the S-matrix. Thus, by using force control, the gripper attempts to grab the target object using the reference force given as input to the control system. Figure 4c details how the force control handles different events. While the gripper force holds, the 4 DOF positioning system lifts the object and position the gripper above the recycling tray. At this moment, the gripper force control ends by updating the S-matrix, and the object is dropped by opening the gripper's jaws using position control. After the object is dropped into the recycling tray, the task is complete, and the 5 DOF system can start a new task. To better explain the sequence, the decision Algorithm 1 that can be more easily understood is outlined below. Figure 4: State machine sub-sections: (**a**) the homing state machine; (**b**) the position control state machine; (**c**) the force control state machine.