## 5 The 5 DOF Hybrid Position/Force Control
Each control block uses a PID to compute the joint-specific drive inputs (Equations (17)-(19)):  
\[U_{x}=X_{PID}\big{(}S_{p},\Delta X\big{)}, \tag{17}\]  
\[U_{f}=F_{PID}\Big{(}S_{f},\Delta F,F_{S-S}\Big{)}, \tag{18}\]  
\[U=U_{x}+U_{f}. \tag{19}\]  
One can see that while the position PID regulator depends on the positioning error between the reference and the computed value, using the direct kinematics method and Denavit-Hartenberg notation [51], the force PID regulator will have as input the force control error computed using the reference value, the real force taken as a function of motor  
Figure 9: Hybrid position/force control diagram. current and the force required to compensate the object weight through the friction force compensator control block. Equation (20) presents the function that receives the robot joint information and sends it to the state machine and direct kinematics blocks:  
\[f(d_{1},d_{2},d_{3},d_{4},\theta_{1}). \tag{20}\]  
Using the joint information, Equations (21)-(23) present the elements required to compute the positioning and force errors, which are inputs to PID regulators that, in the end, drive the 5 DOF manipulators' motors. \[U=X_{PID}\big{(}S_{p},\Delta X\big{)}+F_{PID}\big{(}S_{f},\Delta F\big{)}, \tag{21}\]  
where:  
\[\Delta X=X_{p}(State,\,Obj(p,\,type),\,Obj(depth))-X_{r}(Rob_{kin}(d_{1},d_{ 2},d_{3},d_{4},\theta_{1})) \tag{22}\]  
\[\Delta F=F_{p}(State)+F_{s-s}(Obj(type))-F_{r}(F_{conv}(I_{m})) \tag{23}\]  
To obtain the PID position control values, we used the direct kinematics equations through Denavit-Hartenberg notation that are shown in Figure 10 and presented in Equation (24). Since \(\alpha\) is \(0^{\circ}\), then the direct kinematic coordinates are given by Relation (24), where a\({}_{1}\) and a\({}_{2}\) are constants given by the mechanical structure dimensions. The gripper's degree of freedom is not taken into account when computing the direct kinematics matrix. This is because we had to position the gripper near the desired object, and then close the gripper's jaws (5 th degree of freedom). Using the direct kinematics matrix:  
\[\left[ \begin{array}{cccc}cos\theta_{1}&-sin\theta_{1}&0&-a_{1}+d_{1}\\ sin\theta_{1}&cos\theta_{1}&0&d_{2}\\ 0&0&1&-a_{2}-d_{3}\\ 0&0&0&1\end{array} \right], \tag{24}\]  
and the PID general control equation:  
\[PID=K_{p}e(t)+K_{i}\int_{0}^{t}e(t)dt+K_{d}\frac{de(t)}{dt}, \tag{25}\]  
the position and force controllers were computed following classic methods [52]. Since the PID controllers are not in the scope of this paper, we will not present their design and tuning procedure. The hybrid method we have proposed in this paper uses hardware components that have a clear separation between position and force control. Because of this, the stability problem is avoided, since when switching the control method, the regulator method is  
Figure 10: Kinematics diagram and D-H notations of the 5 DOF, TTTRT manipulator. changed for that specific motor (DoF), and the motor is in a paused state (not moving). When changing the control method, we can say that the control mission is restarted, which is why there is no stability issue at the switching moment within the control law.