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.
Theta3 = Absolute angle for link No.3.
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.
D = Movement of prismatic joint P.
5
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.
9
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.
10
F.CIRCLE TRACING
FUNCTION :
Plots a circle of defined Radius = Major Axis = Minor Axis and Center point co-ordinates.
11
G.CURVE TRACING
CURVE TRACING ALONG X-X DIRECTION
CURVE TRACING ALONG Y-Y DIRECTION
12
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.
13
H.ERROR MESSAGES


An error Message “Curve does not lie in Workspace” is displayed when any one of the userselected 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.
14
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);
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];
15
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);
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;
16
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);
Yee=Yc+ b*sind(theta);
InvKin= [Xee Yee]';
plot(Xee,Yee,'-r',Xc,Yc,'o')
grid on;
end
17
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);
pause(0.05);
elseif(inv(5)==0)
msgbox('Curve does not lie in the Workspace','Singularity Error')
break
end
end
end
18
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;
19
PARALLEL MANIPULATOR (2R-R-R CHAINS & 1 R-P-R CHAIN)
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
a11
a21
a02
a12
a03
a13
a23
=
=
=
=
=
=
=
=
sqrt(X1^2+Y1^2);
25;
25;
sqrt(X5^2+Y5^2);
25;
sqrt(X7^2+Y7^2);
25;
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(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',X98,Y98,'go-',X87,Y87,'ro-',X34,Y34,'bo-',X49,Y49,'bo-','linewidth',4);grid
on
grid on
end
22
Forward Kinematics
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);
23
s33= sind(phi-deltae3);
f1 = a01*c01+a11*c11+a21*c21+a31*c31-Xe;
f2 = a01*s01+a11*s11+a21*s21+a31*s31-Ye;
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
24
Inverse Kinematics
Chain1. R-R-R
function [inv1] = InvKin1(Xe,Ye,Xb1,Yb1,theta11,theta21,a11,a21,L)
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
25
if (theta11<0)
theta11=360+theta11;
end
if (theta21<0)
theta21=360+theta21;
end
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
c12
c32
s02
s12
s32
=
=
=
=
=
=
cosd(theta02);
cosd(theta12);
cosd(phi-deltae2);
sin(theta02);
sin(theta12);
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
26
if (theta12<0)
theta12=360+theta12;
end
end
Chain3. R-R-R
function [inv3] = InvKin3(Xe,Ye,Xb3,Yb3,theta13,theta23,a13,a23,a33)
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;
27
end
if (theta13<0)
theta13=360+theta13;
end
if (theta23<0)
theta23=360+theta23;
end
inv3 = [theta13 theta23]
end
28