function A_all = DH_mat(theta,d,a,alfa)
%function used to assembly DH matrix

% DH matrix A
A = @(a,d,alfa,theta) [cos(theta) -sin(theta)*cos(alfa)  sin(theta)*sin(alfa)  a*cos(theta);
                       sin(theta)  cos(theta)*cos(alfa)  -cos(theta)*sin(alfa) a*sin(theta);
                          0            sin(alfa)              cos(alfa)            d;
                          0                0                      0                1];

%build DH link matrices
A_all = zeros(4,4,6); %initialization
for i=1:6
    A_all(:,:,i)=A(a(i),d(i),alfa(i),theta(i));
end

end
