% 2c. dan racunskih vezbi iz Tehnicke robotike u zimskom semestru skolske 1998/99
% 26. 11. 1998.

% PART 6 : trajectory analysis

disp('Trajectory Analysis')

% set start point and speed

% position in world coordinates [x, y, z]

disp('Starting location:')

X = [0.8; 0; 0.6]

%X = [0.1; -1; 0.8]



% desired speed [Vx, Vy, Vz]

disp('Tip Speed:')

XDOT = [0; 0.2; 0]



% increment between solution points [dx, dy, dz]

DX = [0; 0.05; 0];



i = 0;  % counter for data storage for plotting later

err = 0;

format compact

disp('      Y             Joint Values                   Joint Rates')



while err == 0  % loop until inverse kinematics finds an illegal position



  % inverse kinematics to give joint positions at this X

  [q, err] = invkin3r(X);

  if err ~= 0

    break

  end



  i = i+1;





  t(:,i) = q*180/pi;      % store joint values for this point (in degrees)

  xc(i) = X(2);           % store y coordinate for this point



  % get the Jacobian at this position

  J = jacob3r(q);

  c(i) = cond(J);         % store condition number for plotting



  % compute joint rates and store

  tdot(:,i) = inv(J)*XDOT;



  % put data in row for printout to screen.  Comment out if not desired.

  prow = [X(2) q' tdot(:,i)'];

  disp(prow)  



  % increment X to the next position

  X = X + DX;

end





% plot joint positions and velocities



figure(1)

subplot(2,1,1)

plot(xc,t)

%axis([0 2 -150 100])

title('Joint positions')

xlabel('Y coordinate')

ylabel('Joint Position (degrees)')

legend('Joint 1 ','Joint 2 ','Joint 3 ')



subplot(2,1,2)

plot(xc,tdot)

%axis([0 2 -2 2])

title('Joint velocities')

xlabel('Y coordinate')

ylabel('Joint Velocity (rad/sec)')

legend('Joint 1 ','Joint 2 ','Joint 3 ')





%plot the condition number of the Jacobian



figure(2)

plot(xc,c)

title('Jacobian Condition Number')

xlabel('Y coordinate')

ylabel('Cond')



