function A = CinDir4dof( q,L,XYZRef )

%-- Parametros de Denavit-Hartenberg -------------------------------------------------
  DH = [-q(1)       0       L(2)    pi/2    %-- Perna direita: S1 -> S6=S6'
        -q(2)       0       L(3)    0
        -q(3)       0       L(4)    0
         q(4)       0       L(5)   -pi/2
         q(5)+pi/2  0       0       pi/2];

%-- Matrizes de transformaçao Ai -----------------------------------------------------
 A(:,:,1) = [ 0  0  1  XYZRef(1)
              0 -1  0  XYZRef(2)
              1  0  0  L(1)+XYZRef(3) 
              0  0  0  1    ];
for i=1:size(DH,1),
     A(:,:,i+1) = [ cos(DH(i,1)) -sin(DH(i,1))*cos(DH(i,4))  sin(DH(i,1))*sin(DH(i,4))  DH(i,3)*cos(DH(i,1))
                    sin(DH(i,1))  cos(DH(i,1))*cos(DH(i,4)) -cos(DH(i,1))*sin(DH(i,4))  DH(i,3)*sin(DH(i,1))
                    0                          sin(DH(i,4))               cos(DH(i,4))  DH(i,2)
                    0             0                          0                          1  ]; 
 end   
% A(:,:,19) = [ 0 -sin(DH(18,1))  -cos(DH(18,1))  L(1)*cos(DH(18,1))
%               0  cos(DH(18,1))  -sin(DH(18,1))  L(1)*sin(DH(18,1))
%               1              0               0                   0
%               0              0               0                   1 ];
 %--------------------------------------------------------------------------------end
