% Constants
r1 = 22;
r2 = 8;
r3 = 22;
r4 = 6;
r5 = 11;
r6 = 10;
r7 = r8 = 7;
pi = 4*atan(1.0);
theta_2 = 270 * pi/180; % Degrees to radians;
alpha = 30 * pi/180;
theta_7 = theta_2 - pi;
% Initial guesses in radians
theta_3 = 120 * pi/180;
theta_4 = 110 * pi/180;
theta_5 = 180 * pi/180;
theta_6 = 110 * pi/180;
theta_8 = 100 * pi/180;
x = [theta_3; theta_4; theta_5; theta_6; theta_8];
for i = 1:5
% Functions of the guessed values
f1 = r1 + r2 * cos(theta_2) + r3 * cos(theta_3) + r4 * cos(theta_4);
f2 = r2 * sin(theta_2) + r3 * sin(theta_3) + r4 * sin(theta_4);
f3 = r1+ r5 * cos(theta_5) + r6 * cos(theta_6) + r7 * cos(theta_7) + r8 * cos(theta_8);
f4 = r5 * sin(theta_5) + r6 * sin(theta_6) + r7 * sin(theta_7) + r8 * sin(theta_8);
f5 = theta_4 - theta_8 + pi - alpha;
f = [f1;f2;f3;f4;f5];
% Partials of f with respect to each element in x
dfdt3 = [-r3 * sin(theta_3);r3 * cos(theta_3);0;0;0];
dfdt4 = [-r4 * sin(theta_4);r4 * cos(theta_4);0;0;1];
dfdt5 = [0;0;-r5 * sin(theta_5);r5 * cos(theta_5);0];
dfdt6 = [0;0;-r6 * sin(theta_6);r6 * cos(theta_6);0];
dfdt8 = [0;0;-r8 * sin(theta_8);r8 * cos(theta_8);1];
A = [dfdt3 dfdt4 dfdt5 dfdt6 dfdt8];
x = x-inv(A)*f;
theta_3 = x(1,1);
theta_4 = x(2,1);
theta_5 = x(3,1);
theta_6 = x(4,1);
theta_7 = x(5,1);
end