% IKINPRI1    inverzna kinematika sa stepenom prioriteta
%
%  [q qd] = ikinprio (dh, r1, rd1, r2, rd2, jacdotfun2, dt,G,q0, qd0);
%
%   dh            kinematicki parametri po DH konvenciji
%   r1            prva manipulaciona promenljiva (deo vektora pozicije i orjentacija)
%   rd1           izvod prve manipulacione promenljive
%   rdd1          drugi izvod 
%   r2            druga manipulaciona promenljiva, sa manjim prioriteom
%   rd2           izvod druge man. promenljive
%   rdd2          drugi izvod 
%   funjac2       funkcija koja racuna drug man. prom. i njen jakobijan
%   dt            korak integracije
%   G		  koef. povratne sprege
%   q0
%   qd0           pocetna stanja manipulatora
%   qdd0
%
%   Izracunavanje invrezne kinematike sa stepenima prvenstva. Prva manipulaciona promenljiva
%   je deo vektora diferencijalnog kretanja r=[dx dy dz drx dry drz]. Druga maipulaciona pro-
%   menljiva moze biti bilo sta, ali se mora zadati funkcija koja racuna njenu vrednost i 
%   jakobijan preko funjac2.
%
% (c) 1996 Rasic Milan

function [q,qd] = ikinprio (dh, r1, rd1, r2, rd2, funjac2, dt,G, q0, qd0);

   n=numrows(dh);    % broj stepeni slobode
   m1=numcols(r1);   % dimenzija prve man. promenljive
   np=numrows(r1);   % broj tacaka na trajektoriji
   
   if nargin<8,
      error ('Nedovoljno ulaznih ragumenata');
   elseif nargin==8,
      q0 = zeros(1,n); 
      qd0 = q0; 
      qdd0= q0;
   elseif nargin==9,
      qd0=zeros(1,n);
   end;
   
   q=[q0;zeros(np-1,n)]; qd=[qd0;zeros(np-1,n)]; 
   
%  Glavna vremenska petlja
   for i=2:1:np,

%      Izracunavanje jakobijana prvu prom. 
       [J1]  = jacobian (dh, q(i-1,:));
       
%      Svodjenje jakobijana na dimenziju prve prom. 
       J1  = J1(1:m1,:);
       
       J1p = pinv(J1);               % pseudoinverzija jakobijana
       
%      Izracunavanje jakobijana za drugu prom.
       [r2_a J2] = feval(funjac2,dh, q(i-1,:));
       
       J2p = pinv(J2);               % pseudoinverzija jakobijana

       
%      Izracunavanje pozicije i brzine u tekucem trenutku
       qi   = q(i-1,:)  +  qd(i-1,:)*dt; 


%      Povratna sprega po gresci gruge promenljive
       h2 = rd2(i) + G*(r2(i)-r2_a);       
	
%      izracunavanje ubrzanja u tekucem trenutku 
        qdi =  (J1p*rd1(i,:)' + (eye(n)-J1p*J1)*J2p*h2')';       

%      Dodeljivanje vrednosti nizovima
       q(i,:)= qi; 
       qd(i,:)=qdi; 
          
  end;    
       
