% =========================================================================
% MT3005 - LABORATORIO 6: Cinemática Diferencial Numérica
% -------------------------------------------------------------------------
% Inciso 2. Ver las instrucciones en la guía adjunta.
% =========================================================================
function A = genDHmatrix(theta, d, a, alpha)

    Rx = [1, 0, 0, 0;...
        0, cos(alpha), -sin(alpha), 0;...
        0, sin(alpha), cos(alpha), 0;...
        0, 0, 0, 1];
    
    Rz = [cos(theta), -sin(theta), 0, 0;...
        sin(theta), cos(theta), 0, 0;...
        0, 0, 1, 0;...
        0, 0, 0, 1];
    
    Tx=[1, 0, 0, a;...
        0, 1, 0, 0;...
        0, 0, 1, 0;...
        0, 0, 0, 1];
    
    Tz=[1, 0, 0, 0;...
        0, 1, 0, 0;...
        0, 0, 1, d;...
        0, 0, 0, 1];
    
    A = Rz*Tz*Tx*Rx;     
end