Home » Posts tagged 'Robotics'

# Tag Archives: Robotics

## Distance Transform Path Planning Algorithm

clc;close all;clear all;

Input Map

```RGB = imread('Mapn.png');
figure('Position',[600 0 600 1000],'color','k');
image(RGB);
hold on;
axis off
axis image
hold off;
% Convert to binary image matrix and inverse matrix values
I = rgb2gray(RGB);
binaryImage = im2bw(RGB, 0.3);
binaryImage = 1-binaryImage;
% Inital pose
xs = 540; ys = 750;
% Goal Pose
xg = 150; yg = 150;```

Distance to Goal

```sz  = size(binaryImage);
for i = 1:sz(1)
for j = 1:sz(2)
rg(i,j) = sqrt((i-yg)^2 +(j - xg)^2);
end
end
% Pose Matrix
pose = zeros(sz(1),sz(2));
pose(ys,xs) = 1;
pose(yg,xg) = 1;```
```Di = 15; % Distance of influence of the obstacles
% Compute for every pixel the distance to the nearest obstacle(non zero
% elements which after inversion corespondent to the obstacles)
[D,IDX] = bwdist(binaryImage);
for i = 1:sz(1)
for j = 1:sz(2)
rd = D(i,j);
if (rd <= Di)
rg(i,j)  = 1e6;
end
end
end```

Distance Transform Path Planning Algorithm

```figure('Position',[600 0 600 1000],'color','k');
hold on;
image(RGB);
colormap gray
contour(rg,15);
spy(pose,'*r');
axis off
axis image
title('Distance Transform Path Planning Algorithm','color','w');
x = xs; y = ys;
last = rg(y-1,x-1);
while (x ~= xg)||(y ~= yg)
dis =[ rg(y-1,x-1), rg(y-1,x),rg(y-1,x+1);
rg(y,x-1), rg(y,x) ,rg(y,x+1);
rg(y+1,x-1), rg(y+1,x),rg(y+1,x+1)];
m = min(dis(:));
[r,c] = find(dis == m);
rg(y,x) = 1E6;
y = y-2+r;
x = x-2+c;
pose(y,x) = 1;
end
spy(pose,'.r');
set(gcf, 'InvertHardCopy', 'off');
hold off;```

## Voronoi Road Map, Path Planing

clc;close all;clear all;

Input Map

```RGB = imread('Map.png');
figure('Position',[100 0 600 700],'color','k');
imagesc(RGB);
axis image
title('Input Map','color','w');
axis off
set(gcf, 'InvertHardCopy', 'off');
hold off;```

Convert to binary image matrix and inverse matrix values

```I = rgb2gray(RGB);
binaryImage = im2bw(RGB, 0.3);
sz  = size(binaryImage);```

Inital pose

```xs = 225; ys = 355;
% Goal Pose
xg = 50; yg = 50;
% Pose Matrix
pose = zeros(sz(1),sz(2));
pose(ys,xs) = 1;
pose(yg,xg) = 1;```

```figure('Position',[100 0 600 700],'color','k');
hold on;
set(gcf, 'InvertHardCopy', 'off');
sk = 1;
for i = 1:sz(1)
for j = 1:sz(2)
if(binaryImage(i,j) == 1 )
xv(sk) = j;
yv(sk) = i;
sk = sk+1;
end
end
end
[vrx,vry]  = voronoi(xv,yv);
set(gca,'YDir','Reverse');
plot(xv,yv,'y.',vrx,vry,'b-');
axis tight
%axis([1 sz(2) 1 sz(1) ]) ;
axis image
'* - Represents Starting and Goal Position '},'color','w');
axis off
spy(pose,'*r');```

Robot Path

```x = xs; y = ys;
indk = 1;
while (abs(x - xg) > 5)||(abs(y - yg)> 5)
path(:,indk) = [x y];
dist = sqrt((vrx - x).^2+(vry - y).^2);
goal = sqrt((xg - x).^2+(xg - y).^2);
% Find closest vertices of the Voronoi edges
[mn ind] = min(dist(:));
xt = vrx(ind);
yt = vry(ind);
goalj = sqrt((xg - xt).^2+(xg - yt).^2);
if (goalj < goal )
x = xt;
y = yt;
vrx(ind) = 1E6;
vry(ind) = 1E6;
indk = indk +1;
else
vrx(ind) = 1E6;
vry(ind) = 1E6;
end
end
path(:,indk) = [xg yg];
plot(path(1,:),path(2,:),'-r');
title({'Voronoi Road Map and Robot Path';
'* - Represents Starting and Goal Position '},'color','w');```

