MAE 593 Final ProjectReport

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