# Real-Time Kinematics-Based Self-Collision Avoidance Algorithm for Dual-Arm Robots
## 2 Collision Model of the Robot  
### The Dual-Arm Robot  
The algorithm was implemented on the upper body of a humanoid dual-arm robot as illustrated in Figure 1a. The robot was designed by our group (the explosives ordnance disposal robot group of Shenyang Institute of Automation). The robot was equipped with two arms with seven degrees of freedom (7-DOF) and the torso with 2-DOF. Meanwhile, each arm was equipped with a gripper with 1-DOF as the end-effector.  
To implement our algorithm, we first divided the robot into three parts: the left arm, the right arm, and the torso. Figure 1b shows the structure of the robot, where the dotted lines represent the joints of the robot; \(q_{ii}\), \(q_{ri}\), and \(q_{ii}\) are defined as the \(i\)-th joint angle of the left-arm, the right-arm, and the torso,  
Figure 1: (**a**) Humanoid dual-arm robot. (**b**) Structure of the robot with 16 degrees of freedom. The robot was divided into three parts: torso, left arm, and right arm.  
respectively. To avoid collision of the links of the left arm, the angle limits of the \(i\)-th joint of the left arm were set as \(\left[q_{i,l},q_{i,u}\right]\). Similarly, the angle limits of the \(i\)-th joint of the right arm and torso were set as \(\left[q_{i,l},q_{i,u}\right]\) and \(\left[q_{i,l},q_{i,u}\right]\), respectively.  
The frames are defined as follows. The base frame \(\left\{S\right\}\) is defined as the world frame in which the robot is located. Frame \(\left\{S_{ki}\right\}\) is defined as the \(i\)-th link frame of the robot part \(k\) (\(k=t,l,r\), where \(t\) indicates the torso part, \(l\) the left-arm, and \(r\) the right-arm). The positions of the control points (center of each spherical volume) are defined according to these link frames. In addition, the position vector \({}^{i}P\in R^{3}\) and the rotation matrices \({}^{S}_{ki}R\) are defined from \(\left\{S\right\}\) to \(\left\{S_{ki}\right\}\); \(J_{bp}\) are Jacobian matrices for control points \(p\) with respect to the \(\left\{S_{ki}\right\}\) frame, as illustrated in Figure 2. In this way, a point expressed as \(P_{0}=\left(x_{0},y_{0},z_{0}\right)^{T}\) in the \(\left\{S_{ki}\right\}\) frame can be expressed in the base frame as \({}^{S}P_{0}=\sum\limits_{k}^{S}R^{k}P_{0}+{}^{S}P_{ikORG}\). Thus, all the control points are expressed in the base frame \(\left\{S\right\}\) to detect a collision in real time.  
Point \(a\) is expressed in frame \(B\) as \({}^{B}P_{a}=\left(x_{a},y_{a},z_{a}\right)^{T}\) and in frame \(A\) as \({}^{A}P_{a}=\left(x^{\prime}_{a},y^{\prime}_{a},z^{\prime}_{a}\right)^{T}\), whereas point \(b\) is expressed in frame \(C\) as \({}^{C}P_{b}=\left(x^{\prime}_{b},y^{\prime}_{a},z^{\prime}_{b}\right)^{T}\) and in frame \(A\) as \({}^{A}P_{b}=\left(x^{\prime}_{a},y^{\prime}_{a},z^{\prime}_{a}\right)^{T}\), as illustrated in Figure 2b. These can be calculated as follows:  
\[{}^{A}P_{a}={}^{A}_{B}R^{B}P_{a}+^{A}P_{BORG}, \tag{1}\]  
\[{}^{A}P_{b}={}^{A}_{C}R^{C}P_{a}+^{A}P_{CORG}, \tag{2}\]  
The distance between different points in each part can be computed as  
\[d_{ab}=\sqrt{\left(x^{\prime}_{a}-x^{\prime}_{b}\right)^{2}+\left(y^{\prime}_ {a}-y^{\prime}_{b}\right)^{2}+\left(z^{\prime}_{a}-z^{\prime}_{b}\right)^{2}} \tag{3}\]