Home » Posts tagged 'Direct and inverse kinematics'

# Tag Archives: Direct and inverse kinematics

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