Matlab wrong matrix calculation

Hej folks,

im wiritng an function for the robotik jacobian matrix. As i finally understand how to set it up, matlab starts to make some weird stuff. IDK maybe it is some weird typecast happening, I do not understand it.

For Example the Cell T(1,2) should not have any coef. infront of the sin or cos terms.

Also all other theta values with a value bigger than 0, have the same behaviour.

Here is my test code:

syms c1 c2 c3 c4 c5 c6 c23 s1 s2 s3 s4 s5 s6 s23 L1 L2 L3 L4 L5 L6 th1 th2 th3 th4 th5 th5 th6 J T01 T02 T03 T04 T05 T06 T12 T23 T34 T45 T56

 L1 = 495;
 L2 = 175; 
 L3= 900;
 L4 = 175;
 L5 = 960.0 ;
 L6 = 135.0;
 
a = [0 L2 L3 L4 0 0];
alp1 = [0 -90 0 90 -90 -90];
% alp1 = [0 -90 0 90 -90 -90].*(pi()/180);

%  alp1 = [0 ;(-90.0); 0; 90.0;(-90.0);(-90.0)]';       %alpha numerisch

% alp = [c1 c2 c3 c4 c5 c6];                        %alpha syBOLISCH, aus
% bequemlichkeit habe ich für alpha symbolisch die übrigen cosini symbole
% genommen da diese nicht verwendet werden. 


d = [L1 0.0 0.0 -L5 0.0 L6];
% theta1=[90.0000 -97.5711 137.5437 -180.0000 84.9726 -180.0000]*pi()/180;
% theta1 = [0 -65.4188 144.4065 -180.0000 123.9877 180.0000].*(pi()/180);
% Kontrollbahn

theta1 = [0 -65.4188 144.4065 -180.0000 123.9877 180.0000];

val_a = a;
val_alp = alp1;
val_d = d;
val_theta = theta1;

sz_a = size(a,2);
sz_alp = size(alp1,2);
sz_d = size(d,2);
sz_th = size(theta1,2);


theta = [th1 th2 (th3-90) th4 th5 (th6+180)];
theta_diff = [th1 th2 th3 th4 th5 th6];

%if. sz_a(1,2) & sz_alp(1,2) & sz_d(1,2) & sz_th(1,2)
    
if sz_a==sz_alp& sz_d==sz_th & sz_a==sz_th

    
     sprintf("Input Values correct format, continue calculation") 
else 
     error("Input Values wrong format, stop calculation")
     


end

ans = "Input Values correct format, continue calculation"

% T=sym('T',[1,sz_th])

T =repmat({''}, 1,sz_th);
T1 =repmat({''}, 1,sz_th);
T2 =repmat({''}, 1,sz_th);


for i=1:sz_th

    T{1,i}= dh_fwk_craig(alp1(1,i),a(1,i),d(1,i),theta(1,i));
 
end

T1 = 4×4

1 0 0 0 0 1 0 0 0 0 1 0 0 0 0 1

T2 = 4×4

1 0 0 0 0 1 0 0 0 0 1 0 0 0 0 1

T3 = 

T4 = 4×4

1 0 0 495 0 1 0 0 0 0 1 0 0 0 0 1

Ta = 4×4

1 0 0 0 0 1 0 0 0 0 1 0 0 0 0 1

Tb = 

Tc = 

T = 

T1 = 4×4

1.0000 0 0 175.0000 0 0.0000 1.0000 0 0 -1.0000 0.0000 0 0 0 0 1.0000

T2 = 4×4

1 0 0 175 0 1 0 0 0 0 1 0 0 0 0 1

T3 = 

T4 = 4×4

1 0 0 0 0 1 0 0 0 0 1 0 0 0 0 1

Ta = 4×4

1.0000 0 0 350.0000 0 0.0000 1.0000 0 0 -1.0000 0.0000 0 0 0 0 1.0000

Tb = 

Tc = 

T = 

T1 = 4×4

1 0 0 900 0 1 0 0 0 0 1 0 0 0 0 1

T2 = 4×4

1 0 0 900 0 1 0 0 0 0 1 0 0 0 0 1

T3 = 

T4 = 4×4

1 0 0 0 0 1 0 0 0 0 1 0 0 0 0 1

Ta = 4×4

1 0 0 1800 0 1 0 0 0 0 1 0 0 0 0 1

Tb = 

Tc = 

T = 

T1 = 4×4

1.0000 0 0 175.0000 0 0.0000 -1.0000 0 0 1.0000 0.0000 0 0 0 0 1.0000

T2 = 4×4

