# Finding Optimal Manipulator Arm Shapes to Avoid Collisions in a Static Environment
## 3 Simulation Model
### Robot
The value of \(q_{5}\) is given simply as the angle between unit vectors \(\vec{z}_{6}\) (known as the IK input) and \(\vec{z}_{3}\) (already known as an axis of \(LCS_{3}\)):  
\[q_{5}=\text{acos}(\vec{z}_{3}\cdot\vec{z}_{6}). \tag{21}\]  
Afterward, we find the projection of the vector \(\vec{z}_{6}\) on the vector \(\vec{z}_{3}\) as  
\[\vec{m}=(\vec{z}_{6}\cdot\vec{z}_{3})\vec{z}_{3}, \tag{22}\]  
and the normalized perpendicular vector from the vector \(\vec{z}_{6}\) to the vector \(\vec{z}_{3}\):  
\[\vec{n}=\frac{\vec{z}_{6}-\vec{m}}{|\vec{z}_{6}-\vec{m}|}. \tag{23}\]  
The angle between \(\vec{n}\) and the axis \(x_{3}\) represents the value \(q_{4}\) (the reference vector \(\vec{z}_{3}\) is used to distinguish between an obtuse and acute angle):  
\[q_{4}= \begin{cases}\text{acos}(\vec{x}_{3}\cdot\vec{n}),&\text{if }(\vec{x}_{3} \times\vec{n})\cdot\vec{z}_{3}\geq 0\\ -\text{acos}(\vec{x}_{3}\cdot\vec{n}),&\text{if }(\vec{x}_{3}\times\vec{n}) \cdot\vec{z}_{3}<0\end{cases} . \tag{24}\]  
Now, it is possible to calculate the transformation matrix \(\mathbf{T}_{3}^{4}(q_{4})\) and the position and orientation of \(LCS_{4}\), and to find the last missing value \(q_{6}\) as the angle between vectors \(\vec{y}_{5}\) and \(\vec{y}_{6}\):  
\[q_{6}= \begin{cases}\text{acos}(\vec{y}_{5}\cdot\vec{y}_{6}),&\text{if }(\vec{y}_{5} \times\vec{y}_{6})\cdot\vec{z}_{6}\geq 0\\ -\text{acos}(\vec{y}_{5}\cdot\vec{y}_{6}),&\text{if }(\vec{y}_{5}\times\vec{y}_{6}) \cdot\vec{z}_{6}<0\end{cases} . \tag{25}\]  
The last step is a check of whether all values \(q_{1},q_{2},\,\ldots,q_{6}\) lie in the allowed ranges given by mechanical limits of the joints. The condition that has to be verified to find if the robot in its current form can fulfill the given task is that the robot is able to reach all parts of the trajectory. To do this, the TCP is successively placed into each point of the trajectory, and all of the following are tested:  
* IK calculation is done to verify that it has a valid solution (if it does not, the robot is not able to reach the point with its end effector). * Joint variables are checked against the limits (if at least one is out, the robot is not able to reach the point with its end effector). * Joint velocities are calculated as symmetric derivatives of the joint angles around this point and checked against the corresponding velocity limits (if at least one value is over the limit, the robot is not able to achieve the required end-point velocity). * Collisions of the robot parts are checked against the robot itself and also against the workspace obstacles (see the Section 3.3). It is also necessary to verify that all points of the trajectory are reachable using the same solution variant of the IK (see Figure 4) because the robot cannot change the configuration abruptly during movement. Only two of the eight solutions of the IK are taken into account because the other solutions do not lead to a different geometry of the manipulator links, and, thus, cannot provide any advantage when collision avoidance is considered. The omitted solutions are related to calculation of \(q_{1}\) (11) and \(q_{5}\) (21).