For the shown general serial link n-DOF robotic arm the joint inertial positions are given by $p_i$, where $i=1,...n$:
I learned that the joints' inertial positions can be calculated in one of two ways:
1- $p_i=r_o + b_o + \sum _{k=1}^{i} (a_k+b_k)$
2- As the first three elements of $\bar{p}_i$, where: $\bar{p}_i=T_o$ $^oT_1\text{...}$ $^{i-1}T_i$ $\bar{p}_0,$ $^{i-1}T_i$ is the homogeneous transformation matrix from coordinate $(i)$ to coordinate $(i-1)$, and $\bar{p}_0=\left( \begin{array}{c} 0 \\ 0 \\ 0 \\ 1 \\ \end{array} \right). [1]$
By definition, the two methods should yield identical results. The problem is that they don't for a $\textbf{ wrist-partitioned manipultor}$.
$\textbf{My question is: Where could I be going wrong?}$
In the following I describe the specifics of my calculations:
1- $a_i$ is calculated as $a_i=A_o$ $^oA_1 ...$ $^{i-1}A_i$ $^ia_i$ (where $^{i-1}A_i$ is the rotation matrix). $b_i$ is calculated in a similar manner.
2- The homogeneous transformation matrix is
$^{i-1}T_i=\left( \begin{array}{cccc} \cos \left(\theta _i\right) & -\sin \left(\theta _i\right) & 0 & a_i \\ \cos \left(\alpha _i\right) \sin \left(\theta _i\right) & \cos \left(\alpha _i\right) \cos \left(\theta _i\right) & -\sin \left(\alpha _i\right) & -d_i \sin \left(\alpha _i\right) \\ \sin \left(\alpha _i\right) \sin \left(\theta _i\right) & \sin \left(\alpha _i\right) \cos \left(\theta _i\right) & \cos \left(\alpha _i\right) & d_i \cos \left(\alpha _i\right) \\ 0 & 0 & 0 & 1 \\ \end{array} \right)$
Where the rotation matrix,$^{i-1}A_i$, is the $3\times{3}$ top-left matrix. And $T_o$ is given by:
$T_o=\left( \begin{array}{cc} A_o & r_o \\ 0 & 1 \\ \end{array} \right)$
Where:
$A_o=\left( \begin{array}{ccc} \cos \left(\theta _{b_z}\right) & -\sin \left(\theta _{b_z}\right) & 0 \\ \sin \left(\theta _{b_z}\right) & \cos \left(\theta _{b_z}\right) & 0 \\ 0 & 0 & 1 \\ \end{array} \right).\left( \begin{array}{ccc} \cos \left(\theta _{b_y}\right) & 0 & \sin \left(\theta _{b_y}\right) \\ 0 & 1 & 0 \\ -\sin \left(\theta _{b_y}\right) & 0 & \cos \left(\theta _{b_y}\right) \\ \end{array} \right).\left( \begin{array}{ccc} 1 & 0 & 0 \\ 0 & \cos \left(\theta _{b_x}\right) & -\sin \left(\theta _{b_x}\right) \\ 0 & \sin \left(\theta _{b_x}\right) & \cos \left(\theta _{b_x}\right) \\ \end{array} \right)$
represents the rotation of the base. For fixed base systems $A_o$ and $T_o$ are identity matrices.
$\underline{EDIT}$:
My robot model and the DH parameters I obtained for it are shown below:
$ \alpha =[0^{\circ},90^{\circ},0^{\circ},-90^{\circ},0^{\circ},-90^{\circ}] \\ a=[0,0,a_2,0,0,0] \\ d=[d_1,d_2,0,d_4,0,0] \\ \theta =[\theta_1(t),\theta_2(t),\theta_3(t),\theta_4(t),\theta_5(t),\theta_6(t)] \\$ $\text{Where:} \\ $ $d_2=L_2 \\ d_1=L_1 \\ a_2=L_3 \\ d_4=L_5 $
$[1]$ Liu Haitao, Zhang Tie, "A New Approach to Avoid Singularities of 6-DOF Industrial Robot"
