0% found this document useful (0 votes)
83 views5 pages

Simulation of A Planar Three Link Robot: Hal 1 DR 5

This document describes a program that simulates a three link planar robot. It includes forward and inverse kinematics calculations to determine the robot's position based on link angles or vice versa. The program uses graphical sliders and buttons to control and visualize the robot's movement in real-time. Equations for inverse kinematics on the first two links come from a reference text, while equations for the third link were derived separately.

Uploaded by

mimin
Copyright
© © All Rights Reserved
We take content rights seriously. If you suspect this is your content, claim it here.
Available Formats
Download as PDF, TXT or read online on Scribd
0% found this document useful (0 votes)
83 views5 pages

Simulation of A Planar Three Link Robot: Hal 1 DR 5

This document describes a program that simulates a three link planar robot. It includes forward and inverse kinematics calculations to determine the robot's position based on link angles or vice versa. The program uses graphical sliders and buttons to control and visualize the robot's movement in real-time. Equations for inverse kinematics on the first two links come from a reference text, while equations for the third link were derived separately.

Uploaded by

mimin
Copyright
© © All Rights Reserved
We take content rights seriously. If you suspect this is your content, claim it here.
Available Formats
Download as PDF, TXT or read online on Scribd
You are on page 1/ 5

Hal 1 dr 5

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%% %%
%%% Simulation of a planar three link robot. %%
%%% %%
%%% The purpose of this program is to simulate a three link planar %%
%%% robot. This robot demo simulates kinematics, inverse kinematics %%
%%% and real-time graphical inverse kinematics %%
%%% To operate this program: %%
%%% 1) Make sure that demobot, forkin, invkin, setplot %%
%%% and option are in the appropriate path. %%
%%% 2) Execute demobot. %%
%%% the push button gui. %%
%%% 4) Have fun! %%
%%% The inverse kinematics equations used on the first two links %%
%%% are from Craig's Robotics. The forward kinematics equations %%
%%% and the equations used to find link angle three where derived %%
%%% by Matt Kontz. %%
%%% demobot.m This is the main program which also all the %%
%%% following programs. %%
%%% forkin.m Inputs three link angles and calculates the new %%
%%% position matrices(forward kinematics). %%
%%% invkin.m Inputs the position is spherical coordinates, does %%
%%% inverse kinematics and find link angles. %%
%%% setplot.m Uses the data calculated in forkin to update the %%
%%% figure window. %%
%%% option.m This program executes the various options: position %%
%%% sliders, click on target, click and drag and %%
%%% angles sliders. %%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
clf;clear all;
global slider1 slider2 slider3 slider4 slider5
global PushBut1 PushBut2 PushBut3 PushBut4
global x1 y1 x2 y2 x3 y3 xt yt Pt % position variable
global S1 S2 S3 S4 S5 S6 % position strings
global pF1 pF2 pF3 % handles for fill
global dis % handles for text display
global C2 C3 Ct txA tx % handles for the joint's circle
global J2 J3 Jt % handles for the joint's pluses
global L1 L2 L3 Link1 Link2 Link3 % Link matrices
global T1 T2 T3 STOP Chose % input variables
global l1 l2 l3 rmax rmin Bmax % constants
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
fig=gcf;
%%% Define Base
angle=225:15:315; % get rit of where links overlap
xa=1.125*cos(angle*pi/180); ya=1.125*sin(angle*pi/180);
xb=[xa .875 .875 2.5 2.5 -2.5 -2.5 -.875 -.875];
yb=[ya -.707107 -1.25 -1.25 -2.875 -2.875 -1.25 -1.25 -.707107];
B=[xb' yb' zeros(size(xb))' ones(size(xb))']'; % Link3 matrix
%%% Link1 and Link2
angle=265:-15:105; % define an arc from 270 to 90 degree r=1.125
xa=1.125*cos(angle*pi/180); ya=1.125*sin(angle*pi/180);
angle=150:15:210; % get rit of where links overlap
xc=8.625+1.125*cos(angle*pi/180);
yc=1.125*sin(angle*pi/180);
xL1=[0 7.1875 7.1875 7.72978 xc 7.72978 7.1875 7.1875 0 xa 0];
yL1=[1.125 1.125 .875 .68133 yc -.68133 -.875 -1.125 -1.125 ya 1.125];
Link1=[xL1' yL1' zeros(size(xL1))' ones(size(xL1))']'; % Link1 matrix
Link2=Link1; % Link1 and Link2 are the same
Hal 2 dr 5

