Matlab wrong matrix calculation
2 views (last 30 days)
Show older comments
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
% 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
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
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.
2 Comments
Answers (0)
See Also
Categories
Find more on Logical in Help Center and File Exchange
Community Treasure Hunt
Find the treasures in MATLAB Central and discover how the community can help you!
Start Hunting!