Is there a way to make this code output a plot with curved lines?

When I run this code it outputs a plot with lines straight across rather than curved. 
Is there a way to fix this?





%Clearing all values to ensure code runs properly
clc; clear;
% Initiating timer
tic
% Setting number of data points 'n' to 100 as to analyze 100 incrimiates of θ_2 
% between 0 and 2PI
n = 360;
% Defining known variables R1, R2, R3, and R4 in meters
r1=2.5;
r2 = 1.402;
r3 = 0.701;
r4 = 0.701;

% Defining known and angle theta1 and setting input
% angle of θ_2 to have 'n' number of incriminates from 0 to 2PI
th1 = 0;
th2 = linspace(0,2*pi,n);

% Defining inital guesses for each unknown value theta3,theta4,theta5,
% and R5 in radians/meters
th3guess = -0.87;
th4guess = 0.87;
th5guess = 0.9;
r5guess = 1.2;
% Pre allocated a nx4 matrix of zeros to improve speed. This is done because as 
% each position of θ_2 is solved, 4 variables (th3,th4,th5,R5)
% will be solved. 
m = zeros(n,4);
% Beginning 'for' loop to iterativly solve for each unknown variable, 
% evaluating each postion of theta 2 from postion in column 1 to column 'n'
% This iterative process is done using the NR function which is a Newton 
% Raphson function definied later in the function section.

for i = 1:n 
 % In this function th3, th4, th5, and R5 are outputs to the function and inital guesses, 
 % known values, and 'n' positions of θ_2 are inputs
 [th3,th4,th5,r5] = NR(th1,th2(i),th3guess,th4guess,th5guess,r1,r2,r3,r4,r5guess);
 % Each loop produces th3, R3, R4, and R5 for the correspoinding
 % position of th2
 th3guess = th3;
 th4guess = th4;
 th5guess = th5;
 r5guess = r5;
 % 'm(i,:)' saves output data from each loop into a matrix for ease of 
 % analysis
 m(i,:) = [th3,th4,th5,r5];
end

th2plot = th2';
th3plot = (m(:,1));
th4plot = (m(:,2));
th5plot = (m(:,3));
r5plot = (m(:,4));


% Minimum, maximum, and index values in the matrix are applied to each solved variable. 
% This is done to identify minimum and maximum points of
% data in each array and also to have their corresponding index value catalogged
[minvalth3,minidxth3] = min(th3plot);
[maxvalth3,maxidxth3] = max(th3plot);
[minvalth4,minidxth4] = min(th4plot);
[maxvalth4,maxidxth4] = max(th4plot);
[minvalth5,minidxth5] = min(th5plot);
[maxvalth5,maxidxth5] = max(th5plot);
[minvalR5,minidxR5] = min(r5plot);
[maxvalR5,maxidxR5] = max(r5plot);

% various 'lbl_dwn' is created to ease in the position of labels that are to be 
% placed on the plot
lbl_dwn = 0.1*min(th3);
lbl_dwn3 = 0.1*min(th4);
lbl_dwn4 = 0.1*min(th5);
lbl_dwn5 = 0.1*min(r5);
% Solved values from the Newton Raphson function for each postion of th2 is 
% plotted w.r.t. th2. 
plot(th2plot,th3plot,th2plot,th4plot,th2plot,th5plot,th2plot,r5plot)
ylim([-3,6]);
hold on;
grid on;

% 'xticks' and 'xtickleabels' provide for a clear and consice notation for
% labeling the x axis as th2's position is denoted in radians. 
xticks([0 pi/4 pi/2 (3*pi)/4 pi (5*pi)/4 (3*pi)/2 (7*pi)/4 2*pi])
xticklabels({'0','\pi/4','\pi/2','3\pi/4','\pi','5\pi/4',...
 '3\pi/2','7\pi/4','2\pi'})
% 'yticks' sets number of y axis tick values.
yticks([-3 -2 -1 0 1 2 3 4 5 6])
% Here both x and y axis are labeled
xlabel('Θ_2 (Radians)')
ylabel('Outputs')
% the legend is placed in 'best fit' and is defined in smaller font as to not distract
% from plot data. A title is also provided here.
legend('Θ_3','Θ_4','Θ_5','R5_{(m)}',"Location","best","fontsize",7)
title('Position Analysis')
hold off

% Max position change for link and angle limits (radians and inches)
th3limit = max(th3plot) - min(th3plot)
th4limit = max(th4plot) - min(th4plot)
th5limit = max(th5plot) - min(th5plot)
r5limit = max(r5plot) - min(r5plot)


% Ending timer
toc