1 0 0 175 0 1 0 0 0 0 1 0 0 0 0 1

T3 = 

T4 = 4×4

1 0 0 -960 0 1 0 0 0 0 1 0 0 0 0 1

Ta = 4×4

1.0000 0 0 350.0000 0 0.0000 -1.0000 0 0 1.0000 0.0000 0 0 0 0 1.0000

Tb = 

Tc = 

T = 

T1 = 4×4

1.0000 0 0 0 0 0.0000 1.0000 0 0 -1.0000 0.0000 0 0 0 0 1.0000

T2 = 4×4

1 0 0 0 0 1 0 0 0 0 1 0 0 0 0 1

T3 = 

T4 = 4×4

1 0 0 0 0 1 0 0 0 0 1 0 0 0 0 1

Ta = 4×4

1.0000 0 0 0 0 0.0000 1.0000 0 0 -1.0000 0.0000 0 0 0 0 1.0000

Tb = 

Tc = 

T = 

T1 = 4×4

1.0000 0 0 0 0 0.0000 1.0000 0 0 -1.0000 0.0000 0 0 0 0 1.0000

T2 = 4×4

1 0 0 0 0 1 0 0 0 0 1 0 0 0 0 1

T3 = 

T4 = 4×4

1 0 0 135 0 1 0 0 0 0 1 0 0 0 0 1

Ta = 4×4

1.0000 0 0 0 0 0.0000 1.0000 0 0 -1.0000 0.0000 0 0 0 0 1.0000

Tb = 

Tc = 

T = 

for k = 1:1:sz_th
     
    if k == 1
      
        T1{1,k} =     T(1,k);

    else 
        T1{1,k} =     T1(1,k-1)*T{1,k};

    end
    
%      T1{1,k};
end


T06 =      T1{1,sz_th};

% A = subs(T06,alp, alp1 );
% 
% T06 = A;
JV = jacobian(T06((1:3),4), theta_diff);    %% Matlab Funktion, erstellt symb. Jakobi
% thetas_val = [90.0000 -97.5711 137.5437 -180.0000 84.9726 -180.0000].*pi()/180; %% Bahn 9: Werte Vektor für Datentyptausch erstellen
JVsubs = subs(JV,theta_diff, val_theta ); %% Substitution der Symbole mit numerischen Werten
JVnum = double(JVsubs)                 %% Typecast sym --> double

JVnum = 3×6

0.0000 31.2701 2.7014 0.0000 1.6661 0.0000 51.9254 0.0000 0.0000 18.0723 0.0000 2.3562 0 -37.1774 -24.1089 0.0000 1.6661 -0.0000

function [T] = dh_fwk_craig(alpha,a,d,theta)
%UNTITLED Summary of this function goes here
%   Detailed explanation goes here
% T = my_rotx(alpha, 0,0,0)*t_ver(a,0,0)*my_rotz(theta,0,0,0)*t_ver(0,0,d)
T1 = my_rotx(alpha, 0,0,0)*t_ver(a,0,0)
T2 = t_ver(a,0,0)
T3 = my_rotz(theta,0,0,0)
T4 = t_ver(d,0,0)

Ta = T1*T2
Tb = Ta*T3
Tc=Tb*T4
T = Tc
end

function [rx] = my_rotx(a, x,y,z)

% syms rx

a = a*pi()/180;
rx = [ 1 0 0 x;0 cos(a) -sin(a) y;0 sin(a) cos(a) z; 0 0 0 1];

end

function [rz] = my_rotz(a, x,y,z)
% syms rz 


a = a*pi()/180;

rz = [  cos(a) -sin(a) 0 x; 
        sin(a) cos(a) 0 y;
        0 0 1 z;
        0 0 0 1];


end

function [T] = t_ver(x,y,z)
T = zeros(4,4);
for i = 1:3
    T(i,i) = 1;
end
T(1:4,4) = [x; y; z; 1];
end

I have edited the post. I Debugged it and extracted those two array which make the error. Extracted in an other file those two array work totaly fine. It is just the craig function wich did something really weird when k=2 and Tb is created.

Please help me im really frustrated at the moment.

Thank you a lot!

BR

DK

EDIT Post:

I have found that MATALB defines a Symolic Zero.

(sym('4967757600021511')*sin((sym(pi)*th2)/180))/sym('81129638414606681695789005144064')

a = 4967757600021511/81129638414606681695789005144064 -> a=...^-17 ~ 0

But I found a new error. Somehow the cell array multiplication was not done correctly.

[Darko Kulic]

Ignore the german text

[Walter Roberson]

We need t_ver function as well.