Small Satellites

Home » Robotics » Direct and inverse kinematics problem,linear motion controller

Direct and inverse kinematics problem,linear motion controller

Direct and inverse kinematics problem and linear motion controller for the SCARA robot.

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);

Advertisements

Leave a Reply

Fill in your details below or click an icon to log in:

WordPress.com Logo

You are commenting using your WordPress.com account. Log Out / Change )

Twitter picture

You are commenting using your Twitter account. Log Out / Change )

Facebook photo

You are commenting using your Facebook account. Log Out / Change )

Google+ photo

You are commenting using your Google+ account. Log Out / Change )

Connecting to %s

Recent Post

%d bloggers like this: