# A Head Control Strategy of the Snake Robot Based on Segmented Kinematics
## 2 The Strategy of the Head Control
### The Workspace of Head Part
Compared to these results, the head part with 7 DOFs has a significant reachability advantage over the one with 6 DOFs. Thus, the head part with 7 DOFs is chosen. ```
1: Generate \(n_{0}\) random samples, each of which includes \(n_{h}\) joint angles. In addition, form the group \(\mathbf{Q}\). Denote the number of links of the head part as \(n_{h}\). 2:for each \(i\in[1,n_{0}]\)do
3:\(\mathbf{P}_{i}=\mathrm{kine}(\mathbf{Q}(i))[0,0,0,1]^{\mathrm{T}}\)
4:\(\mathbf{P}=\mathbf{P}\cup\mathbf{P}_{i}\);
5:endfor
6:\(\mathbf{B}_{p}=\mathrm{boundary}(\mathbf{P})\)
7: Construct a box, side length \(L=2\sum_{1}^{n_{h}}L_{i}\) and its origin \(\mathbf{O}_{box}=\mathbf{O}_{base}\). Divide equally the box into a small box group \(\mathbf{B}\)
8:\(\mathbf{B}_{s}=\mathrm{convex}(\mathbf{B}_{p})\cap\mathbf{B}\);
9:for all\(b\) such that \(b\in\mathbf{B}_{s}\)do
10:\(sol=0\)
11:\(S_{b}=\mathrm{innerSphere}(b)\)
12:\(\mathbf{P}_{s}=\mathrm{distribute}(S_{b})\)
13:for all\(\mathbf{p}\) such that \(\mathbf{p}\in\mathbf{P}_{s}\)do
14:\(\mathbf{f}_{r}=\mathrm{frame}(\mathbf{p})\)
15:\(\mathbf{F}_{p}=\mathrm{addPosition}(\mathrm{rotFrame}(\mathbf{f}_{r}), \mathbf{p})\)
16:if \(\mathrm{TRAC}-\mathrm{IK}(\mathbf{F}_{p})>0\)then
17:\(sol=sol+1\)
18:endif
19:endfor;
20:\(Sol=Sol\cup sol\)
21:endfor;
22: Define the size of \(\mathbf{P}_{s}\) as \(N_{s}\) and reachability index \(D\)
23:for all\(sol\) such that \(sol\in Sol\)do
24:\(D=\frac{sol}{N_{s}}*100\%\)
25: Draw the sphere corresponding \(sol\) with a color in the gradient color bar according to index \(D\)
26:endfor;
```  
**Algorithm 1** The algorithm for calculating and drawing the reachability of the workspace of the head part. \begin{table}
\begin{tabular}{l l} \hline \hline
**Symbol** & **Description** \\ \hline \(\mathrm{kine}(\mathbf{x})\) & calculate the forward kinematics using joint angles \(\mathbf{x}\). \\ \hline \(\mathrm{boundary}(\mathbf{x})\) & extracts the space boundary face of point group \(\mathbf{x}\). \\ \hline \(\mathrm{convex}(\mathbf{x})\) & build a polyhedron based on the boundary \(\mathbf{x}\). \\ \hline \(\mathrm{innerSphere}(x)\) & build an inscribed sphere of the box \(x\). \\ \hline \(\mathrm{distribute}(x)\) & distribute a certain number of points evenly on the sphere \(x\). \\ \hline \(\mathrm{frame}(\mathbf{p})\) & build a frame with an origin \(\mathbf{p}\), and z-axis is unit vector from the center of the sphere to \(\mathbf{p}\). X-axis and y-axis build a plane tangent to the sphere and obey right-hand law. \\ \hline \(\mathrm{rotFrame}(\mathbf{f}_{r})\) & rotate frame \(\mathbf{f}_{r}\) along its Z-axis to generate several frames evenly in a range of 360 degree. \\ \hline \(\mathrm{addPosition}(\mathbf{rotF},\mathbf{p})\) & add a position vector \(\mathbf{p}\) to a rotation matrix \(\mathbf{rotF}\) and build a transformation matrix \(\mathbf{T}\). \\ \hline \(\mathrm{TRAC}-\mathrm{IK}(\mathbf{T})\) & use TRAC-IK algorithm to solve the transformation matrix \(\mathbf{T}\). \\ \hline \(L_{i}\) & the link length of the \(i\)-th link for the head part. \\ \hline \(\mathbf{O}_{base}\) & the origin of the base frame for the head part. \\ \hline \hline \end{tabular}
\end{table} 
Table 1: The symbols of Algorithm 1.