Denavit Hartenberg parameters for 3R spatial manipulator - excercise

Question

I'm learning about robotics and comparing two ways to calculate homogeneous transformation matrix. I'm comparing PoE (product of exponentials) and Denavit-Hartenberg parameters. For the same base frame and same position and orientation of the final (end effector frame) these two ways should produce the same result (i.e. coordinate transformation between base - fixed frame and tool's frame). However, I'm not getting the same result and this is because I don't use D-H parameters correctly. I'm pretty sure the error is the third row of D-H table. Mutual positions of the joints is in such way, I cannot determine the axis correctly.

Here is what I did so far. Any help or advice is greatly appreciated.

However, if I do step by step calculation in Matlab, I got correct result
The code is:

L1 = 0.4; L2 = 0.3; th1 = pi/4; th2 = pi/3; th3 = pi/6;
%step by step Tms
% rotate around z by th1
T1m = rotm2tform(rotz(th1*180/pi));
%translate on x axis by L1 
T21m = [1 0 0 L1;0 1 0 0;0 0 1 0; 0 0 0 1];
T22m =  rotm2tform(rotx(90)) * rotm2tform(rotz(th2*180/pi));
% rotate around x by -90°
T31m= rotm2tform(rotz(-90))
% translate on x axis by L2
T32m = [1 0 0 L2;0 1 0 0;0 0 1 0; 0 0 0 1];
%rotate around x by -90°then rotate around z by th3
T33m = rotm2tform(rotx(-90)) * rotm2tform(rotz(th3*180/pi));
%calculate final transf matrix
Tms = T1m*T21m*T22m*T31m*T32m*T33m

The correct result should be:

    0.1768   -0.9186    0.3536    0.4666
    0.8839    0.3062    0.3536    0.4666
   -0.4330    0.2500    0.8660   -0.1500
         0         0         0    1.0000

Thank you in advance.

Answer 1

For PoE, the zero-configuration matrix and the screw axes in the spatial frame are $$ \begin{align} M&=\begin{bmatrix} 0&0&1&L_1\\0&1&0&0\\-1&0&0&-L_2\\0&0&0&1\end{bmatrix} \\ \mathcal{S}_1&=\begin{bmatrix} 0&0&1&0&0&0\end{bmatrix}^T \\ \mathcal{S}_2&=\begin{bmatrix} 0&-1&0&0&0&-L_1\end{bmatrix}^T \\ \mathcal{S}_3&=\begin{bmatrix} 1&0&0&0&-L_2&0\end{bmatrix}^T. \\ e^{[\mathcal{S}_1]\theta_1} &= \begin{bmatrix} e^{[\omega_1]\theta_1} & \Big[I\theta_1+(1-c_1)[\omega_1]+ (\theta_1-s_1)[\omega_1]^2\Big] v\\0 &1 \end{bmatrix}= \begin{bmatrix} c_1&-s_1&0&0\\ s_1&c_1&0&0\\ 0&0&1&0\\ 0&0&0&1 \end{bmatrix} \\ e^{[\mathcal{S}_2]\theta_2} &= \begin{bmatrix} e^{[\omega_2]\theta_2} & \Big[I\theta_2+(1-c_2)[\omega_2]+ (\theta_2-s_2)[\omega_2]^2\Big] v\\0 &1 \end{bmatrix}= \begin{bmatrix} c_2&0&-s_2&-L_1(c_2-1)\\ 0&1&0&0\\ s_2&0&c_2&-L_2s_2\\ 0&0&0&1 \end{bmatrix} \\ e^{[\mathcal{S}_3]\theta_3} &= \begin{bmatrix} e^{[\omega_3]\theta_3} & \Big[I\theta_3+(1-c_3)[\omega_3]+ (\theta_3-s_3)[\omega_3]^2\Big] v\\0 &1 \end{bmatrix}= \begin{bmatrix} 1&0&0&0\\ 0&c_3&-s_3&-L_2(c_3-1)\\ 0&s_3&c_3&-L_2s_3\\ 0&0&0&1 \end{bmatrix}\\ T^0_3 &=e^{[\omega_1]\theta_1}e^{[\omega_2]\theta_2}e^{[\omega_3]\theta_3}M \end{align} $$ The result is

For DH, the table is $$ \begin{array}{cccc} \hline \text{Link} & a_i & \alpha_i & d_i & \theta_i \\ \hline 1 & L_1 & 90^\circ & a_1 & \theta_1 \\ \hline 2 & L_2 & -90^\circ & 0 & \theta_2-90^\circ \\ \hline 3 & 0 & 0 & 0 & \theta_3 \\ \hline \end{array} $$

The transofmration matrix is $$ T^0_3 = T^0_1T^1_2T^2_3 $$ The result is

They are identical.