function [th3,th4,th5,r5] = NR(~,~,th3guess,th4guess,th5guess,~,r2,r3,r4,r5guess)
% Function 'NR' is a Newton Raphson function that inputs known/defined
% variables, an input variable 'th2', as well as inital guesses for unknown 
% function uses Ax = b --> x = A\b (backslash method) to iteratively produce 

% and update guesses until the output meets the defined convergence criterion
% for convergence is met, the solution of unknown 'x' array of variables is 
% Setting criterion for convergence as 1e-6
epsilon = 1e-6;
% Initial residual matrix formulation
f = [-(0 + r3.*cos(th3guess) - r4.*cos(th4guess));...
 -(r2 + r3.*sin(th3guess) - r4.*sin(th4guess));...
 -(cos(th3guess) + r5guess.*cos(th5guess) - cos(th4guess));...
 -(sin(th3guess) + r5guess.*sin(th5guess) - sin(th4guess))];
% iteration counter begins at 0
iter = 0;
% while statement confirming convergence criterion using 'norm' function
% for residual
while norm(f)>epsilon 
 
 % for each 'while' loop iteration 1 is added to iteration counter
 iter = iter + 1;
 
 % Jacobian for data is set to be evaluated
 J = [-r3.*sin(th3guess),r4.*sin(th4guess),0,0;...
 r3.*cos(th3guess),-r4.*cos(th4guess),0,0;...
 cos(th3guess),-cos(th4guess),-r5guess.*sin(th5guess),cos(th5guess);...
 sin(th3guess),-sin(th4guess),r5guess.*cos(th5guess),sin(th5guess)]; 
 % change in all variabels in displayed by 'dth' which builds a matrix
 % of values to be added to inital guesses.
 dth = J\f;
th3guess = th3guess+dth(1);
th4guess = th4guess+dth(2);
th5guess = th5guess+dth(3);
 r5guess = r5guess+dth(4);
 % residual is now redifined with updated guesses and loop continues
 % until convergence is reached and unknown 'x' values are produced.
f = [-(0 + r3.*cos(th3guess) - r4.*cos(th4guess));...
 -(r2 + r3.*sin(th3guess) - r4.*sin(th4guess));...
 -(cos(th3guess) + r5guess.*cos(th5guess) - cos(th4guess));...
 -(sin(th3guess) + r5guess.*sin(th5guess) - sin(th4guess))];
% setting break criterion is iterations do not converge.
 if iter > 100
 break
 end
end
% converged values of unknown variables are output as th3 for
% theta3, th4 for theta4, th5 for theta5,and r5 
th3 = th3guess;
th4 = th4guess;
th5 = th5guess;
r5 = r5guess;
end

[Stephen23]

"When I run this code it outputs a plot with lines straight across rather than curved."

Because your NR function always returns the same output values. Did you look at the matrix m ?

"Is there a way to fix this?"

If you expect NR to return different values then you will need to debug NR.

[Bryan Tassin]

I thought that was the reason. Any suggestions on how to debug?

[Stephen23]

"Any suggestions on how to debug?"

Ultimately debugging comes down to running code line-by-line and checking that each line produces the expected ouput. There is no "shortcut" or "easy way to debug", you just have to do it. Some tools to help you:

https://www.mathworks.com/help/matlab/debugging-code.html

https://www.mathworks.com/help/matlab/matlab_prog/debugging-process-and-features.html

Answer 1

[AndresVar]

There might be an issue with your f or your jacobian

After the first th2, your f~0 so norm(f)~0 and there are no convergence iterations, even if you remove tolerance, dth~0 so solution doesn't change in 100 iterations.

Also your jabocian doesn't match your f. Compare you j with the last 2 rows here

syms th3guess th4guess th5guess r5guess r4 r3 r2
f = [-(0 + r3.*cos(th3guess) - r4.*cos(th4guess));...
 -(r2 + r3.*sin(th3guess) - r4.*sin(th4guess));...
 -(cos(th3guess) + r5guess.*cos(th5guess) - cos(th4guess));...
 -(sin(th3guess) + r5guess.*sin(th5guess) - sin(th4guess))];

J=jacobian(f,[th3guess,th4guess,th5guess,r5guess])*-1

J = 

[AndresVar]

btw even with the new j your angles becomes ~90 so f~0 and same issue happen

[Bryan Tassin]

yeah i just tried it with the new matrix, but same result. not sure what else to do.

[Bryan Tassin]

i think the f is the problem over the j though. im basing it off of these two equations

R2e^(theta2)+R3e^(theta3)-R4e^(theta4) and RBCe^(theta3)+R5e^(theta5)-RBEe^(theta4)