Clear Filters
Clear Filters

Output argument "St" (and maybe others) not assigned during call to "function ".

1 view (last 30 days)
I got an error
Output argument "St" (and maybe others) not assigned during call to "get_StCarretero".
Error in InverseVelocityCarretero (line 25)
[St,p]=get_StCarretero(t,P)
Error in RK4_PhRS (line 21)
[k1,~,~,P1,~,~,~,q1] = F(t,th,P,q);
Error in CarreterosPRS_20200812_Velocity (line 24)
[JointValue,Stm,MM,Pose,G,FRK,Rd,q_out,Pd]=
RK4_PhRS(@InverseVelocityCarretero,0,ts,1,th,p,q);
when running the following program.
function [St,p] = get_StCarretero(t,pint)
ts = 1/1000;
rp=1000;
L=1000;
alpha=2*pi/3;
beta=4*pi/3;
A=cos(alpha)-cos(beta);
B=sin(alpha)-sin(beta);
C=(cos(beta)-1)/tan(beta)-(cos(alpha)-1)/tan(alpha);
z=707.1068;
if t==0
x=pint(1);
y=pint(2);
z=pint(3);
else
th=0.2*cos(2*pi*t);
psi=0.2*sin(2*pi*t);
R=A*(cos(th)-cos(psi))+B*cos(th)*sin(psi);
S=A*sin(th)*sin(psi)-B*cos(th)+C*cos(psi);
phi=atan(-R/S);
x=-rp*(cos(th)*cos(phi)+sin(psi)*sin(th)*sin(phi))*cos(alpha)-rp*(-cos(th)*sin(phi)+sin(psi)*sin(th)*cos(phi))*sin(alpha)+(rp/tan(alpha))*(cos(psi)*sin(phi)*(cos(alpha)-1)+cos(psi)*cos(phi)*sin(alpha));
y=-cos(psi)*sin(phi)*rp;
p=[x;y;z]
yaw = phi;
pitch= th;
roll = psi;
vx=diff([x])/ts;
vy=diff([y])/ts;
vz=diff([z])/ts;
droll=diff([psi0,roll])/ts;
dpitch=diff([th0,pitch])/ts;
dyaw=diff([phi0,yaw])/ts ;
JT=[cos(pitch)*cos(yaw) sin(yaw) 0; cos(pitch)*sin(yaw) cos(yaw) 0; -sin(pitch) 0 0];
omg=JT*[dpitch;droll;dyaw];
wx=omg(1);
wy=omg(2);
wz=omg(3);
St =[vx;vy;vz;wx;wy;wz];
end
end
Any help is apperciated
Thank you
  9 Comments
HN
HN on 22 Aug 2020
Edited: HN on 22 Aug 2020
Walter Roberson, Thank you so much.
Now, things get better. However, the code below always differentiate the current value from initial value. Not previous value. initialized variables always remember the first value than previous one. Where did I make a mistake ?
function [S] = get_St3PhRS(t)
persistent th0 psi0 phi0 x0 y0 z0 vx vy vz w
initializing = (t == 0 || isempty(th0) || isempty(psi0) || isempty(phi0) || isempty(x0) || isempty(y0) || isempty(z0));
if initializing
ts=1/1000;
rp=1000; % Radius of the base plate in mm
th=-0.2*cos(2*pi*t);
psi=0.2*sin(2*pi*t);
z=707.1068;
phi=atan2(sin(psi)*sin(th),(cos(psi)+cos(th)));
T=Rot('y',th)*Rot('x',psi)*Rot('z',phi)
x=1/2*rp*(-cos(phi)*cos(psi)+cos(phi)*cos(th)+sin(phi)*sin(psi)*sin(th));
y=-rp*cos(psi)*sin(phi);
Pose=[x;y;z;th;psi;phi]
x0=x;
y0=y;
z0=z;
th0=th;
psi0=psi;
phi0=phi;
vx=(x-x0)/ts;
vy=(y-y0)/ts;
vz=(z-z0)/ts;
dth=(th-th0)/ts; %(2*pi*sin(2*pi*t))/5;
dpsi=(psi-psi0)/ts; %(2*pi*cos(2*pi*t))/5;
dphi=(phi-phi0)/ts;%(cos(psi)*sigma1*dpsi+cos(th)*dpsi-cos(psi)*sin(psi)*sin(th)*dth)/(-sigma2*sigma1+sigma2+2*cos(psi)*cos(th)+2*sigma1);
JT=[0, cos(th), cos(psi)*sin(th);1, 0, -sin(psi);0, -sin(th), cos(psi)*cos(th)];
dTh=[dth; dpsi;dphi]
w=JT*dTh;
S=[vx;vy;vz;w(1,:);w(2,:);w(3,:)];
else
ts=1/1000;
rp=1000; % Radius of the base plate in mm
th=-0.2*cos(2*pi*t);
psi=0.2*sin(2*pi*t);
z=707.1068;
phi=atan2(sin(psi)*sin(th),(cos(psi)+cos(th)));
T=Rot('y',th)*Rot('x',psi)*Rot('z',phi)
x=1/2*rp*(-cos(phi)*cos(psi)+cos(phi)*cos(th)+sin(phi)*sin(psi)*sin(th));
y=-rp*cos(psi)*sin(phi);
vx=(x-x0)/ts;
vy=(y-y0)/ts;
vz=(z-z0)/ts;
dth=(th-th0)/ts; %(2*pi*sin(2*pi*t))/5;
dpsi=(psi-psi0)/ts; %(2*pi*cos(2*pi*t))/5;
dphi=(phi-phi0)/ts;
JT=[0, cos(th), cos(psi)*sin(th);1, 0, -sin(psi);0, -sin(th), cos(psi)*cos(th)];
dTh=[dth; dpsi;dphi]
w=JT*dTh;
S=[vx;vy;vz;w(1);w(2);w(3)];
end
end
Walter Roberson
Walter Roberson on 22 Aug 2020
After you have done the calculations, but before you return, you need to update x0, y0, z0 with the current x, y, z.

Sign in to comment.

Answers (0)

Community Treasure Hunt

Find the treasures in MATLAB Central and discover how the community can help you!

Start Hunting!