# Finding Optimal Manipulator Arm Shapes to Avoid Collisions in a Static Environment
## 3 Simulation Model
### Robot  
The robot follows a trajectory given by its TCP, so it is necessary to calculate the inverse kinematics (IK) to get joint variables from the TCP position and orientation. As the kinematic structure of the robot is given and known, an analytical method of IK can be used. The calculation of IK with an analytical method is extremely fast and stable; the solution is calculated in one pass (it is not an iterative method). Changing the shape of the arms does not affect the IK calculation because the joints stay in the same relative positions. Changing the length of the arms affects just two constants in the algorithm--two Denavit-Hartenberg (D-H) parameters, namely \(a_{2}\) and \(d_{4}\) (see Table 1). The first step in the IK calculation is getting the \(q_{1}\) value. Based on the known TCP position \(P_{6}\), we calculate point \(P_{4}\) (the origin of \(LCS_{4}\)--local coordinate system of the fourth joint) as  
\[P_{4}=P_{6}-\vec{z}_{6}\cdot d_{6}, \tag{10}\]  
Figure 3: Geometry of the chosen shape for an arm link consisting of two cubic Bézier curves; \(A\) and \(B\) represent locations of the two joints. where \(d_{6}=|P_{4}P_{6}|\) is a D-H parameter. Using the components of point \(P_{4}=[x_{4},y_{4},z_{4}]\), we can write  
\[q_{1}=\text{atan2}(y_{4},x_{4}). \tag{11}\]  
Next, it is possible to calculate  
\[z_{4}^{\prime} =z_{4}-d_{0}, \tag{12}\] \[r_{4} =\sqrt{x_{4}^{2}+y_{4}^{2}}, \tag{13}\]  
where \(d_{0}=|P_{g}P_{0}|\) is a D-H parameter. Further calculations are done according to Figure 4:  
\[p=\sqrt{r_{4}^{2}+z_{4}^{\prime\,2}},\qquad\qquad p\leq a_{2}+d_{4}, \tag{14}\]  
where \(a_{2}=|P_{1}P_{2}|\) and \(d_{4}=|P_{2}P_{4}|\) are D-H parameters. The mentioned condition is necessary for verification of whether the arm is able to reach the desired target--if not, the calculation can be aborted here with an error. As can be seen in Figure 4, for the given points \(P_{1}\) and \(P_{4}\), it is possible to find two different solutions for the positions of \(P_{2}\). The calculations of the following auxiliary angles are identical for both cases:  
\[\delta =\text{atan2}\big{(}z_{4}^{\prime},r_{4}\big{)}, \tag{15}\] \[\alpha =\text{acos}\frac{-p^{2}+a_{2}^{2}+d_{4}^{2}}{2a_{2}d_{4}},\] (16) \[\beta =\text{acos}\frac{-d_{4}^{2}+a_{2}^{2}+p^{2}}{2a_{2}p}. \tag{17}\]  
However, the way of expressing \(q_{2}\) and \(q_{3}\) is different--for the first solution, it is  
Figure 3:0  
and for the second solution, it is  
Figure 3:1  
At this stage, it is possible to calculate the transformation matrices \(\mathbf{T}_{g^{\prime}}^{0}\), \(\mathbf{T}_{0}^{1}(q_{1})\), \(\mathbf{T}_{1}^{2}(q_{2})\), and \(\mathbf{T}_{2}^{3}(q_{3})\) from the Denavit-Hartenberg formula:  
\[\mathbf{T}_{i-1}^{i}= \begin{bmatrix}\cos\!\theta_{i}&-\!\sin\!\theta_{i}\cos \!\alpha_{i}&\sin\!\theta_{i}\sin\!\alpha_{i}&a_{i}\cos\!\theta_{i}\\ \sin\!\theta_{i}&\cos\!\theta_{i}\cos\!\alpha_{i}&-\!\cos\!\theta_{i}\sin\! \alpha_{i}&a_{i}\sin\!\theta_{i}\\ 0&\!\sin\!\alpha_{i}&\cos\!\alpha_{i}&d_{i}\\ 0&0&0&1\end{bmatrix} , \tag{20}\]  
Figure 4: Inverse kinematics of the chosen robot structure—diagram for the calculation of \(q_{2}\) and \(q_{3}\) (two different solutions). and subsequently, to apply forward kinematics and get the positions and orientations of the coordinate systems \(LCS_{0}\), \(LCS_{1}\), \(LCS_{2}\), and \(LCS_{3}\). The remaining joint variables can be found using vector algebra.