MAE 593
     MATHEMATICAL METHODS IN ROBOTICS




                FINAL PROJECT REPORT




            P-R-R-R SERIAL MANI...
2
INDEX


Sr.No   Description                                         Pg.No




 1      Project Objective                   ...
OBJECTIVE :-

       To implement a P-R-R-R Serial Manipulator and explore its functionality by providing a Graphical
   ...
Theta2 = Absolute angle for link No.2.

Theta3 = Absolute angle for link No.3.

Xe = X Co-ordinate of End Effector.

Ye = ...
B.Parallel Manipulator (2RRR Chains and 1PRR Chain)




a11 = Length of link 1 of manipulator chain no.1.

a21 = Length of...
Phi = angle made by end effector with the horizontal.

D = Movement of prismatic joint P.

                               ...
B.WORKSPACE PLOTTING




Push Button :- WORKSPACE

“WORKSPACE” plots the ‘region of reach’ for the manipulator using Newto...
C.FORWARD KINEMATICS




FORWARD KINEMATICS :-

FUNCTION :-

      Allows user to select the values of joint angles , the...
D.INVERSE KINEMATICS




INVERSE KINEMATICS :-

FUNCTION :-

      Allows user to select the values of End Effector Co-or...
position to take place in accordance with the entered values of end effector co-ordinates.




E.ELLIPSE TRACING




FUNCT...
F.CIRCLE TRACING




FUNCTION :-

      Plots a circle of defined Radius = Major Axis = Minor Axis and Center point co-or...
G.CURVE TRACING




                  CURVE TRACING ALONG X-X DIRECTION




                                              ...
CURVE TRACING ALONG X-Y DIRECTION


FUNCTION :-

      Allows the user to select a set of 5 points anywhere on the screen...
H.ERROR MESSAGES




      An error Message “Curve does not lie in Workspace” is displayed when any one of the user-
    ...
PROGRAM CODES


Following are the important function codes that are incorporated in the GUI.

