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;
Voronoi Road Map
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 title({'Voronoi Road Map'; '* - 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);
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
Small Satellites 