Error using plot Vectors must be the same length.

clc, clear, close all


% ========================================================================
% ========================= INITIALIZATION PHASE =========================
% ========================================================================

iter = 0; T = 0; % initial iteration and time vector

% ========================================================================
% ======= All changes in the initial conditions of the trajectory ========
% ======== and the initial position of the robot are done here ===========
% ========================================================================

xr = 0; yr = -6; thr = 0; % ### initial reference states
x = -0.7; y = 0; th = -90*pi/180; % ### initial robot states

%x = -0.7; y = 0; th = -pi/2; % ### initial robot states 1
%x = -0.7; y = 0.5; th = -pi; % ### initial robot states 2
%x = -0.9; y = 0.6; th = -pi/180; % ### initial robot states 3

%xr = 0; yr = -0.6; thr = 0*pi/180; % ### initial reference states 4
%x = 1; y = 1; th = 0; % ### initial robot states 4

Xr = xr; Yr = yr; THr = thr; % initial reference states vectors
X = x; Y = y; TH = th; % initial robot states vectors

% ========================================================================
% ========================================================================


Vr = []; Wr = []; % initial reference velocities vectors
V = []; W = []; % initial robot velocities vectors


% ========================================================================
% ======================== SIMULATION PARAMETERS =========================
% ========================================================================

thetaplot = linspace(0,2*pi,180);
rlength = 0.06; rwidth = 0.04;

ts = 0.05; tstop = 20;

% ==================== Bounds on robot velocities ========================
vmax = 0.4; wmax = pi/2; % (A)
%vmax = 0.9; wmax = pi;  % (B)

% ========================================================================
% ============================ SIMULATION LOOP ===========================
% ========================================================================

