Why is my script not working?

I keep getting this same error:

Warning: Matrix is close to singular or badly

scaled. Results may be inaccurate. RCOND =

8.154826e-18.

> In originalfailure (line 61)

clear all; clc;

pi=4.0*atan(1.0);

%scalar knowns
r1=4.8;
r2=2.0;
r6=3.63;
t1=-pi;
t5=-pi/2;
t6=0;

%input theta 2
t2= 283*pi/180;

%guess values of scalar unknowns
r3=3.0;
r4=11.0;
r5=4.0;
t3=5.236;
t4=5.236;

%vector containing initial guesses:
x0=[r3;r4;r5;t3;t4];
x=[0;0;0;0;0];
% counter to limit iterations
counter = 0;
counterLimit = 1e3;
while abs(x-x0) >= 1e-2
    counter = counter+1;
    
    ct2=cos(t2);
    st2=sin(t2);
    ct3=cos(t3);
    st3=sin(t3);
    ct4=cos(t4);
    st4=sin(t4);

    
    %compute functions at current guessed values
    f1=r2*ct2-r3*ct3+r1;
    f2=r2*st2-r3*st3;
    f3=r6-r4*ct4+r1;
    f4=-r5-r4*st4;
    f5=t4-t3;
    
    %define vector for computed functions of guessed values
    f=[f1;f2;f3;f4;f5];
    
    %calculate partial derivatives of f w.r.t. each element of x
    dfdr3=[-ct3; 0; 0; r3*st3; 0];
    dfdr4=[-st3; 0; 0; -r3*ct3; 0]; 
    dfdr5=[0; -ct4; 0; 0; r4*st4];
    dfdt3=[0; -st4; -1; 0; r4*ct4];
    dfdt4=[0; 0; 0; -1; 1];
    
    % Define the A matrix
    A= [dfdr3 dfdr4 dfdr5 dfdt3 dfdt4];
    
    %use equation (2.67) to compute the solution x
    x=-inv(A)*f+x0;
    r3=x(1,1);
    r4=x(2,1);
    r5=x(3,1);
    t3=x(4,1);
    t4=x(5,1);
end

if counter == counterLimit
    warning('Failed')
end


fprintf(' %f\n', x);

[James Tursa]

Your iterative scheme appears to be diverging.

Answer 1

[Fabio Freschi]

I try

1) I think you are caluclating the jacobian matrix. To avoid mistakes I did this symbolically and the result is

>> syms r1 r2 r3 r4 r5 r6 t1 t2 t3 t4 t5 t6
>> f = [r2*cos(t2)-r3*cos(t3)+r1; r2*sin(t2)-r3*sin(t3); r6-r4*cos(t4)+r1; r5-r4*sin(t4); t4-t3]
 
f =
 
 r1 + r2*cos(t2) - r3*cos(t3)
      r2*sin(t2) - r3*sin(t3)
         r1 + r6 - r4*cos(t4)
              r5 - r4*sin(t4)
                      t4 - t3
 
>> A = jacobian(f,[r3 r4 r5 t3 t4])
 
A =
 
[ -cos(t3),        0, 0,  r3*sin(t3),           0]
[ -sin(t3),        0, 0, -r3*cos(t3),           0]
[        0, -cos(t4), 0,           0,  r4*sin(t4)]
[        0, -sin(t4), 1,           0, -r4*cos(t4)]
[        0,        0, 0,          -1,           1]

It seems that you have different signs in your variable dfdt3

2) you load your derivatives in the matrix columns. It seems to me you are working with the transpose Jacobian.

3) Never ever use inv(A)*f but rather use matrix solution A\f

All in all, I modified your code as follows

clear all; clc;
pi=4.0*atan(1.0);
%scalar knowns
r1=4.8;
r2=2.0;
r6=3.63;
t1=-pi;
t5=-pi/2;
t6=0;
%input theta 2
t2= 283*pi/180;
%guess values of scalar unknowns
r3=3.0;
r4=11.0;
r5=4.0;
t3=5.236;
t4=5.236;
%vector containing initial guesses:
x0=[r3;r4;r5;t3;t4];
x=[0;0;0;0;0];
% counter to limit iterations
counter = 0;
counterLimit = 1e3;
while abs(x-x0) >= 1e-2
    counter = counter+1;
    
    ct2=cos(t2);
    st2=sin(t2);
    ct3=cos(t3);
    st3=sin(t3);
    ct4=cos(t4);
    st4=sin(t4);
    
    %compute functions at current guessed values
    f1=r2*ct2-r3*ct3+r1;
    f2=r2*st2-r3*st3;
    f3=r6-r4*ct4+r1;
    f4=-r5-r4*st4;
    f5=t4-t3;
    
    %define vector for computed functions of guessed values
    f=[f1;f2;f3;f4;f5];
    
    %calculate partial derivatives of f w.r.t. each element of x
    dfdr3=[-ct3; 0; 0; r3*st3; 0];
    dfdr4=[-st3; 0; 0; -r3*ct3; 0];
    dfdr5=[0; -ct4; 0; 0; r4*st4];
    dfdt3=[0; -st4; 1; 0; -r4*ct4];
    dfdt4=[0; 0; 0; -1; 1];
    
    % Define the A matrix
    A= [dfdr3 dfdr4 dfdr5 dfdt3 dfdt4];
    
    %use equation (2.67) to compute the solution x
    x=-A.'\f+x0;
    r3=x(1);
    r4=x(2);
    r5=x(3);
    t3=x(4);
    t4=x(5);
end
if counter == counterLimit
    warning('Failed')
end
fprintf(' %f\n', x);

This code does provide a solution, if it is correct is up to you.

As a side note, I think it should be better coding not to use r1 r2 r3 r4 r5 r6 t1 t2 t3 t4 t5 t6 as variables, but I would rather use the vector x, and accessing with indexing (x(1), x(2), ...)