function T = dh2Tmat(theta,d,a,alpha)
sin_th = sin(theta);
cos_th = cos(theta);
sin_al = sin(alpha);
cos_al = cos(alpha);

% T = [cos_th        -sin_th       0        a;
%      sin_th*cos_al cos_th*cos_al -sin_al -sin_al*d;
%      sin_th*sin_al cos_th*sin_al cos_al  cos_al*d;
%      0             0             0        1;
%     ];

T = [cos_th,  -sin_th*cos_al,  sin_th*sin_al,  a*cos_th;
     sin_th,  cos_th*cos_al,   -cos_th*sin_al, a*sin_th;
     0,       sin_al,          cos_al,         d;
     0,             0 ,            0 ,       1;
    ];
end