function [ HTM ] = DH_to_HTM( theta, alpha, r, d, thetaoffset)
HTM = [cos(theta+thetaoffset), -sin(theta+thetaoffset)*cos(alpha),  sin(theta+thetaoffset)*sin(alpha), r*cos(theta+thetaoffset) ;
       sin(theta+thetaoffset),  cos(theta+thetaoffset)*cos(alpha), -cos(theta+thetaoffset)*sin(alpha), r*sin(theta+thetaoffset) ;
                0,             sin(alpha),             cos(alpha),            d ;
                0,                      0,                      0,            1];
end