% test sequence for forward and inverse kinematics



clear



%load the Mitsubishi dh parameters

mitsudh;



% generate legal joint angles at random

[q1, q1d] = mitjoint;

disp('Starting joint angles (degrees):')

qzad= q1d'

disp('Pritisni razmaknicu da bi nastavio rad!');
pause;




disp('-----  FORWARD  KINEMATICS  -----')



% generate T5 matrix

disp('direct solution using mitsufk:')

flops(0);

T5m = mitsufk(q1)

fprintf('flops = %10.0f\n\n',flops)

disp('Pritisni razmaknicu da bi nastavio rad!');
pause;
disp('indirect solution using fkine:')

flops(0);

T5f = fkine(dh,q1)

fprintf('flops = %10.0f\n\n',flops)


disp('Pritisni razmaknicu da bi nastavio rad!');
pause;
disp('-----  INVERSE  KINEMATICS  -----')

% inverse solution

disp('direct solution using mitsuik:')

flops(0);

qm = mitsuik(T5m);

fprintf('flops = %10.0f\n',flops)

disp('solution:')

qmd = qm*180/pi


disp('Pritisni razmaknicu da bi nastavio rad!');
pause;
disp('numerical solution using Toolbox:')

qguess = q1 + .1*(rand(5,1)-.5);



disp('original guess:')

qguessd = qguess'*180/pi

tol = 1e-5;

flops(0)

qi = ikine(dh,T5f,qguess,M,tol);

fprintf('flops = %10.0f\n',flops)

disp('solution using ikine:')

qid = qi*180/pi



disp('error in ikine solution:')

disp('qerror = qzad - qid');
qerror = q1d' - qid

disp('maxerror=max(abs(qerror))');
maxerror = max(abs(qerror))


disp('Pritisni razmaknicu da bi nastavio rad!');
pause;
% compare all solutions

disp('Original joint vector:')

qzad=q1d'

disp('mitsuik direct solution joint vector:')

qmd

disp('Toolbox: original guess (degrees):')

qguessd

disp('Toolbox: solution joint vector:')

qid

