How can it be that a function is outputing a vector with 38 elements and Simulink takes it as one element? - matlab

Question

In the Simulink model below my interpreted function output is a vector with 38 elements. I have two functions that have the same outputs one of them works perfectly (desiredtrajectory_sim.m) but the other one doesn't (desiredtrajectory.m).
Any suggestions. thanks
Here is the Simulink model
function [desired_state] = desiredtrajectory_sim(in)
t = in(1);
Sf = [ 1; 2; pi/4];
dSf = [0;0;0];
Pf = [ 0.1*t; 0; 0.5*sin(0.03*pi*t) + 2; 0; 0.01*pi*t ; 0];
dPf = [ 0.1; 0; 0.5*0.03*pi*cos(0.03*pi*t); 0; 0.01*pi; 0];
pf = Sf(1); qf = Sf(2); betaf = Sf(3);
xf = Pf(1); yf = Pf(2); zf = Pf(3);
phif = Pf(4); thetaf = Pf(5); psif = Pf(6);
rf = sqrt(pf^2 + qf^2 - 2*pf*qf*cos(betaf));
h1 = sqrt(0.5*(pf^2 + qf^2 - 0.5*rf^2));
h2 = sqrt(0.5*(rf^2 + pf^2 - 0.5*qf^2));
h3 = sqrt(0.5*(qf^2 + rf^2 - 0.5*pf^2));
alpha1 = acos((4*(h1^2+h2^2)-9*pf^2)/(8*h1*h2));
alpha2 = acos((4*(h1^2+h3^2)-9*qf^2)/(8*h1*h3));
Rot = RPYtoRot_ZXY(phif, thetaf, psif);
r1 = Rot*[2/3*h1;0;0];
r2 = Rot*[2/3*h2*cos(alpha1);2/3*h2*sin(alpha1);0];
r3 = Rot*[2/3*h3*cos(alpha2);-2/3*h3*sin(alpha2);0];
pos_des1 = [xf;yf;zf] + r1;
pos_des2 = [xf;yf;zf] + r2;
pos_des3 = [xf;yf;zf] + r3;
omega = [0 -sin(psif) cos(thetaf)*cos(psif);...
0 -cos(psif) cos(thetaf)*sin(psif);...
1 0 -sin(thetaf)]*dPf(4:6);
vel_des1 = dPf(1:3) + cross(omega, r1);
vel_des2 = dPf(1:3) + cross(omega, r2);
vel_des3 = dPf(1:3) + cross(omega, r3);
acc_des = [0;0;0];
desired_state1 = [pos_des1;vel_des1;acc_des];
desired_state2 = [pos_des2;vel_des2;acc_des];
desired_state3 = [pos_des3;vel_des3;acc_des];
desired_state = [desired_state1;desired_state2;desired_state3; psif; 0; Pf;
Sf]
size(desired_state)
end
Here is the Simulink block and the error message
As you can notice the bus gives just one element compared to the previous one which gives 38 elements, although they have the same output.
function [desired_state] = desiredtrajectory(in)%(t, pos)
tm= in(1)
pos = in(2:10);
syms t xf yf zf phif thetaf psif pf qf betaf
rf = sqrt(pf^2+qf^2-2*pf*qf*cos(betaf));
h1 = sqrt(0.5*(pf^2+qf^2-0.5*rf^2));
h2 = sqrt(0.5*(rf^2+pf^2-0.5*qf^2));
h3 = sqrt(0.5*(qf^2+rf^2-0.5*pf^2));
alf1 = acos((4*(h1^2+h2^2)-9*pf^2)/(8*h1*h2));
alf2 = acos((4*(h1^2+h3^2)-9*qf^2)/(8*h1*h3));
Rot = RPYtoRot_ZXY(phif, thetaf, psif);
eps = [Rot*[2/3;0;0]+[xf;yf;zf]
Rot*[2/3*h2*cos(alf1);2/3*h2*sin(alf1);0]+[xf;yf;zf]
Rot*[2/3*h2*cos(alf2);-2/3*h3*sin(alf2);0]+[xf;yf;zf]];
X = [ xf yf zf phif thetaf psif pf qf betaf];
Sf = [ 1; 2; pi/4];
dSf = [0;0;0];
Pf = [ 0.1*t; 0; 0.5*sin(0.03*pi*t) + 2; 0; 0.01*pi*t ; 0];
dPf = [ 0.1; 0; 0.5*0.03*pi*cos(0.03*pi*t); 0; 0.01*pi; 0];
qd = [Pf; Sf];
qddot = [dPf; dSf];
jac = jacobian(eps,X);
%%%%%%%%%%%%%
pf = Sf(1); qf = Sf(2); betaf = Sf(3);
xf = Pf(1); yf = Pf(2); zf = Pf(3);
phif = Pf(4); thetaf = Pf(5); psif = Pf(6);
x1=pos(1);
y1=pos(2);
z1=pos(3);
x2=pos(4);
y2=pos(5);
z2=pos(6);
x3=pos(7);
y3=pos(8);
z3=pos(9);
qpf=[(x1+x2+x3)/3;...
(y1+y2+y3)/3;...
(z1+z2+z3)/3;...
atan2((2*z1/3-z2/3-z3/3),(2*y1/3-y2/3-y3/3)); ...
-atan2((2*z1/3-z2/3-z3/3),(2*x1/3-x2/3-x3/3)); ...
atan2((2*y1/3-y2/3-y3/3),(2*x1/3-x2/3-x3/3))];
qsf=[sqrt((x1-x2)^2+(y1-y2)^2+(z1-z2)^2); ...
sqrt((x1-x3)^2+(y1-y3)^2+(z1-z3)^2); ...
acos((pf^2+qf^2-rf^2)/(2*pf*qf))];
q = [qpf;qsf];
%%%%%%%%%%%%%
%%%pos_desired%%%%%%%
pos_des = eval(eps);
pos_des =subs(pos_des,t,tm);
jacval = eval(jac);
qd = eval(qd);%subs(qd,t,tm);
q = eval(q);
qe = qd - q;
qddot = eval(qddot);%subs(qddot,t,tm);
kappa=0.2*eye(9);
qrefdot = qddot + kappa*qe;
vel_des = jacval*qrefdot;
vel_des = subs(vel_des,t,tm);
acc_des = zeros(3,1);
yaw = 0;
yawdot = 0;
% =================== Your code ends here ===================
desired_state1 = [pos_des(1:3);vel_des(1:3);acc_des];
desired_state2 = [pos_des(4:6);vel_des(4:6);acc_des];
desired_state3 = [pos_des(7:9);vel_des(7:9);acc_des];
Pf = subs(Pf,t,tm);
Sf = subs(Sf,t,tm);
format short
digits(3);
desired_state = vpa([desired_state1;desired_state2;desired_state3; psif; 0;
Pf; Sf])
size(desired_state)
end

Answer 1

The second image shows that the output of the second function is a scalar - the dimension at the output of the block is 1 - not 38 as you believe it is.
That is, your functions do not give the same output as you believe they do.
The error is because the Selector blocks expect their inputs to be dimension 38, and they are not.
To determine why what you believe is happening is not actually what is happening, you could use the editor to set a break point in the m-code, run the model, then step through the code to determine why it gives a scalar output when you expect it to do otherwise.
Another approach would be to run your functions from the MATLAB Command Line with fake input data. Something like
tmp = desiredtrajectory(randn(10,1))
would be appropriate here.
The answer is that desiredtrajectory outputs desired_state, which is a symbolic variable. Yes it contains a 38 element vector, but Simulink treats the object itself is a scalar.
The real problem though is that you can't propagate a symbolic variable down a Simulink signal. You need the output to be a numeric vector.
One way to overcome this is to put the line
desired_state = double(desired_state);
at the end of your file to cast the symbolic object to a double, which will have 38 elements.
(However it's not clear why you are using symbolic math in the first place, and I'd suggest it would be better, and is certainly more efficient, if you didn't use it.)