## 4 Example: Kinematic and Dynamic Modelling of a Real Robot
Note that \(q_{1}\), \(q_{2}\), \(q_{3}\), \(q_{4}\), \(q_{11}\), \(q_{12}\), \(q_{21}\), \(q_{22}\), \(q_{31}\), \(q_{32}\), \(q_{41}\) and \(q_{42}\) are 12 generalized coordinates of the robot, in which  
\(\mathbf{u}=\left[ \begin{array}{cccc}q_{1}&q_{12}&q_{22}&q_{4}\end{array}  \right]^{T}\) is the vector of 4 independent generalized coordinates,  
\(\mathbf{z}=\left[ \begin{array}{cccc}q_{2}&q_{3}&q_{11}&q_{21}&q_{31}&q_{32}&q_ {41}&q_{42}\end{array} \right]^{T}\) is the vector of 8 dependent generalized coordinates and  
\(\mathbf{s}=\left[ \begin{array}{cccc}\mathbf{u}&\mathbf{z}\end{array} \right]^ {T}\) is the vector of all 12 generalized coordinates of the robot. _Constraint equations_  
By using the formulation Equation (7), the constraint equations can be derived for 4 closed loops as follows:  
\[f_{1}=(L_{2}-b_{2})\cos q_{2}+L_{1}-(q_{12}+L_{11})\cos q_{11}=0 \tag{26}\]  
\[f_{2}=(L_{2}-b_{2})\sin q_{2}-(q_{12}+L_{11})\sin q_{11}=0 \tag{27}\]  
\[f_{3}=b_{3}\cos q_{3}-(L_{2}-c_{2})+(q_{22}+L_{21})\cos q_{21}=0 \tag{28}\]  
\[f_{4}=b_{3}\sin q_{3}+(q_{22}+L_{21})\sin q_{21}=0 \tag{29}\]  
\[f_{5}=c_{1}\cos(q_{2}+q_{32}-3\pi/2)+L_{2}\cos q_{2}-L_{2}\cos q_{31}+c_{1}=0 \tag{30}\]  
\[f_{6}=c_{1}\sin(q_{2}+q_{32}-3\pi/2)+L_{2}\sin q_{2}-L_{2}\sin q_{31}=0 \tag{31}\]  
\[f_{7}=-(L_{3}-b_{3})\cos(q_{32}+q_{41})-(L_{3}-b_{3})\cos q_{3}-c_{1}\sin(q_{ 3}+q_{42})-c_{1}\cos q_{32}=0 \tag{32}\]  
\[f_{8}=(L_{3}-b_{3})\sin(q_{32}+q_{41})+(L_{3}-b_{3})\sin q_{3}-c_{1}\sin(q_{ 3}+q_{42})+c_{1}\sin q_{32}=0 \tag{33}\]  
_Kinematics of the robot_  
By using the formulation Equation (1), the transformation matrix for the end-effector is calculated as follows:  
\[\mathbf{H}_{04}=\mathbf{H}_{01}(q_{1})\mathbf{H}_{12}(q_{2})\mathbf{H}_{23}(q _{3})\mathbf{H}_{34}(q_{4}) \tag{34}\]  
Since \(\Theta_{3}\) and \(\Theta_{4}\) are the loops of Type B, by applying Equation (4), the matrices \(\mathbf{H}_{23}\big{(}q_{3}\big{)}\) and \(\mathbf{H}_{34}\big{(}q_{4}\big{)}\) are recalculated as follows:  
\[\mathbf{H}_{23}\big{(}q_{3}\big{)}=\mathbf{H}_{2(31)}\big{(}q_{31}\big{)} \mathbf{H}_{(31)(32)}\big{(}q_{32}\big{)}\mathbf{H}_{(32)3}\big{(}q_{3}\big{)} \tag{35}\]  
\begin{table}
\begin{tabular}{c c c c c c} \hline \hline \multirow{2}{*}{**Index \(j\)**} & \multirow{2}{*}{**Link**} & \multicolumn{3}{c}{**Mass Centre (m)**} & \multicolumn{2}{c}{**Mass (kg)**} \\ \cline{3-5}  & & \(\mathbf{x_{C}}\) & \(\mathbf{y_{C}}\) & \(\mathbf{z_{C}}\) & \\ \hline
1 & \(l_{1}\) & 0 & \(b_{1}/2\) & 0 & \(m_{1}=81.3\) \\ \hline
2 & \(l_{2}\) & \(L_{2}/2\) & 0 & 0 & \(m_{2}=50.6\) \\ \hline
3 & \(l_{11}\) & 0 & 0 & \(L_{11}/2\) & \(  
\[f_{2}=(L_{2}-b_{2})\sin q_{2}-(q_{12}+L_{11})\sin q_{11}=0 \tag{27}\]0  
Finally, the position of the end-effector \(\mathbf{E}=\left[ \begin{array}{ccc}x_{E}&y_{E}&z_{E}\end{array} \right]^{T}\) is yielded as follows:  
\[\left[ \begin{array}{c}x_{E}\\ y_{E}\\ z_{E}\end{array} \right]=\left[ \begin{array}{ccc}\cos q_{1}(L_{4}+L_{42}+(L_{ 3}-b_{3})\cos(q_{2}+q_{3})\ +\ L_{2}\cos q_{2})\\ \sin q_{1}(L_{4}+L_{42}+(L_{3}-b_{3})\cos(q_{2}+q_{3})\ +\ L_{2}\cos q_{2})\\ (L_{3}-b_{3})\sin(q_{2}+q_{3})\ +\ L_{2}\sin q_{2}\ +\ b_{1}\end{array} \right] \tag{37}\]  
Note that  
\[\cos q_{2}=\frac{\left(q_{12}+L_{11}\right)^{2}-L_{1}{}^{2}-(L_{2}-b_{2})^{2} }{2L_{1}(L_{2}-b_{2})} \tag{38}\]  
\[\cos q_{3}=\frac{-\left(q_{22}+L_{21}\right)^{2}+b_{3}{}^{2}+(L_{2}-c_{2})^{2} }{2b_{3}(L_{2}-c_{2})} \tag{39}\]  
Note that all the joint variables and the position of the end-effector in Equations (34)-(39) must be functions of time \(t\). However, to simplify the representation of such the complex equations, the term \((t)\) is truncated. In order to verify the formulation of the kinematics model, the following numerical experiments were carried out:  
1. Calculate the trajectory of the end effector \(\mathbf{E}(t)=\left[x_{E}(t)y_{E}(t)z_{E}(t)\right]^{T}\), with the input data is given as \(q_{1}(t)=\frac{\pi}{18}\times t\), \(q_{12}(t)=0.1+0.01\times t\) and \(q_{22}(t)=0.15+0.005\times t\).