Home Postion
function Home_C...
Forward Kinematics
function X= BotFwdKin(L1,L2,L3,d1,Theta1,Theta2,Theta3)
Xee=d1+L1*cosd(Theta1)+L2*cosd(Theta2)+L3*cosd(...
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 -Ye...
Yee=Yc+ b*sind(theta);
InvKin= [Xee Yee]';
plot(Xee,Yee,'-r',Xc,Yc,'o')

grid on;
end




Curve Tracing
function TraceCurv...
pause(0.05);

elseif(inv(5)==0)
    msgbox('Curve does not lie in the Workspace','Singularity Error')
    break
end
end
en...
PARALLEL MANIPULATOR (2R-R-R CHAINS & 1 R-P-R CHAIN)



The User Interface




                              Basic GUI



...
Following are the important codes developed but not implemented in GUI.



Bot Plot
function botplot(X1,Y1,X5,Y5,X7,Y7,the...
Y65 = [a01*sind(theta01)+a11*sind(theta11)+a21*sind(theta21)+L*sind(-
45),a01*sind(theta01)+a11*sind(theta11)+a21*sind(the...
function [fwd] =
FwdKin(Xb1,Yb1,Xb2,Yb2,Xb3,Yb3,d,theta21,theta23,Xe,Ye,phi,theta11,theta12,th
eta13)
a01 = sqrt(Xb1^2+Yb1...
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...
theta31 = 45;
deltae1 = 45;
phi = 90;
a01 = sqrt(Xb1^2+Yb1^2);
theta01 = atan2(Yb1,Xb1);

a31 = L;

Delta = [1 1]';
n=1;
w...
inv1 = [theta11 theta21];

end

Chain2. R-P-R
function [inv2] = InvKin2(Xe,Ye,Xb2,Yb2,d,theta12,a12)

a02 = sqrt(Xb2^2+Yb2...
theta31 = 45;
deltae3 = 135;
phi = 90;
a03 = sqrt(Xb3^2+Yb3^2);
theta03 = atan2(Yb3,Xb3);


Delta = [1 1]';
n=1;
while(abs...
29
Upcoming SlideShare
Loading in …5
×

MAE 593 Final ProjectReport

767 views

Published on

Published in: Education, Technology, Business
  • Be the first to comment

  • Be the first to like this

MAE 593 Final ProjectReport

  1. 1. 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
  2. 2. 2
  3. 3. 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 3
  4. 4. 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. 4
  5. 5. Theta2 = Absolute angle for link No.2. 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 5
  6. 6. 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. 6
  7. 7. Phi = angle made by end effector with the horizontal. 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. 7
  8. 8. 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. 8
  9. 9. 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. 9
  10. 10. 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 10
  11. 11. position to take place in accordance with the entered values of end effector co-ordinates. E.ELLIPSE TRACING 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. 11
  12. 12. F.CIRCLE TRACING FUNCTION :-  Plots a circle of defined Radius = Major Axis = Minor Axis and Center point co-ordinates. 12
  13. 13. G.CURVE TRACING CURVE TRACING ALONG X-X DIRECTION 13 CURVE TRACING ALONG Y-Y DIRECTION
  14. 14. CURVE TRACING ALONG X-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. 14
  15. 15. H.ERROR MESSAGES  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. 15
  16. 16. PROGRAM CODES 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); 16
  17. 17. 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 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; end c1=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; end c1=cosd(theta1); c2=cosd(theta2); 17
  18. 18. 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 if(abs(f1)>3 && abs(f2)>3 || abs(d1)>50) chk=0; else chk=1; end while(theta1>360) theta1=theta1-360; end while(theta1<-360) theta1=theta1+360; end while(theta2>360) theta2=theta2-360; end while(theta2<-360) theta2=theta2+360; end while(theta3>360) theta3=theta3-360; end while(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; end inv=[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); 18
  19. 19. 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) 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; end if(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); 19
  20. 20. pause(0.05); elseif(inv(5)==0) msgbox('Curve does not lie in the Workspace','Singularity Error') break end end end Workspace Plotting 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 on for 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 end end hold off; 20
  21. 21. PARALLEL MANIPULATOR (2R-R-R CHAINS & 1 R-P-R CHAIN) The User Interface Basic GUI 21
  22. 22. 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)]; 22
  23. 23. 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(th eta01)+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(th eta01)+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-',X9 8,Y98,'go-',X87,Y87,'ro-',X34,Y34,'bo-',X49,Y49,'bo-','linewidth',4);grid on grid on end Forward Kinematics 23
  24. 24. function [fwd] = FwdKin(Xb1,Yb1,Xb2,Yb2,Xb3,Yb3,d,theta21,theta23,Xe,Ye,phi,theta11,theta12,th eta13) 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; 24
  25. 25. 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+1 end while(theta21>360) theta21=theta21-360; end while(theta21<-360) theta21=theta21+360; end while(theta23>360) theta23=theta23-360; end while(theta23<-360) theta23=theta23+360; end if (theta21<0) theta21=360+theta21; end if (theta23<0) theta23=360+theta23; end fwd = [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) 25
  26. 26. 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; end while(theta11<-360) theta11=theta11+360; end while(theta21>360) theta21=theta21-360; end while(theta21<-360) theta21=theta21+360; end if (theta11<0) theta11=360+theta11; end if (theta21<0) theta21=360+theta21; end 26
  27. 27. 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+1 end inv2 = [d theta12] while(theta12>360) theta12=theta12-360; end while(theta12<-360) theta12=theta12+360; end if (theta12<0) theta12=360+theta12; end end Chain3. R-R-R function [inv3] = InvKin3(Xe,Ye,Xb3,Yb3,theta13,theta23,a13,a23,a33) 27
  28. 28. 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; end while(theta13<-360) theta13=theta13+360; end while(theta23>360) theta23=theta23-360; end while(theta23<-360) theta23=theta23+360; end if (theta13<0) theta13=360+theta13; end if (theta23<0) theta23=360+theta23; end inv3 = [theta13 theta23] end 28
  29. 29. 29

×