% JACOBDOT      izracunavanje jakobijana i njegovog izvoda
%
%     [J Jdot] = jacobian( dh, q, qd)
%
%   dh  matrica kinematickih parametara
%   q	vektor polozaja
%   qd  vektor brzina
%
% (c) 1996. Rasic Milan

function [J, Jdot]=jacobdot(dh,q,qd);
    
    n=numrows(dh);

% Inicijalizacija promenljivih
    T=fkine(dh,q(1:n)); 
    pn=T(1:3,4); % Vektor plozaja vrha robota
    z=[0 0 1]';  % Z-osa prvog zgloba
    z1=[];       % matrica svih Z-osa
    T=eye(4);    % Transformaciona matrica
    p=[0 0 0]';  % Vektor polozaja prvog zgloba

% Izracunavanje Jakobijana

%   petlja za svaku kolonu jakobijana
    for i=1:n,

        if dh(i,5)==0,
%       za rotacioni zglob
           Ji=[cross(z,(pn-p)); z];
        else
%       za translatorni zglob
           Ji=[z; [0 0 0]'];
        end;

        T= T*linktran(dh(i,:),q(i)); % Trans. matrica za sledeci zglob
        p= T(1:3,4);                 % Vektor polozaja sledeceg zgloba
        z= T(1:3,1:3)*[0 0 1]';  	    % Z-osa sledeceg zgloba

        z1=[z1 z];
        J= [J Ji];

    end;

% Izracunavanje izvoda jakobijana

    E=zeros(6,n*n);  % Matrica pomocnih vektora
    Jdot=[];

%   Petlja za svaku kolonu
    for i=1:n,

        z=z1(:,i);
        Jidot= zeros(6,1);

%       Petlja za sracunavanje mesovitih izvoda po svim promenljivima
        for j=1:n,

            ind=(i-1)*n+j;
            ind2=(j-1)*n+i;
            
            if i<=j,
%           Izracunavanje parcijalnog izvoda
               
               J1j = J(1:3,j);
               J2j = J(4:6,j);
               
               E(1:3, ind)= cross(z, J1j);
               E(4:6, ind)= cross(z, J2j);
               
               E(:,ind2) = E(:,ind);  % Mesoviti izvodi su jednaki
            end; % if
            
            Jidot = Jidot + E(:,ind)*qd(j);
        end; % for j
        
        Jdot = [Jdot Jidot];
    
    end; % for i
