function Tz=HTz(d)
     Tz=[ 1     0  0  0; 0     1  0  0;  0     0  1  d;  0     0  0  1];
end