## Potential Field Path Planning

clc;close all;clear all;

Input Map

```RGB = imread('Mapn.png');
figure('Position',[600 0 600 1000],'color','k');
image(RGB);
hold on;
axis off
axis image```

Convert to binary image matrix and inverse matrix values

```I = rgb2gray(RGB);
binaryImage = im2bw(RGB, 0.3);
binaryImage = 1-binaryImage;
% Inital pose
xs = 540; ys = 750;
% Goal Pose
xg = 150; yg = 150;```

Attractive potential

```sz  = size(binaryImage);
for i = 1:sz(1)
for j = 1:sz(2)
rg = sqrt((i-yg)^2 +(j - xg)^2);
U_att(i,j) = 0.5*rg^2;
end
end
contour(U_att,40);

set(gcf, 'InvertHardCopy', 'off');
% Pose Matrix
pose = zeros(sz(1),sz(2));
pose(ys,xs) = 1;
pose(yg,xg) = 1;
spy(pose,'*r');
title({'Attractive Potential Field';...
'Circles Center Corespondents to the Goal Position '},'color','w');
hold off;```

Repulsive potential

```Di = 35; % Distance of influence of the object
% Compute for every pixel the distance to the nearest obstacle(non zero
% elements which after inversion corespondent to the obstacles)
[D,IDX] = bwdist(binaryImage);
for i = 1:sz(1)
for j = 1:sz(2)
rd = D(i,j);
if (rd <= Di)
U_rep(i,j)  = 0.5*(rd - Di)^2;
else
U_rep(i,j)  = 0;
end
end
end
figure('Position',[600 0 600 1000],'color','k');
hold on;
colormap jet;
contourf(U_rep,5);
spy(pose,'*r');
axis off
axis image
set(gcf, 'InvertHardCopy', 'off');
title('Repulsive Potential Field','color','w');
hold off;```

Summary Potential Field

```k_s  = 2; % Scaling factor
U_sum = U_rep/max(U_rep(:))+k_s*U_att/max(U_att(:));
figure('Position',[600 0 600 1000],'color','k');
hold on;
contourf(U_sum,15);
spy(pose,'*r');
axis off
axis image
title('Summary Potential Field','color','w');
set(gcf, 'InvertHardCopy', 'off');
hold off;```

Potential Field Path Planning

```figure('Position',[600 0 600 1000],'color','k');
hold on;
contourf(U_sum,15);
spy(pose,'*r');
axis off
axis image
title('Potential Field Path Planning,*Note the Potential Trap','color','w');
x = xs; y = ys;
last = U_sum(y-1,x-1);
while (x ~= xg)||(y ~= yg)
dis =[ U_sum(y-1,x-1), U_sum(y-1,x),U_sum(y-1,x+1);
U_sum(y,x-1), U_sum(y,x) ,U_sum(y,x+1);
U_sum(y+1,x-1), U_sum(y+1,x),U_sum(y+1,x+1)];
m = min(dis(:));
[r,c] = find(dis == m);
U_sum(y,x) = inf;
y = y-2+r;
x = x-2+c;
pose(y,x) = 1;
end
spy(pose,'.r');
set(gcf, 'InvertHardCopy', 'off');
hold off;```

## Dynamics simulator program for the robot

clear all; clc; close all;

```% Characteristics of the Robot
m1 = 10;  % kg
m2 = 5;   % kg
d1 = 200; % mm
l2 = 400; % mm
w0 = [-100;-200;200;0;0;-1];
w1 = [200;150;200;0;0;-1];
T  = 15;   % s
dt = 0.01; % s
t = 0:dt:T;```

Function sddot(t, T) is the speed profile to be used for that motion in tool-configuration space is a linear acceleration profile and indirectly given by

```a0 = 6/T^2;
sddot = a0 - 2*a0*t/T;```

Function sdot(t, T) delivering the value of the integral of speed profile

```sz = size(t);
sdot(1) = 0;
for i =1:sz(2)-1;
sdot(i+1) = sdot(i) + dt*sddot(i);
end```

Function s(t, T)

```s(1) = 0;
for i =1:sz(2)-1;
s(i+1) = s(i) + dt*sdot(i);
end```

Function w(t, T, w0, w1) delivering the tool-configuration position at time t of the straight line between w0 and w1 Function wdot(t, T, w0, w1) delivering the tool-configuration velocity at time t of the straight line between w0 and w1,computed using function sdot(t, T) Function wddot(t, T, w0, w1) delivering the tool-configuration acceleration at time t of the straight line between w0 and w1, computed using function sddot(t, T)

