mae 593 final projectreport
TRANSCRIPT
MAE 593
MATHEMATICAL METHODS IN ROBOTICS
FINAL PROJECT REPORT
P-R-R-R SERIAL MANIPULATOR
2R-R-R CHAIN AND 1P-R-R CHAIN PARALLEL MANIPULATOR
SUBMITTED BY –
NIKHIL SAPRE
TEJAS PUNTAMBEKAR
1
INDEX
Sr.No Description Pg.No
1 Project Objective 3
2 P-R-R-R Serial Manipulator Overview 3
3 Basic Features 4
4 Parallel Manipulator (2RRR Chains and 1PRR Chain) 5
5 The User Interface - Serial Manipulator 6
6 Workspace plotting 7
7 Forward Kinematics 8
8 Inverse Kinematics 9
9 Ellipse Tracing 10
10 Circle Tracing 11
11 Curve Tracing 12
12 Error Messages 14
13 MatLab Program Codes - Serial Manipulator 15
14 The User Interface 20
15 MatLab Program Codes -Parallel Manipulator 21
2
OBJECTIVE :-
To implement a P-R-R-R Serial Manipulator and explore its functionality by providing a Graphical User Interface in MatLab.
To implement a parallel manipulator with 2 R-R-R chains and 1 P-R-R chain and to explore its functionality using Graphical User Interface in MatLab.
A. P-R-R-R Serial Manipulator :- Overview
L1 = Length of Link No.1
L2 = Length of Link No.2
L3 = Length of Link No.3
d1 = Movement of the prismatic joint P.
Theta1 = Absolute angle for link No.1.
Theta2 = Absolute angle for link No.2.
3
Theta3 = Absolute angle for link No.3.
Xe = X Co-ordinate of End Effector.
Ye = Y Co-ordinate of End Effector.
BASIC FEATURES :-
Base Position fixed to (0,0)
Initial “Home Position” of the manipulator is fixed at the origin.
Can Slide along X-Axis from -50 to 50
This movement along X direction is possible due to prismatic joint P.
User defined Link Lengths (5-50)
User can set the values for lengths of the links 1,2,3 using the scroll bar.
Task Space Configuration
Join Space Configuration
Plot WorkSpace
Trace Ellipse/Circle
Trace Curve
Return to Home Position
4
B.Parallel Manipulator (2RRR Chains and 1PRR Chain)
a11 = Length of link 1 of manipulator chain no.1.
a21 = Length of link 2 of manipulator chain no.1.
a12 = Length of link 1 of manipulator chain no.2.
a22 = Length of link 2 of manipulator chain no.2.
a13 = Length of link 1 of manipulator chain no.3.
a23 = Length of link 2 of manipulator chain no.3.
thetaj1 ,for j=1,2,3 ,absolute angles made by all the 3 links of manipulator chain 1
thetaj2 ,for j=1,2,3 ,absolute angles made by all the 3 links of manipulator chain 2
thetaj3 ,for j=1,2,3 ,absolute angles made by all the 3 links of manipulator chain 3.
theta11 , theta12 , theta13 = base angles in degrees.
theta21,theta23 ,theta22 = joint angles.
Xee,Yee = End effector co-ordinates.
Phi = angle made by end effector with the horizontal.
5
D = Movement of prismatic joint P.
SERIAL P-R-R-R MANIPULATOR
THE USER INTERFACE
BASIC FUNCTIONALITY
A.HOME POSITION
“Home” Returns the manipulator to the configuration as shown above with all the link lengths equal to 25 and joint angles of zero degree. Movement ‘D1’ of the prismatic joint is set to zero initially.
The end effector co-ordinates are Xee = 75 and Yee = 0 as displayed above under inverse kinematics.
6
B.WORKSPACE PLOTTING
Push Button :- WORKSPACE
“WORKSPACE” plots the ‘region of reach’ for the manipulator using Newton Raphson Method.
The green space displayed above denotes the feasible region or workspace for the PRR manipulator.
7
C.FORWARD KINEMATICS
FORWARD KINEMATICS :-
FUNCTION :-
Allows user to select the values of joint angles , theta1, theta2, theta3 in degrees and set the movement of prismatic joint ‘D1’.
Theta1 , theta2 and theta3 are all absolute angles and range from 0 to 360 degrees. Movement D1 of prismatic joint can be varied from -50 to 50 along X-axis. The function evaluates forward kinematics for the manipulator and displays the values of End
Effector co-ordinates (Xee,Yee). User is required to press “Update Forward Kinematics” button to allow the changes in robot
position to take place in accordance with the entered values of joint variables.
8
D.INVERSE KINEMATICS
INVERSE KINEMATICS :-
FUNCTION :-
Allows user to select the values of End Effector Co-ordinates (Xee,Yee) and it displays the corresponding values of joint angles ( theta1, theta2, theta3 in degrees) and prismatic joint movement ‘D1’.
Theta1 , theta2 and theta3 are all absolute angles and range from 0 to 360 degrees. Movement D1 of prismatic joint can be varied from -50 to 50 along X-axis. User is required to press “Update Inverse Kinematics” button to allow the changes in robot
position to take place in accordance with the entered values of end effector co-ordinates.
E.ELLIPSE TRACING
9
FUNCTION :-
Ellipse
Allows user to select the values of Major axis and Minor axis of an ellipse along with the X and Y co-ordinates of its center.
Values of Major axis and Minor axis can be varied from 10 to 50. Values of Center co-ordinates(X,Y) range from -25 to 25.
F.CIRCLE TRACING
10
FUNCTION :-
Plots a circle of defined Radius = Major Axis = Minor Axis and Center point co-ordinates.
G.CURVE TRACING
11
12
CURVE TRACING ALONG X-X DIRECTION
CURVE TRACING ALONG Y-Y DIRECTION
FUNCTION :-
Allows the user to select a set of 5 points anywhere on the screen by mouse-click. The curve passing through the user-defined points is plotted by using ‘Interp1’ function. Displays Error Messages for infeasible points.
H.ERROR MESSAGES
13
CURVE TRACING ALONG X-Y DIRECTION
An error Message “Curve does not lie in Workspace” is displayed when any one of the user-selected point is outside the reach of the manipulator i.e. outside workspace.
Error Message “Points are too close to each other cannot trace curve” is displayed when user selects the points which cannot be effectively interpolated.
PROGRAM CODES
14
Following are the important function codes that are incorporated in the GUI.
Home Postion
function Home_Callback(hObject, eventdata, handles)% hObject handle to Home (see GCBO)% eventdata reserved - to be defined in a future version of MATLAB% handles structure with handles and user data (see GUIDATA)hold off;botplot(25,25,25,0,0,0,0)set(handles.D1edit,'String',0); set(handles.Theta1edit,'String',0); set(handles.Theta2edit,'String',0); set(handles.Theta3edit,'String',0); set(handles.L1edit,'String',25); set(handles.L2edit,'String',25); set(handles.L3edit,'String',25); set(handles.Xee,'String',75); set(handles.Yee,'String',0); set(handles.D1,'Value',0.5);set(handles.L1,'Value',0.5);set(handles.L2,'Value',0.5);set(handles.L3,'Value',0.5);set(handles.Theta1,'Value',0);set(handles.Theta2,'Value',0);set(handles.Theta3,'Value',0);set(handles.majorAedit,'String',10); set(handles.minorAedit,'String',10); set(handles.majorA,'Value',0); set(handles.minorA,'Value',0); set(handles.Cx,'Value',0.5); set(handles.Cy,'Value',0.5); set(handles.Cxedit,'String',0); set(handles.Cyedit,'String',0);
Forward Kinematics
function X= BotFwdKin(L1,L2,L3,d1,Theta1,Theta2,Theta3)Xee=d1+L1*cosd(Theta1)+L2*cosd(Theta2)+L3*cosd(Theta3);Yee=L1*cosd(Theta1)+L2*cosd(Theta2)+L3*cosd(Theta3);X=[Xee,Yee];
Inverse Kinematics
15
function [inv] = InvKin(l1,l2,l3,Xee,Yee,d,th1,th2,th3)L1=l1;L2=l2;L3=l3;theta1=th1;theta2=th2;theta3=th3;d1=d; check=0;n=1;Delta= [1 1 1 1]';while(abs(Delta) >0.0001 & n<1000) while(d1>50) d1=d1-50; end while d1<-50 d1=d1+50; endc1=cosd(theta1);c2=cosd(theta2);c3=cosd(theta3);s1=sind(theta1);s2=sind(theta2);s3=sind(theta3);f1=d1+L1*c1+L2*c2+L3*c3 - Xee;f2=L1*s1+L2*s2+L3*s3 -Yee; J=[ 1 -L1*s1 -L2*s2 -L1*s3; 0 L1*c1 L2*c2 L3*c3]; Delta= -pinv(J)*[f1 f2]';d1=d1+Delta(1);theta1=theta1+Delta(2);theta2=theta2+Delta(3);theta3=theta3+Delta(4);n=n+1;endc1=cosd(theta1);c2=cosd(theta2);c3=cosd(theta3);s1=sind(theta1);s2=sind(theta2);s3=sind(theta3);f1=d1+L1*c1+L2*c2+L3*c3 - Xeef2=L1*s1+L2*s2+L3*s3 -Yeeif(abs(f1)>3 && abs(f2)>3 || abs(d1)>50) chk=0;else chk=1;end while(theta1>360) theta1=theta1-360;endwhile(theta1<-360) theta1=theta1+360;end
16
while(theta2>360) theta2=theta2-360;endwhile(theta2<-360) theta2=theta2+360;end while(theta3>360) theta3=theta3-360;endwhile(theta3<-360) theta3=theta3+360;end if (theta1<0) theta1=360-theta1;end if (theta2<0) theta2=360-theta2;end if (theta3<0) theta3=360-theta3;endinv=[d1 theta1 theta2 theta3 chk];end
Ellipse Tracing
function [InvKin]=Ellipsetracing(Xc,Yc,a,b)theta=0:5:380;Axis([-150 150 -150 150]);Xee=Xc+ a*cosd(theta);Yee=Yc+ b*sind(theta);InvKin= [Xee Yee]';plot(Xee,Yee,'-r',Xc,Yc,'o') grid on;end
Curve Tracing
function TraceCurve_Callback(hObject, eventdata, handles)
17
hold off;[XEE,YEE] = ginput(5);if(abs(max(XEE)-min(XEE))>10)X1=min(XEE);X2=max(XEE);X1I=X1:X2;Y1I=Interp1(XEE,YEE,X1I,'cubic');trace=1;elseif(abs(max(YEE)-min(YEE))>10)Y1=min(YEE);Y2=max(YEE);Y1I=Y1:Y2;X1I=Interp1(YEE,XEE,Y1I,'cubic');trace=1; else msgbox('Points are to close too each other cannot trace the curve','Interplation Error') trace=0;endif(trace~=0) L1=round(str2double(get(handles.L1edit,'String')));L2=round(str2double(get(handles.L2edit,'String')));L3=round(str2double(get(handles.L3edit,'String')));th1=round(str2double(get(handles.Theta1edit,'String')));th2=round(str2double(get(handles.Theta2edit,'String')));th3=round(str2double(get(handles.Theta3edit,'String')));d=round(str2double(get(handles.D1edit,'String'))); for i=1:1:length(X1I)inv=InvKin(L1,L2,L3,X1I(i),Y1I(i),d,th1,th2,th3)if(inv(5)==1) d=inv(1); set(handles.D1edit,'String',d); th1=inv(2); set(handles.Theta1edit,'String',th1); th2=inv(3); set(handles.Theta2edit,'String',th2); th3=inv(4); set(handles.Theta3edit,'String',th3); botplotcurve(L1,L2,L3,d,th1,th2,th3,X1I,Y1I,XEE,YEE); pause(0.05); elseif(inv(5)==0) msgbox('Curve does not lie in the Workspace','Singularity Error') breakendendend
Workspace Plotting
18
function WorkSpace_Callback(hObject, eventdata, handles)
L1=get(handles.L1edit,'String');L1=round(str2double(L1)); L2=get(handles.L2edit,'String');L2=round(str2double(L2)); L3=get(handles.L3edit,'String');L3=round(str2double(L3)); D=50;X=-(D+L1+L2+L3+5):5:(D+L1+L2+L3+5);Y=-(5+L1+L2+L3):5:(5+L1+L2+L3);hold onfor i=1:1:length(X) for j=1:1:length(Y); inv=InvKin(L1,L2,L3,X(i),Y(j),-50,90,90,90); if(inv(5)==1) plot(X(i),Y(j),'dg'); elseif(inv(5)==0) plot(X(i),Y(j),'xr'); end endendhold off;
PARALLEL MANIPULATOR (2R-R-R CHAINS & 1 R-P-R CHAIN)
19
The User Interface
Basic GUI
20
Following are the important codes developed but not implemented in GUI.
Bot Plot
function botplot(X1,Y1,X5,Y5,X7,Y7,theta11,theta21,theta12,d,theta13,theta23) L = 10;theta31 = 45;
a01 = sqrt(X1^2+Y1^2);a11 = 25;a21 = 25;a02 = sqrt(X5^2+Y5^2);a12 = 25;a03 = sqrt(X7^2+Y7^2);a13 = 25;a23 = 25; theta01 = atan2(Y1,X1);theta02 = atan2(Y5,X5);theta03 = atan2(Y7,X7);theta01=theta01*180/pi;theta02 = theta02*180/pi;theta03 = theta03*180/pi; X01 = [0,a01*cosd(theta01)];Y01 = [0,a01*sind(theta01)]; X12 = [a01*cosd(theta01),a01*cosd(theta01)+a11*cosd(theta11)];Y12 = [a01*sind(theta01),a01*sind(theta01)+a11*sind(theta11)]; X23 = [a01*cosd(theta01)+a11*cosd(theta11),a01*cosd(theta01)+a11*cosd(theta11)+a21*cosd(theta21)];Y23 = [a01*sind(theta01)+a11*sind(theta11),a01*sind(theta01)+a11*sind(theta11)+a21*sind(theta21)]; X36 = [a01*cosd(theta01)+a11*cosd(theta11)+a21*cosd(theta21),a01*cosd(theta01)+a11*cosd(theta11)+a21*cosd(theta21)+L*cosd(-45)];Y36 = [a01*sind(theta01)+a11*sind(theta11)+a21*sind(theta21),a01*sind(theta01)+a11*sind(theta11)+a21*sind(theta21)+L*sind(-45)]; X65 = [a01*cosd(theta01)+a11*cosd(theta11)+a21*cosd(theta21)+L*cosd(-45),a01*cosd(theta01)+a11*cosd(theta11)+a21*cosd(theta21)+L*cosd(-45)+(a12+d)*cosd(180+theta12)];
21
Y65 = [a01*sind(theta01)+a11*sind(theta11)+a21*sind(theta21)+L*sind(-45),a01*sind(theta01)+a11*sind(theta11)+a21*sind(theta21)+L*sind(-45)+(a12+d)*sind(180+theta12)]; X69 = [a01*cosd(theta01)+a11*cosd(theta11)+a21*cosd(theta21)+L*cosd(-45),a01*cosd(theta01)+a11*cosd(theta11)+a21*cosd(theta21)+L*cosd(-45)+L*cosd(45)];Y69 = [a01*sind(theta01)+a11*sind(theta11)+a21*sind(theta21)+L*sind(-45),a01*sind(theta01)+a11*sind(theta11)+a21*sind(theta21)+L*sind(-45)+L*sind(45)]; X98 = [a01*cosd(theta01)+a11*cosd(theta11)+a21*cosd(theta21)+L*cosd(-45)+L*cosd(45),a01*cosd(theta01)+a11*cosd(theta11)+a21*cosd(theta21)+L*cosd(-45)+L*cosd(45)+a23*cosd(180+theta23)];Y98 = [a01*sind(theta01)+a11*sind(theta11)+a21*sind(theta21)+L*sind(-45)+L*sind(45),a01*sind(theta01)+a11*sind(theta11)+a21*sind(theta21)+L*sind(-45)+L*sind(45)+a23*sind(180+theta23)]; X87 = [a01*cosd(theta01)+a11*cosd(theta11)+a21*cosd(theta21)+L*cosd(-45)+L*cosd(45)+a23*cosd(180+theta23),a01*cosd(theta01)+a11*cosd(theta11)+a21*cosd(theta21)+L*cosd(-45)+L*cosd(45)+a23*cosd(180+theta23)+a13*cosd(180+theta13)];Y87 = [a01*sind(theta01)+a11*sind(theta11)+a21*sind(theta21)+L*sind(-45)+L*sind(45)+a23*sind(180+theta23),a01*sind(theta01)+a11*sind(theta11)+a21*sind(theta21)+L*sind(-45)+L*sind(45)+a23*sind(180+theta23)+a13*sind(180+theta13)]; X34 = [a01*cosd(theta01)+a11*cosd(theta11)+a21*cosd(theta21),a01*cosd(theta01)+a11*cosd(theta11)+a21*cosd(theta21)+L*cosd(45)];Y34 = [a01*sind(theta01)+a11*sind(theta11)+a21*sind(theta21),a01*sind(theta01)+a11*sind(theta11)+a21*sind(theta21)+L*sind(45)]; X49 = [a01*cosd(theta01)+a11*cosd(theta11)+a21*cosd(theta21)+L*cosd(45),a01*cosd(theta01)+a11*cosd(theta11)+a21*cosd(theta21)+L*cosd(45)+L*cosd(-45)];Y49 = [a01*sind(theta01)+a11*sind(theta11)+a21*sind(theta21)+L*sind(45),a01*sind(theta01)+a11*sind(theta11)+a21*sind(theta21)+L*cosd(45)+L*sind(-45)];Axis([-100 100 -100 100])hold on;plot(X12,Y12,'ro-',X23,Y23,'go-',X36,Y36,'bo-',X65,Y65,'ro-',X69,Y69,'bo-',X98,Y98,'go-',X87,Y87,'ro-',X34,Y34,'bo-',X49,Y49,'bo-','linewidth',4);grid ongrid on end
Forward Kinematics
22
function [fwd] = FwdKin(Xb1,Yb1,Xb2,Yb2,Xb3,Yb3,d,theta21,theta23,Xe,Ye,phi,theta11,theta12,theta13)a01 = sqrt(Xb1^2+Yb1^2);theta01 = atan2(Yb1,Xb1);theta01=theta01*180/pi; a02 = sqrt(Xb2^2+Yb2^2);theta02 = atan2(Yb2,Xb2);theta02=theta02*180/pi; a03 = sqrt(Xb3^2+Yb3^2);theta03 = atan2(Yb3,Xb3);theta03=theta03*180/pi; a11 = 20;a21 = 25;a31 = 15;a12 = 30;a32 = a31*sqrt(2);a13 = 22;a23 = 18;a33 = a31;deltae1 = 45;deltae2 = 90;deltae3 = 135;Delta = [1 1 1 1 1 1]';n=1; while(abs(Delta) >0.001 & n<500) c01 = cosd(theta01);c11 = cosd(theta11);c21 = cosd(theta21);c31 = cosd(phi-deltae1);c02 = cosd(theta02);c12 = cosd(theta12);c32 = cosd(phi-deltae2);c03= cosd(theta03);c13= cosd(theta13);c23= cosd(theta23);c33= cosd(phi-deltae3); s01 = sind(theta01);s11 = sind(theta11);s21 = sind(theta21);s31 = sind(phi-deltae1);s02 = sind(theta02);s12 = sind(theta12);s32 = sind(phi-deltae2);s03= sind(theta03);s13= sind(theta13);s23= sind(theta23);s33= sind(phi-deltae3); f1 = a01*c01+a11*c11+a21*c21+a31*c31-Xe;f2 = a01*s01+a11*s11+a21*s21+a31*s31-Ye;
23
f3 = a02*c02+a12*c12+d*c12+a32*c32-Xe;f4 = a02*s02+a12*s12+d*s12+a32*s32-Ye; f5 = a03*c03+a13*c13+a23*c23+a33*c33-Xe;f6 = a03*s03+a13*s13+a23*s23+a33*s33-Ye; J = [0 -a21*s21 0 -1 -1 -a31*s31 ; 0 a21*c21 0 -1 -1 a31*c31; c12 0 0 -1 -1 -a32*s32; s12 0 0 -1 -1 a32*c32; 0 0 -a23*s23 -1 -1 -a33*s33; 0 0 a23*c23 -1 -1 a33*c33] Delta = -inv(J)*[f1 f2 f3 f4 f5 f6]'; det(J) d = d + Delta(1); theta21 = theta21 + Delta(2); theta23 = theta23 + Delta(3); Xe = Xe + Delta(4); Ye = Ye + Delta(5); phi = phi + Delta(6); n = n+1end while(theta21>360) theta21=theta21-360;endwhile(theta21<-360) theta21=theta21+360;end while(theta23>360) theta23=theta23-360;endwhile(theta23<-360) theta23=theta23+360; end if (theta21<0) theta21=360+theta21;end if (theta23<0) theta23=360+theta23;endfwd = [d theta21 theta23 Xe Ye phi]; end
Inverse Kinematics
Chain1. R-R-R
function [inv1] = InvKin1(Xe,Ye,Xb1,Yb1,theta11,theta21,a11,a21,L)
24
theta31 = 45;deltae1 = 45;phi = 90;a01 = sqrt(Xb1^2+Yb1^2);theta01 = atan2(Yb1,Xb1); a31 = L; Delta = [1 1]';n=1;while(abs(Delta) >0.001 & n<500)c01 = cosd(theta01);c11 = cosd(theta11);c21 = cosd(theta21);c31 = cosd(phi-deltae1);s01 = sin(theta01);s11 = sin(theta11);s21 = sin(theta21);s31 = sin(phi-deltae1);f1 = a01*c01+a11*c11+a21*c21+a31*c31-Xe;f2 = a01*s01+a11*s11+a21*s21+a31*s31-Ye; J = [-a11*s11 -a21*s21; a11*c11 a21*c21]; Delta = -inv(J)*[f1 f2]'theta11 = theta11+Delta(1);theta21 = theta21+Delta(2);n=n+1;end
while(theta11>360) theta11=theta11-360;endwhile(theta11<-360) theta11=theta11+360;end while(theta21>360) theta21=theta21-360;endwhile(theta21<-360) theta21=theta21+360; end if (theta11<0) theta11=360+theta11;end if (theta21<0) theta21=360+theta21;end
25
inv1 = [theta11 theta21]; end
Chain2. R-P-R
function [inv2] = InvKin2(Xe,Ye,Xb2,Yb2,d,theta12,a12) a02 = sqrt(Xb2^2+Yb2^2);theta02 = atan2(Yb2,Xb2);deltae2 = 90;phi = 90; a32 = sqrt(2)*15 Delta = [1 1]';n=1;while(abs(Delta) >0.001 & n<500) c02 = cosd(theta02);c12 = cosd(theta12);c32 = cosd(phi-deltae2);s02 = sin(theta02);s12 = sin(theta12);s32 = sin(phi-deltae2); f1 = a02*c02+a12*c12+d*c12+a32*c32-Xe;f2 = a02*s02+a12*s12+d*s12+a32*s32-Ye; J = [c12 -a12*s12-d*s12; s12 a12*c12+a12*c12]; Delta = -inv(J)*[f1 f2]' d = d + Delta(1); theta12 = theta12 + Delta(2); n = n+1end inv2 = [d theta12] while(theta12>360) theta12=theta12-360;endwhile(theta12<-360) theta12=theta12+360;end if (theta12<0) theta12=360+theta12;endend
Chain3. R-R-R
function [inv3] = InvKin3(Xe,Ye,Xb3,Yb3,theta13,theta23,a13,a23,a33)
26
theta31 = 45;deltae3 = 135;phi = 90;a03 = sqrt(Xb3^2+Yb3^2);theta03 = atan2(Yb3,Xb3); Delta = [1 1]';n=1;while(abs(Delta) >0.001 & n<500)c03= cosd(theta03);c13= cosd(theta13);c23= cosd(theta23);c33= cosd(phi-deltae3);s03 = sind(theta03);s13 = sind(theta13);s23 = sind(theta23);s33 = sind(phi-deltae3);f1 = a03*c03+a13*c13+a23*c23+a33*c33-Xe;f2 = a03*s03+a13*s13+a23*s23+a33*s33-Ye; J = [-a13*s13 -a23*s23; a13*c13 a23*c23]; Delta = -inv(J)*[f1 f2]'theta13 = theta13+Delta(1);theta23 = theta23+Delta(2);n=n+1;end while(theta13>360) theta13=theta13-360;endwhile(theta13<-360) theta13=theta13+360;end while(theta23>360) theta23=theta23-360;endwhile(theta23<-360) theta23=theta23+360; end if (theta13<0) theta13=360+theta13;end if (theta23<0) theta23=360+theta23;end inv3 = [theta13 theta23] end
27
28