% RMAC   Inverzna kinematika metodom Resolved Motion Acceleration Control
%
%   [q qd qdd] = rmrac (dh, r, rd, rdd, dt, q0, qd0m M);
%
%   r   - m*6 vektor polozaja i orjentacije [x y z alfa beta gama]
%   rd  - m*6 vektor linearnih i ugaonih brzina
%   rdd - m*6 vektor linearnih i ugaonih ubrzanja
%   dt  - vremenski korak 
%   q0  - pocetno stanje manipulatora
%   qd0 - pocetne brzine manipulatora
%   M   - matrica koja oznacava prostorne koordinate koje se uzimaju u obzir
%
% (c) 1996. Rasic Milan

function [q, qd, qdd]= rmac(dh,r,rd,rdd,dt,q0,qd0,M);
   ss= numrows(dh);
   np= numrows(r);
   m= numcols(r);
   if nargin<6,
      q0= zeros(1,ss);
   elseif nargin<7,
      qd0= zeros(1,ss);
   end;
   if (ss<6)&(nargin<8),
      error ('Matrica M mora biti zadata');
   end;
   if nargin==8,
      ind=[];
      for i=1:6,
         if M(i)~=0,
            ind=[ind, i];
         end;
      end;
   else
      ind=[1:6];
   end;
   
   q=  [q0;   zeros(np-1, ss)];
   qd= [qd0;  zeros(np-1, ss)];
   qdd=zeros(np,ss);
   
%  glavna vremenska petlja
   for i=2:1:np,
       
       [J Jdot] = jacobdot(dh, q(i-1,:), qd(i-1,:));

        J = J(ind,:);    % svodjenje na dimenziju zadatka
        Jdot = Jdot(ind,:);

       r1= (rdd(i,:)'-Jdot*(qd(i-1,:)'));
       
       qddi = (pinv(J)*r1)';
       
       qi  = q (i-1,:) + qd (i-1,:)*dt + 0.5*qdd(i-1,:)*dt^2;
       qdi = qd(i-1,:) + qdd(i-1,:)*dt;
   
       if (fix(i/100)==i/100),
          disp(i);
       end;
       
       q(i,:)  =qi;
       qd(i,:) =qdi;
       qdd(i,:)=qddi;
   end;
   
 