```for i = 1:sz(2)
w_des(:,i)      = (1 - s(i))*w0 + s(i)*w1;
wdot_des(:,i)   = sdot(i)*(w1-w0);
wddot_des(:,i)  = sddot(i)*(w1-w0);
end```

Function qw(w), yielding the vector of joint variables given a tool-configuration position (i.e. solution of inverse kinematics)

```for i = 1:sz(2)
q_des(1,i) = atan2(-w_des(1,i),w_des(2,i));
%q_des(2,i) = w_des(2,i)/cos(q_des(1,i)); % vers1
q_des(2,i) = sqrt(w_des(1,i)^2+w_des(2,i)^2);
end```

Function qdot(w, wdot), computing the vector of joint velocities given a tool-configuration position and velocity Function qddot(w, wdot, wddot), computing the vector of joint accelerations given a tool-configuration position, velocity and acceleration

```for i = 1:sz(2)
qdot(1,i) = (w_des(1,i)*wdot_des(2,i)-w_des(2,i)*wdot_des(1,i))/...
(w_des(1,i)^2+w_des(2,i)^2);
qdot(2,i) = (w_des(1,i)*wdot_des(1,i)+w_des(2,i)*wdot_des(2,i))/...
sqrt(w_des(1,i)^2+w_des(2,i)^2);
w1 = w_des(1,i);
w2 = w_des(2,i);
wdt1 = wdot_des(1,i); wdt2 = wdot_des(2,i);
wddt1 = wddot_des(1,i);wddt2 = wddot_des(2,i);
qddot(1,i) = ((w1^2+w2^2)*(w1*wddt2-w2*wddt1)-2*(w1*wdt2-w2*wdt1)*...
(w1*wdt1+w2*wdt2))/((w1^2+w2^2)^2);
qddot(2,i) = ((w1^2+w2^2)*(w1*wddt1+w2*wddt2)+(w1*wdt2-w2*wdt1)^2)/...
((w1^2+w2^2)^1.5);
end```

Function torques(q, qdot, qddot), computing the vector of torques given the vectors of joint-space positions q, velocities qdot and accelerations qddot, equations of motion, direct dynamics

```for i = 1:sz(2)
% torques(q, qdot, qddot)
trq1(i) = m2*(l2^2/3 - l2*q_des(2,i)+q_des(2,i)^2)*qddot(1,i)  +...
m2*(2*q_des(2,i)-l2)*qdot(1,i)* qdot(2,i);
trq2(i) = m2*qddot(2,i)+m2*(l2/2 - q_des(2,i))*qdot(1,i)^2;
%
end
q_cur    = [q_des(1,1); q_des(2,1)];
qdot_cur = [0;0];
for i = 1:sz(2)
tau   = [trq1(i) trq2(i)];
ct = t(i); % Current time
[t0, y0] = ode45(@(ct,y) dyns(ct,y,tau), [ct,ct+dt],...
[q_cur(1); qdot_cur(1); q_cur(2); qdot_cur(2)]);
rs = size(y0);
q_cur = [y0(rs(1),1);y0(rs(1),3)];
q_curv(i,:) = q_cur;
qdot_cur = [y0(rs(1),2);y0(rs(1),4)];
% Function wq(q), yielding the tool-configuration position for  vector
% of  joint  variables q,   direct kinematics
% Given  q = [q1 ,q2], return w
w_cur = [-sin(q_cur(1))*q_cur(2); cos(q_cur(1))*q_cur(2); d1];
% fprintf('[ %6.4f %6.4f  %6.4f ] \n',w_cur);
w_curv(i,:) = w_cur;
end
% Plots
figure('Position',[200 0 600 600])
subplot(2,1,1);
hold on;
plot(t,q_curv(:,1)*180/pi ,'r');
xlabel('Time[s]');
ylabel('q1(t)[deg]');
grid on;
subplot(2,1,2);
plot(t,q_curv(:,2),'b');
xlabel('Time[s]');
ylabel('q2(t)[mm]');
grid on;
figure('Position',[200 0 600 600])
subplot(2,1,1);
plot(t,w_curv(:,1),'r');
xlabel('Time[s]');
ylabel('w1(t)[mm]');
grid on;
subplot(2,1,2);
plot(t,w_curv(:,2),'b');
xlabel('Time[s]');
ylabel('w2(t)[mm]');
grid on;```

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