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

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

[Stephen23]

When t==0 what values do you allocate to the output arguments St and p ? (hint: none, you do not define them). So what do you expect MATLAB to return when t==0 ?

You need to define some values for St and p for t==0.

[HN]

Thank you so much Stephen Cobeldick ,

Is there any way that I can keep preveous value of x y z to solve diff? It keep reurn zero for St after each iteration.

Thank you

[Walter Roberson]

        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));

That looks like a scalar. diff(x) would be empty.

To remember x y z values from previous calls see

 persistent

[HN]

Walter Roberson,

When I call get_St3PhRS in another function which has a for loop (RK4_RhPRS), S gives always zero because it doesn't retain previous value to calculate the derivative. I tried to use persistent, but not well implemented. Can you give me some more help?

Thank you

% time function is to create a time vector   
function time(t)    
    persistent n
    n=t;
    if isempty(n)
        n = 0;
    end
    n = n+1
end

function [Pose, S] = get_St3PhRS(t) 
         ts=1/1000;
         tt=time(t);
         rp=1000;   % Radius of the base plate in mm 
         th=-0.2*cos(2*pi*tt);
         psi=0.2*sin(2*pi*tt);
         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]
         
         % derivative of moving plate rotation angles
          vx=gradient(x,ts);
          vy=gradient(y,ts);
          vz=gradient(z,ts);
          dth=gradient(th,ts);  %(2*pi*sin(2*pi*t))/5;
          dpsi=gradient(psi,ts); %(2*pi*cos(2*pi*t))/5;          
          dphi=gradient(phi,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)];
end 


function [dtheta,Stm,MM,V,G,FRK,R,dq,P]  = InverseVelocityPhRs20200820(T,th,~,q)
        alpha=2*pi/3;
        beta=4*pi/3;
        L=1000;
        b1 = th(1);
        b2 = th(2);
        b3 = th(3);
        th21 = th(4);
        th22 = th(5);
        th23 = th(6);
        
        s11=[1;0;0];                      % first joint (active) screw of leg 1
        s12=[cos(alpha);sin(alpha);0];    % first joint (active) screw of leg 2
        s13=[cos(beta);sin(beta);0];
        
         s21=[0;1;0];                      % second joint (passive) screw of leg 1
         s22=[-sin(alpha);cos(alpha);0];   % second joint (passive) screw of leg 2
         s23=[-sin(beta);cos(beta);0];
         S=get_St3PhRS(T)  % Here is where the function is called 
end 

function [thout,Stm_out,MM_out,Pout,G_out,FRK_OUT,R_out,q_out,Pd_out] = RK4_RhPRS(F,t0,h,tfinal,y0,p0,q0)
   % ODE4  Classical Runge-Kutta ODE solver.
      th = y0;
      R = eye(3);
      P =  p0;
      q =  q0;
      q_out=q;
      thout = th;
      Pout = P;
      Pd=[-9.9667;0;707.1068];
      Pd_out=Pd;
      Stm=[0;0;0;1.1080;0; 0.8593];
      Stm_out = Stm;
      MM_out = [];
      G_out = [];      
      R_out=R;
      FRK=[0;0;0;1.1080;0; 0.8593];
      FRK_OUT = FRK;
      for t = t0 : h : tfinal-h
         [k1,~,~,P1,~,~,~,q1] = F(t,th,P,q); % Error in RK4_RhPRS (line 20) 
         :
         :
      end 
end 

% Main function call 
[J,S,M,P,G,F,R,q_out,Pd]= RK4_RhPRS(@InverseVelocityPhRs20200820,0,ts,1,th,P,q);
%% Error
Error using time
Too many output arguments.

Error in get_St3PhRS (line 3)
         tt=time(t); % t contains all t values but 

Error in InverseVelocityPhRs20200820 (line 19)
         S=get_St3PhRS(T)

Error in RK4_RhPRS (line 20)
         [k1,~,~,P1,~,~,~,q1] = F(t,th,P,q);

Error in Main3PhRS_20200820 (line 103)
[J,S,M,P,G,F,R,q_out,Pd]= RK4_RhPRS(@InverseVelocityPhRs20200820,0,ts,1,th,P,q);
 

[Walter Roberson]

persistent old_x old_y old_z
initializing = (t == 0 || isempty(old_x));

if initializing
    do whatever is appropriate for first round
    old_x = x;
    old_y = y;
    old_z = z;
else
    do whatever is appropriate for other rounds,
    making use of x, y, z, and old_x, old_y, old_z
end

[HN]

Thank you Walter Roberson,

1: Can I initialize all variables like what I did below ? But it gives the error shown below.

Also, list of initialization cause the same result...S=0 vector in each iteration.

2: Why I am unable to accept your answer ?

function [Pose, S] = get_St3PhRS(t) 
persistent th0 psi0 phi0 x0 y0 z0 
initializing = (t == 0 || isempty(th0,psi0,phi0,x0,y0,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=gradient(x,ts);
          vy=gradient(y,ts);
          vz=gradient(z,ts);
          dth=gradient(th,ts);  %(2*pi*sin(2*pi*t))/5;
          dpsi=gradient(psi,ts); %(2*pi*cos(2*pi*t))/5;          
          dphi=gradient(phi,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
         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);
         
         % derivative of moving plate rotation angles
          vx=gradient(x,ts);
          vy=gradient(y,ts);
          vz=gradient(z,ts);
          dth=gradient(th,ts);  %(2*pi*sin(2*pi*t))/5;
          dpsi=gradient(psi,ts); %(2*pi*cos(2*pi*t))/5;          
          dphi=gradient(phi,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)];
end
end 


Error using isempty
Too many input arguments.

Error in get_St3PhRS (line 3)
initializing = (t == 0 || isempty(th0,psi0,phi0,x0,y0,z0));

Error in InverseVelocityPhRs20200820 (line 19)
         S=get_St3PhRS(T)

Error in RK4_RhPRS (line 21)
         [k2,~,~,P2,~,~,~,q2] = F(t+h/2,th+h*k1/2,P+h*P1/2,q+h*q1/2);

Error in Main3PhRS_20200820 (line 103)
[JointValue,Stm,MM,Pose,G,FRK,Rd,q_out,Pd]= RK4_RhPRS(@InverseVelocityPhRs20200820,0,ts,1,th,P,q);

Thank you

[Walter Roberson]

initializing = (t == 0 || isempty(th0) || isempty(psi0) || isempty(phi0) || isempty(x0) || isempty(y0) || isempty(z0));

However, chances are you only need to test one of the variables, because you always set all of the variables after a run, so testing any one of them for emptiness should tell you whether you have already initialized.

[HN]

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]

After you have done the calculations, but before you return, you need to update x0, y0, z0 with the current x, y, z.