while T(end)<=tstop % loop until current time exceeds stop time

    clf, figure(1), hold on % close the previous and open a new figure

    set(gcf,'color',[1,1,1]) % set figure properties
    set(gca,'fontname','times','fontsize',14,'box','on')

    xlabel('x [m]','fontname','times','fontsize',14)
    ylabel('y [m]','fontname','times','fontsize',14)

    % ====================================================================
    % ============ Two cases of simulations will be used: ================
    % ====== either the reference velocities (vr,wr) are given ===========
    % ======= or the reference trajectory (x(t),y(t) is given ============
    % ====================================================================

    % ====================================================================
    % ======== In the case that the reference velocities are given =======
    % =================== then insert them here ==========================
    % ====================================================================
    
    vr = 0.08+abs(0.05*sin(2*pi/tstop*iter*ts)); % current reference translational velocity
    wr = 0.5*sin(2*pi/tstop*iter*ts); % current reference rotational velocity

   % vr = 0.08+abs(0.08*sin(0.1*pi/tstop*iter*ts)); % current reference translational velocity 1 AND 2
   % wr = 0.5*sin(0.1*pi/tstop*iter*ts); % current reference rotational velocity 1 AND 2
   
   % vr = 0.3+abs(0.08*sin(0.1*pi/tstop*iter*ts)); % current reference translational velocity 3
   % wr = 0.7*sin(0.1*pi/tstop*iter*ts); % current reference rotational velocity 3
   
%     xr = (20-t)^3+(9*(20-t)^2*t)+(12*(20-t)*t^2+2*t^3) /8000; % 4
%     yr = (20-t)^3+(3*(20-t)^2*t)+(9*(20-t)*t^2)/8000;         % 4
%      x =   1  ; y = 1 ;  thr = 0 ;                            % 4
   

   
    % ====================================================================
    % ====== In the case that the reference position is given then =======
    % =========== use the Inverse Kinematics formulas to calculate them ==
    % =========== and comment out the previous lines of the code =========
    % ====================================================================
    
    % vr = ;
    % wr = ;
    
   % Vr = [Vr;vr]; Wr = [Wr;wr]; % insert current reference velocities into reference velocities vectors

    % ====================================================================
    % ==================== REFERENCE ROBOT ===============================
    % ====================================================================
     
    % ====================================================================
    % ======== In the case that the reference velocities are given =======
    % ====== then use this code to calculate the Forward Kinematics ======
    % ====================================================================
    
    [tsolr,xsolr] = ode45(@DifferentialDriveKinematics,[iter*ts (iter+1)*ts],[xr,yr,thr]',[],[vr,wr]); % call ode45 to compute current reference states

    xsolr = xsolr(end,:); tsolr = tsolr(end); % keep the last index
    xr = xsolr(1); yr = xsolr(2); thr = xsolr(3); % these are the current reference states
  
    % ====================================================================
    % === In the case that the reference position is given then insert ===
    % =========== the formulas for the reference trajectory here =========
    % =========== and comment out the previous lines of the code =========
    % ====================================================================
    
    % xr = ;
    % yr = ;
    % thr = ;
    
   % Xr = [Xr;xr]; Yr = [Yr;yr]; THr = [THr;thr]; % insert current reference states into reference states vector

    % ====================================================================
    % ======================= CONTROLLER DESIGN ==========================
    % ====================================================================
    v = vr;
    w = wr;
    
    z = 0.5; 
    g = 20; 
    

    % =============== Application of velocity constraints ================ 
    v = sign(vr)*min([abs(vr),vmax]); 
    w = sign(wr)*min([abs(wr),wmax]);

    V = [V;v]; W = [W;w]; % insert the current velocities into the robot velocities vector


    % ====================================================================
    % ====================== REAL ROBOT ==================================
    % ====================================================================
       
    [tsol,xsol] = ode45(@DifferentialDriveKinematics,[iter*ts (iter+1)*ts],[x,y,th]',[],[v,w]); % call ode45 to compute current robot states

    xsol = xsol(end,:); tsol = tsol(end); % keep the last index
    x = xsol(1); y = xsol(2); th = xsol(3); % these are the current robot states

    T = [T;tsol]; % insert current time into time vector
    X = [X;x]; Y = [Y;y]; TH = [TH;th]; % insert current robot states into robot states vector

    
    
    % ====================================================================
    % ============================ PLOT FRAME ============================
    % ====================================================================
    
    plot(Xr,Yr,'b-','linewidth',2) % plot the reference path
    plot(xr,yr,'bo','markersize',7,'markerfacecolor','b') % plot the current reference position

    plot(X,Y,'r-','linewidth',2) % plot the robot path
    fill(x + cos(th)*rlength*cos(thetaplot) - sin(th)*rwidth*sin(thetaplot),...
        y + sin(th)*rlength*cos(thetaplot) + cos(th)*rwidth*sin(thetaplot),[0.8 0 0],'EdgeColor','none','FaceAlpha', 0.8);
    
    plot([x+cos(th)*(-rlength/3)-sin(th)*rwidth x+cos(th)*(rlength/3)-sin(th)*rwidth],...
        [y+sin(th)*(-rlength/3)+cos(th)*rwidth y+sin(th)*(rlength/3)+cos(th)*rwidth],'k','linewidth',5);
    
    plot([x+cos(th)*(-rlength/3)-sin(th)*(-rwidth) x+cos(th)*(rlength/3)-sin(th)*(-rwidth)],...
        [y+sin(th)*(-rlength/3)+cos(th)*(-rwidth) y+sin(th)*(rlength/3)+cos(th)*(-rwidth)],'k','linewidth',5);
    
    
    title(sprintf('Blue: Reference Robot -  Red: Real Robot\n Current time [sec]: %.2f',T(end)),...
        'fontname','times','fontsize',14) % insert a title with current simulation time

    axis tight, axis equal % set the axes to fill the plot and equalize the axes

    pause(0.04); % a small pause


    % ============================ NEXT LOOP =============================
    % ====================================================================
    iter=iter+1; % go to the next iteration

end % end the simulation loop

% ========================================================================
% ================= PLOTS AFTER SIMULATION ENDS ==========================
% ========================================================================

% Plot the error between reference and real robot states
figure(2), hold on
set(gcf,'color',[1,1,1])
set(gca,'fontname','times','fontsize',14,'box','on')
xlabel('t [sec]','fontname','times','fontsize',14)
ylabel('errors','fontname','times','fontsize',14)
plot(T,Xr-X,'b-','linewidth',2), plot(T,Yr-Y,'r-','linewidth',2)
plot(T,THr-TH,'g-','linewidth',2), plot([0,T(end)],[0,0],'k:')
legend('e_x [m]','e_y [m]','e_\theta [rad]','steady state','location','best'), hold off
axis tight


% Plot the transformed errors
figure(3), hold on
set(gcf,'color',[1,1,1])
set(gca,'fontname','times','fontsize',14,'box','on')
xlabel('t [sec]','fontname','times','fontsize',14)
ylabel('transformed errors','fontname','times','fontsize',14)
E1=cos(TH).*(Xr-X)+sin(TH).*(Yr-Y);
E2=-sin(TH).*(Xr-X)+cos(TH).*(Yr-Y);
E3=THr-TH;
plot(T,E1,'b-','linewidth',2), plot(T,E2,'r-','linewidth',2)
plot(T,E3,'g-','linewidth',2), plot([0,T(end)],[0,0],'k:')
legend('e_1 [m]','e_2 [m]','e_3 [rad]','steady state','location','best'), hold off
axis tight


% Plot the reference and real robot translational velocities
figure(4), hold on
set(gcf,'color',[1,1,1])
set(gca,'fontname','times','fontsize',14,'box','on')
xlabel('t [sec]','fontname','times','fontsize',14)
ylabel('translational velocities','fontname','times','fontsize',14)
plot(T(1:end-1),V,'r-','linewidth',2), plot(T(1:end-1),Vr,'b-','linewidth',2)
legend('\upsilon [m/sec]','\upsilon^r [m/sec]','location','best'), hold off
axis tight


% Plot the reference and real robot rotational velocities
figure(5), hold on
set(gcf,'color',[1,1,1])
set(gca,'fontname','times','fontsize',14,'box','on')
xlabel('t [sec]','fontname','times','fontsize',14)
ylabel('rotational velocities','fontname','times','fontsize',14)
plot(T(1:end-1),W,'r-','linewidth',2), plot(T(1:end-1),Wr,'b-','linewidth',2)
legend('\omega [rad/sec]','\omega^r [rad/sec]','location','best'), hold off
axis tight

-------------------------------------------------------------------------------

the function (that is in a sperate .m file):

function Xdot=DifferentialDriveKinematics(t,X,U)

% This is a function used to simulate the Amigobot's kinodynamic model :
%
% xdot = v cos(theta)
% ydot = v sin(theta)
% xdot = w 
%
% Its arguments are :  U = (v, w) and X = (x, y, theta), along with the
% simulation time interval t.


Xdot=[U(1)*cos(X(3));
      U(1)*sin(X(3));
      U(2)];

[Walter Roberson]

you posted over 300 lines of code but do not show us which line the error is on.

[Kunal Kundalia]

the error occurs after it has ran as the code is displaying the trajectory of two robots one is the real the other is the refrence, they dont seem to ever meet which is why the error occurs about the plot length, it was code provided to me to learn about trajectroy control but i am not sure what i am doing wrong, probley something very simple.

Answer 1

[Walter Roberson]

You initialize Vr with capital V, and you plot() Vr with capital V.

Inside your loop you assign values to vr with lower-case v .

Each value that you assign to vr with lower-case v is a scalar.

So probably you need to insert

Vr = [Vr; vr];