% IDYNACTO      Inverzna dinamika kompletnog modela robota 
%
%  u = IDYNACTO (DYN, ACTDYN, Q, QD, QDD)
%  u = IDYNACTO (DYN, ACTDYN, [Q QD QDD])
%
%  Racuna potrebne pogonske napone da bi se postigla zadata pozicija, brzina i ubrzanje
%
%  Pretpostavalja se da gravitacija deluje u negativnom smeru Z ose sa 9.81 m/s^2, ali se moze
%  zadati proizvoljna gravitacija pomocu gravitacionog vektora [gx gy gz].
%
%  u = IDYNACTO (DYN, ACTDYN, Q, QD, QDD, GRAV)
%  u = IDYNACTO (DYN, ACTDYN, [Q QD QDD], GRAV)
%
%  Spoljasnje opterecenje sila/moment koji deluju na hvataljci se mogu zadati pomocu
%  sestokomponentnog vektora [Fx Fy Fz Mx My Mz]
%
%  u = IDYNACTO (DYN, ACTDYN, Q, QD, QDD, GRAV, FEXT)
%  u = IDYNACTO (DYN, ACTDYN, [Q QD QDD], GRAV, FEXT)
%
%  gde su Q, QD, QDD vektori vrste pozicije brzine i ubraznja manipulatora
%
%  vidi : RNE, FDYNACT2, INERTIA
%
%  (C) 1996 Rasic Milan

function u= idynact0(dh_dyn, actdyn, a1, a2, a3, a4, a5)
	z0 = [0;0;1];
	grav = 9.81*z0;

	n = numrows(dh_dyn);
	if numcols(a1) == 3*n,
		Q = a1(:,1:n);
		Qd = a1(:,n+1:2*n);
		Qdd = a1(:,2*n+1:3*n);
		np = numrows(Q);
		if nargin >= 4,	
			grav = a2;
		end
		if nargin == 4,
			fext = a3;
		end
	else
		np = numrows(a1);
		Q = a1;
		Qd = a2;
		Qdd = a3;
		if numcols(a1) ~= n | numcols(Qd) ~= n | numcols(Qdd) ~= n | ...
			numrows(Qd) ~= np | numrows(Qdd) ~= np,
			error('bad data');
		end
		if nargin >= 6,	
			grav = a4;
		end
		if nargin == 7,
			fext = a5;
		end
	end
	
	% Glavna vremenska petlja
	for p=1:np,

      	%  sracunavanje dimaickog modela manipulatora
	   H = inertia(dh_dyn, Q(p,:));	
	   h = rne(dh_dyn, Q(p,:), Qd(p,:), zeros(1,numcols(Q)));

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

           H1 = H./(a*ones(1,n));
           h1 = (h+c'.*Qd(p,:))./a';
	
        %  izracunavanje pogonskih napona
           u(p,:)= (H1*Qdd(p,:)' + h1')';
           u(p,1)=u(p,1)+Qdd(p,1)*actdyn(1,4)*actdyn(1,5)*actdyn(1,1)/actdyn(1,2);
           u(p,2)=u(p,2)+Qdd(p,2)*actdyn(2,4)*actdyn(2,5)*actdyn(2,1)/actdyn(2,2);
           u(p,3)=u(p,3)+Qdd(p,3)*actdyn(3,4)*actdyn(3,5)*actdyn(3,1)/actdyn(3,2);
           u(p,4)=u(p,4)+Qdd(p,4)*actdyn(4,4)*actdyn(4,5)*actdyn(4,1)/actdyn(4,2);
           u(p,5)=u(p,5)+Qdd(p,5)*actdyn(5,4)*actdyn(5,5)*actdyn(5,1)/actdyn(5,2);
        
        end;
        % Kraj vremenske petlje