%%% Link3
xL3=[0 2.875 3.125 3.125 4.875 6.625 6.6258 4.875 3.125 3.125 2.875 0 xa 0];
yL3=[1.125*ones(1,3) .875 .875 .25 -.25 -.875 -.875 -1.125*ones(1,3) ya 1.125];
Link3=[xL3' yL3' zeros(size(xL3))' ones(size(xL3))']'; % Link3 matrix
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
l1=8.625; % distance between frame '1' and '2'
l2=l1; % distance between frame '2' and '3'
l3=6.125; % distance between frame '3' and 'tool'
rmax=l1+l2+l3; % maximum distance between (0,0) and tool frame
rmin=(l2^2+(l1-l3)^2)^0.5; % minimum distance between (0,0) and tool frame
Bmax=atan2(l1-l3,l2)+pi/2;
x0=0; % x position of frame '0'
y0=0; % y position of frame '0'
x1=x0; % x position of frame '1'
y1=y0; % y position of frame '1'
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
r_i=rmax; % initial radius
psi_i=90*pi/180; % initial angle
T1=0;T2=0;T3=0; forkin
r=rmax; psi=pi/2;
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
tx(1)=text(29.7,-11,num2str(rmin),'HorizontalAlignment','right');
tx(2)=text(29.7,-4,'Radius','HorizontalAlignment','right');
tx(3)=text(29.7,3,num2str(0.1*round(10*rmax)),'HorizontalAlignment','right');
tx(4)=text(29.7,11,'0^o','HorizontalAlignment','right');
tx(5)=text(29.7,18,'Angle','HorizontalAlignment','right');
tx(6)=text(29.7,25,[num2str(180),'^o'],'HorizontalAlignment','right');
set(tx,'visible','off') % sets label on slider and turns off
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
sr = ['r = get(gco,''Value'');' ... % defines the new radius slider value
'invkin(r,psi);' ...
'forkin;' ...
'setplot;'];% ... % calls setplot to figure
slider4=uicontrol(fig,'Style','slider','Units','normalized', ...
'Position',[0.96 0.1 0.03 0.35],'min',rmin,'max',rmax, ...
'Value',r_i,'Callback',sr,'visible','off','BackgroundColor',[1 1 1]);
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
sp = ['psi = get(gco,''Value'');' ... % defines teh new angle slider values
'invkin(r,psi);' ...
'forkin;' ...
'setplot;'];% ... % calls setplot to figure
slider5=uicontrol(fig,'Style','slider','Units','normalized', ...
'Position',[0.96 0.55 0.03 0.35],'min',0,'max',pi, ...
'Value',psi_i,'Callback',sp,'visible','off','BackgroundColor',[1 1 1]);
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
txA(1)=text(29,17.25,'-90','HorizontalAlignment','right');
txA(2)=text(29,22.25,'\theta_{3}','HorizontalAlignment','right');
txA(3)=text(29,27.25,'90','HorizontalAlignment','right');
txA(4)=text(29,1.5,'-90','HorizontalAlignment','right');
txA(5)=text(29,6.5,'\theta_{2}','HorizontalAlignment','right');
txA(6)=text(29,11.5,'90','HorizontalAlignment','right');
txA(7)=text(29,-14.25,'-90','HorizontalAlignment','right');
txA(8)=text(29,-9.25,'\theta_{1}','HorizontalAlignment','right');
txA(9)=text(29,-4.25,'90','HorizontalAlignment','right');
set(txA,'visible','off') % sets label on slider and turns off
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
s3 = ['T3 = get(gco,''Value'');' ... % defines T3 as slider value
'forkin;' ...
'setplot;']; % calls setplot to figure
slider3=uicontrol(fig,'Style','slider','Units','normalized', ...
'Position',[0.95 0.7 0.03 0.25],'min',-90,'max',90, ...
Hal 3 dr 5

'Value',0,'Callback',s3,'visible','off','BackgroundColor',[.2 .2 .8]);
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
s2 = ['T2 = get(gco,''Value'');' ... % defines T2 as slider value
'forkin;' ...
'setplot;']; % calls setplot to figure
slider2=uicontrol(fig,'Style','slider','Units','normalized', ...
'Position',[0.95 0.375 0.03 0.25],'min',-90,'max',90, ...
'Value',0,'Callback',s2,'visible','off','BackgroundColor',[.8 .9 .2]);
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
s1 = ['T1 = get(gco,''Value'');' ... % defines T1 as slider value
'forkin;' ...
'setplot;']; % calls setplot to figure
slider1=uicontrol(fig,'Style','slider','Units','normalized', ...
'Position',[0.95 0.05 0.03 0.25],'min',-90,'max',90, ...
'Value',0,'Callback',s1,'visible','off','BackgroundColor',[.2 .8 .2]);
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
P1 = ['option(1);'];
PushBut1=uicontrol(fig,'Style','pushbutton','Units','normalized', ...
'Position',[0.10 .02 0.17125 0.05],'string','Position-Sliders', ...
'Callback',P1,'visible','on','BackgroundColor',[0.8 0.8 0.8]);
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
P2 = ['option(2,0)'];
PushBut2=uicontrol(fig,'Style','pushbutton','Units','normalized', ...
'Position',[0.30125 .02 0.17125 0.05],'string','Click on Target', ...
'Callback',P2,'visible','on','BackgroundColor',[0.8 0.8 0.8]);
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
P3 = ['option(3);'];
PushBut3=uicontrol(fig,'Style','pushbutton','Units','normalized', ...
'Position',[0.5025 .02 0.17125 0.05],'string','Click and Drag', ...
'Callback',P3,'visible','on','BackgroundColor',[0.8 0.8 0.8]);
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
P4 = ['option(4)'];
PushBut4=uicontrol(fig,'Style','pushbutton','Units','normalized', ...
'Position',[0.70375 .02 0.17125 0.05],'string', 'Angle-Sliders', ...
'Callback',P4,'visible','on','BackgroundColor',[0.8 0.8 0.8]);
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
P5 = ['close;'];
PushBut5=uicontrol(fig,'Style','pushbutton','Units','normalized', ...
'Position',[0.1 .92 0.17 0.05],'string','EXIT', ...
'Callback',P5,'visible','on','BackgroundColor',[0.8 0.8 0.8]);
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
HELP=0;
P6 = ['if HELP==0;' ...
'set(Help,''visible'',''on'');' ...
'set(Help_mes,''visible'',''on'');' ...
'set(pF1,''visible'',''off'');' ...
'set(pF2,''visible'',''off'');' ...
'set(pF3,''visible'',''off'');' ...
'set(C2,''visible'',''off'');' ...
'set(C3,''visible'',''off'');' ...
'set(Ct,''visible'',''off'');' ...
'set(J2,''visible'',''off'');' ...
'set(J3,''visible'',''off'');' ...
'set(Jt,''visible'',''off'');' ...
'set(dis,''visible'',''off'');' ...
'set(PushBut6,''string'',''HIDE HELP'');' ...
'HELP=1;' ...
'else;' ...
'set(Help,''visible'',''off'');' ...
'set(Help_mes,''visible'',''off'');' ...
'set(pF1,''visible'',''on'');' ...
Hal 4 dr 5

'set(pF2,''visible'',''on'');' ...
'set(pF3,''visible'',''on'');' ...
'set(C2,''visible'',''on'');' ...
'set(C3,''visible'',''on'');' ...
'set(Ct,''visible'',''on'');' ...
'set(J2,''visible'',''on'');' ...
'set(J3,''visible'',''on'');' ...
'set(Jt,''visible'',''on'');' ...
'set(dis,''visible'',''on'');' ...
'set(PushBut6,''string'',''HELP'');' ...
'HELP=0;' ...
'end;'];
PushBut6=uicontrol(fig,'Style','pushbutton','Units','normalized', ...
'Position',[0.70375 .92 0.17 0.05],'string','HELP', ...
'Callback',P6,'visible','on','BackgroundColor',[0.8 0.8 0.8]);
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
figure(1); title('Three Link Planar Robot','FontSize',14)
axis([-25 25 -3 25]) % axis limits
axis manual % set axis to exact manual value(i.e [-25 25 -10 25])
axis equal % x-scale=y-scale
hold on % does not erase previous graphs
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
arc=(0:1:180)*pi/180; % plot desired workspace
arc2=(180:-1:0)*pi/180;
plot([rmax*cos(arc) rmin*cos(arc2) rmax],[rmax*sin(arc) rmin*sin(arc2) 0], ...
'Color',[.8 .4 .2]); legend('Workspace')
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
grid off % turns on grid
% manual grid so that axis will be black and grid gray
for y=0:1:24 % minor grid lines
plot([-24.9 25],[y y],'Color',[.97,.97,.97])
end
for x=-24:1:24
plot([x x],[0 25],'Color',[.97,.97,.97])
end
for y=0:5:20 % major grid lines
plot([-24.9 25],[y y],'Color',[0.9 0.9 0.9])
end
for x=-20:5:20
plot([x x],[0 25],'Color',[.9,.9,.9])
end
plot([rmax*cos(arc) rmin*cos(arc2) rmax],[rmax*sin(arc) rmin*sin(arc2) 0], ...
'Color',[.8 .4 .2]); pos=[15,20]; lg=legend('Workspace',1); % plot workspace or grid
set(lg,'Position',[0.66 0.815 0.203571 0.0492857])
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
set(gca,'GridLineStyle','-') % solid lines
set(gca,'YColor',[0,0,0]) % y axis color
set(gca,'XColor',[0,0,0]) % y axis color
set(gca,'XTick',[-25:5:25]) % numbers on y-axis
set(gca,'YTick',[0:5:25]) % numbers onf x-axis
set(gca,'Color',[1,1,1]) % plot background color
set(gca,'FontSize',8);
set(gcf,'Color',[.95,.95,.95])% edge background color
set(gca,'Position',[0.10 0.11 0.775 0.815]) % size of data windown
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
BF=fill(B(1,:),B(2,:),'r'); % color fill base
set(BF,'FaceColor',[.8 .3 .3]);
pF1=fill(L1(1,:),L1(2,:),'g', 'erasemode','xor'); % color fill link1
set(pF1,'FaceColor',[.2 .8 .2]);
pF2=fill(L2(1,:),L2(2,:),'y', 'erasemode','xor'); % color fill link2
set(pF2,'FaceColor',[.8 .9 .2]);
Hal 5 dr 5

