clc;clear all;
fid = fopen('out.dat', 'w');
q    =   [0 0 100 pi/2];     % Home
d    =   [877 0 q(3) 200];
a    =   [425 375 0 0];    % mm
alfa =   [ pi 0 0 0];      % rad

% Inverse Kinematics of a Four-Axis SCARA Robot
w  = [100; 100; 200; 0; 0; -0.4];
q = Inv_k(w,a,d);
fprintf(fid,'Checking the Algorithms for sec. a and b  \n');
fprintf(fid,'Set 1 \n');
fprintf(fid,'Inverse Kinematics of a Four-Axis SCARA Robot\n');
fprintf(fid,'w = [%10.4f %10.4f %10.4f %10.4f %10.4f %10.4f] \n',w);
fprintf(fid,'q = [%10.4f %10.4f %10.4f %10.4f ] \n',q);
% Direct Kinematics
% q = [-0.2628     3.8539   577.0000    -1.6048]
wq = Dir_k(q,a,d);
fprintf(fid,'\nDirect Kinematics of a Four-Axis SCARA Robot\n');
fprintf(fid,'q = [%10.4f %10.4f %10.4f %10.4f ] \n',q);
fprintf(fid,'w = [%10.4f %10.4f %10.4f %10.4f %10.4f %10.4f] \n',wq);
clear q wq w

Set 2 Inverse Kinematics of a Four-Axis SCARA Robot

w = [200; 200; 100; 0; 0; -0.6];
q = Inv_k( w,a,d );
fprintf(fid,'\nSet 2 \n');
fprintf(fid,'Inverse Kinematics of a Four-Axis SCARA Robot \n\n');
fprintf(fid,'w = [%10.4f %10.4f %10.4f %10.4f %10.4f %10.4f] \n',w);
fprintf(fid,'q = [%10.4f %10.4f %10.4f %10.4f ] \n',q);
% Direct Kinematics
wq = Dir_k(q,a,d);
fprintf(fid,'\nDirect Kinematics of a Four-Axis SCARA Robot \n\n');
fprintf(fid,'q = [%10.4f %10.4f %10.4f %10.4f ] \n',q);
fprintf(fid,'w = [%10.4f %10.4f %10.4f %10.4f %10.4f %10.4f] \n\n',wq);

Tool-configuration vector

tau = 1;
T   = 5;
dt  = 0.1;
w0  = [100; 100; 200; 0; 0; -0.4];
w1  = [200; 200; 100; 0; 0; -0.6];
% Time
t = 0:dt:T;
sz = size(t);
sp = speed(t,T,tau,dt);
fprintf(fid,'\n\n Main Program \n\n');
% Calculating the desired position in tool configuration space
for i = 1:sz(2)
    wd(:,i) = (1 - sp(i))*w0 + sp(i)*w1;
end

w = w0;
q = Inv_k(w,a,d );
% Calculate the  desired  linear velocity in tool-configuration space
for i = 1:sz(2)-1
        dwddt(:,i) = (wd(:,i+1) - w)/dt;

        % Algorithm for section c
        qd(2,i) = -2*(w(1)*dwddt(1,i)+w(2)*dwddt(2,i))/...
            (((2*a(1)*a(2))^2 - (w(1)^2 + w(2)^2 - a(1)^2 - a(2)^2)^2 )^0.5);
        bd1 = (a(1) + a(2)*cos(q(2)))*dwddt(1,i)-a(2)*sin(q(2))*dwddt(2,i)...
            - a(2)*(sin(q(2))*w(1)+cos(q(2))*w(2))*qd(2,i);
        bd2 = (a(1) + a(2)*cos(q(2)))*dwddt(2,i)+a(2)*sin(q(2))*dwddt(1,i)...
            + a(2)*(cos(q(2))*w(1)-sin(q(2))*w(2))*qd(2,i);
        b1 = (a(1) + a(2)*cos(q(2)))*w(1)-a(2)*sin(q(2))*w(2) ;
        b2 = a(2)*sin(q(2))*w(1) + (a(1) + a(2)*cos(q(2)))*w(2);
        qd(1,i) = (b1*bd2-b2*bd1)/(b1^2 + b2^2);
        qd(3,i) = -dwddt(3,i);
        qd(4,i) = pi*dwddt(6,i)/w(6);
        % To simulate the behavior of the real robot,
        % calculate  the new current position in joint space  as

        fprintf(fid,'time     = %10.4f\n',t(i));
        fprintf(fid,'speed    = %10.4f\n',sp(i));
        fprintf(fid,'wd(t+dt) = [%10.4f %10.4f %10.4f %10.4f %10.4f %10.4f] \n',wd(:,i)');
        fprintf(fid,'dw/dt    = [%10.4f %10.4f %10.4f %10.4f %10.4f %10.4f] \n',dwddt(:,i)');
        fprintf(fid,'dq/dt    = [%10.4f %10.4f %10.4f %10.4f] \n',qd(:,i)');
        fprintf(fid,'q(t+dt)  = [%10.4f %10.4f %10.4f %10.4f] \n',q');
        fprintf(fid,'w(t+dt)  = [%10.4f %10.4f %10.4f %10.4f %10.4f %10.4f] \n\n',w);
        q = q + qd(:,i)'*dt;
        w = Dir_k(q,a,d);
end
        i = i+1;
        fprintf(fid,'time     = %10.4f\n',t(i));
        fprintf(fid,'speed    = %10.4f\n',sp(i));
        fprintf(fid,'wd(t+dt) = [%10.4f %10.4f %10.4f %10.4f %10.4f %10.4f] \n',wd(:,i)');
        fprintf(fid,'q(t+dt)  = [%10.4f %10.4f %10.4f %10.4f] \n',q');
        fprintf(fid,'w(t+dt)  = [%10.4f %10.4f %10.4f %10.4f %10.4f %10.4f] \n\n',w);

fclose(fid);

Functions
function w = Dir_k( q,a,d )
% Input is the joint variable vector q 
% Output is the tool-configuration vector wq 
w = [ a(1)*cos(q(1))+ a(2)*cos(q(1)-q(2));a(1)*sin(q(1))+...
 a(2)*sin(q(1)-q(2));d(1)-q(3)-d(4);0;0; -exp(q(4)/pi)];
end
function q = Inv_k( w,a,d )
% Input is the tool-configuration vector
% Output is joint variable vector qw
q2 = acos((w(1)^2+w(2)^2-a(1)^2 - a(2)^2)/(2*a(1)*a(2)));
q3 = (d(1) - d(4) - w(3));
q4 = pi*log(abs(w(6))) ;
b1 = a(2)*sin(q2)*w(1) + (a(1) + a(2)*cos(q2))*w(2);
b2 = (a(1) + a(2)*cos(q2))*w(1)-a(2)*sin(q2)*w(2) ;
q1 = atan2(b1,b2);
q = [q1 q2 q3 q4];
end
function sp = speed(t,T,tau,dt)
sz = size(t);
%% Speed Density
for i = 1:sz(2)
if t(i) < tau
 sdt(i) =t(i)/((T-tau)*tau);
else
 if t(i) >(T -tau) 
 sdt(i) = (T- t(i))/((T-tau)*tau);
 else
 sdt(i) = 1/(T - tau);
 end
end
sp(i) =0;
end
% Speed
for i = 2:sz(2)
 sp(i) = sp(i-1) + sdt(i-1)*dt; 
end
end