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);
r1=4.8;
r2=2.0;
r6=3.63;
t1=-pi;
t5=-pi/2;
t6=0;
t2= 283*pi/180;
r3=3.0;
r4=11.0;
r5=4.0;
t3=5.236;
t4=5.236;
x0=[r3;r4;r5;t3;t4];
x=[0;0;0;0;0];
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);
f1=r2*ct2-r3*ct3+r1;
f2=r2*st2-r3*st3;
f3=r6-r4*ct4+r1;
f4=-r5-r4*st4;
f5=t4-t3;
f=[f1;f2;f3;f4;f5];
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];
A= [dfdr3 dfdr4 dfdr5 dfdt3 dfdt4];
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), ...)
Best Answer