pF3=fill(L3(1,:),L3(2,:),'b','erasemode','xor'); % color fill link3


set(pF3,'FaceColor',[.2 .2 .8]);
plot(x1,y1,'om'); % circle at joint '1'
C2=plot(x2,y2,'ob', 'erasemode','xor'); % circle at joint '2'
C3=plot(x3,y3,'oy', 'erasemode','xor'); % circle at joint '3'
Ct=plot(xt,yt,'oy', 'erasemode','xor'); % circle at joint 'T'
plot(x1,y1,'+m'); % plus at joint '1'
J2=plot(x2,y2,'+b', 'erasemode','xor'); % plus at joint '2'
J3=plot(x3,y3,'+y', 'erasemode','xor'); % plus at joint '3'
Jt=plot(xt,yt,'+y', 'erasemode','xor'); % plus at joint 'T'
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
dis(1)=fill([-15.5 -15.5 15.5 15.5 -15.5],[-9 -4 -4 -9 -9],'w'); % plot a white box
dis(2)=plot([-15.5 -15.5 15.5 15.5 -15.5],[-9 -4 -4 -9 -9],'k'); % box's black outline
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
yrange = axis;
vspace = (yrange(4) - yrange(3))/20;
px=-14;
py=-5.5;
dis(3)=text(px, py, mat2str(S1),'erasemode','xor'); % theta 1
dis(4)=text(px,(py-1.2*vspace),mat2str(S2),'erasemode','xor'); % xt
dis(5)=text(px+11, py, mat2str(S3),'erasemode','xor'); % theta2
dis(6)=text(px+11,(py-1.2*vspace),mat2str(S4),'erasemode','xor'); % yt
dis(7)=text(px+21, py, mat2str(S5),'erasemode','xor'); % theta3
dis(8)=text(px+21,(py-1.05*vspace),mat2str(S6),'erasemode','xor'); % phi
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
px=-24;
py=24;
text(px, py,'Matthew Kontz','FontSize',8);
text(px,(py-1*vspace),'Walla Walla College','FontSize',8);
text(px,(py-2*vspace),'February 2001','FontSize',8);
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
set(gco,'BackingStore','off') % for realtime inverse kinematics
set(gco,'Units','data')
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
Help(1)=fill([-20 20 20 -20 -20 ],[20 20 -10 -10 20],'w'); % plot a white box
Help(2)=plot([-20 20 20 -20 -20 ],[20 20 -10 -10 20],'k'); % box's black outline
set(Help,'visible','off')
Help_mes(1)=text(-19,19,'There are four different ways you can manipulate this');
Help_mes(2)=text(-19,17,'three link planar robot. Here are your choices.');
Help_mes(3)=text(-19,15,'Position sliders: For this option you can change r and \phi ');
Help_mes(4)=text(-19,13,'by moving the sliders. (r and \phi are the polar coordinate');
Help_mes(5)=text(-19,11,'equivalent of x and y in Cartesian coordinates.)');
Help_mes(6)=text(-19,9,'Click on Target: You click on any point inside the work-');
Help_mes(7)=text(-19,7,'space and watch the robot move into that position.');
Help_mes(8)=text(-19,5,'Click and Drag: Simply hold down your mouse button');
Help_mes(9)=text(-19,3,'and drag the end of the robot around the workspace');
Help_mes(10)=text(-19,1,'for real-time graphical inverse kinematics.');
Help_mes(11)=text(-19,-1,'Angle sliders: This allow you to manually change each');
Help_mes(12)=text(-19,-3,'link and watch how the robot moves.');
Help_mes(13)=text(-19,-9,'To make a selection, click on one of the buttons below.');
Help_mes(14)=text(-19,-5,'NOTE: \theta_{1},\theta_{2}, and \theta_{3} are joints space
angles. X_{t},Y_{t},');
Help_mes(15)=text(-19,-7,'and \phi_{t} are the position and angle of the tool frame.');
set(Help_mes,'visible','off')

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% SELESAI... %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

You might also like