% FDYNACT2     Racuna vrednosti izvoda za integraciju komp. din. model
% V2

function xd= fdynact2(t,x);
   global FA_DYN FA_ACTDYN FA_VOLTFUN;
  
   n = numrows(FA_DYN);

%  izdvajanje promenljvih stanja  
   q  = x(1:n)';
   qd = x(n+1:2*n)';

%  sracunavanje H i h za dinamicki model manipulatora
   H = inertia(FA_DYN, q);	
   h = rne(FA_DYN, q, qd, zeros(size(q)));

%  ukljucivanje aktuatora u dinaicki model
   a= FA_ACTDYN(:,5).*FA_ACTDYN(:,2)./FA_ACTDYN(:,1);
   c= FA_ACTDYN(:,2).*FA_ACTDYN(:,3).*(FA_ACTDYN(:,5).^2)./FA_ACTDYN(:,1);

   H1 = H./(a*ones(1,n));
   h1 = (h+c'.*qd)./a';

%  Izracunavanje pogonskog napona
   u = feval(FA_VOLTFUN, t, x);

%  Odredjivanje ubrzanja
   qdd = inv(H1) * (u - h1)';

%  Izvodi promenljivih stanja
   xd = [x(n+1:2*n,1), qdd];


