I am getting an error 'Index in position 2 is invalid. Array indices must be positive integers or logical values.' please help ? Thanks in advance.

clf
clc
clear all
close all

%Obstacle #1
Obpf1= 4.5;
Obs1 = [275 0];

%Obstacle #2
Obpf2= 4;
Obs2 = [200 90];

%Obstacle #3
Obpf3= 3;
Obs3 = [200 190];

%Obstacle #4
Obpf4= 2;
Obs4 = [200 290];

%Moving Obstacle
Obpf5 =2;
Obs5 = [125 0];

%Initial Goal 1
Gpf1 =1;
Gi1 = [170 90];

%Initial Goal 2
Gpf2 =1;
Gi2 = [170 190];

%Initial Goal 3
Gpf3 =1;
Gi3 = [170 290];

%Secondary Goal 1
Gsf1 = 1.5;
Gs1 = [0 145];

%Secondary Goal 2
Gsf2 = 1.5;
Gs2 = [0 195];

%Secondary Goal 3
Gsf3 = 1.5;
Gs3 = [0 245];

%Robot 1
R1pf = 1;
R1 = [20 372];

%Robot 2
R2pf = 1;
R2 = [50 372];

%Robot 3
R3pf = 1;
R3 = [80 372];

%%%%%%%%%%%%%%%%%%%%%%%%%%%
%Initial Robot Environment
%%%%%%%%%%%%%%%%%%%%%%%%%%%

rectangle('position',[275 0 150 400],'Facecolor','red','edgecolor','black','linewidth',1.5)  % sortation area 
hold on

rectangle('position',[200 90 75 20],'Facecolor','yellow')  %CHUTE NUMBER 1
rectangle('position',[200 190 75 20],'Facecolor','yellow')  %CHUTE NUMBER 2
rectangle('position',[200 290 75 20],'Facecolor','yellow')  %CHUTE NUMBER 3
hold on

rectangle('position',[168 88 20 20],'curvature',[1,1],'Facecolor','black')  %static robot 1
rectangle('position',[170 190 20 20],'curvature',[1,1],'Facecolor','black') %static robot 2
rectangle('position',[170 290 20 20],'curvature',[1,1],'Facecolor','black')  %static robot 3
hold on 

rectangle('position',[0 145 20 18],'facecolor','b') %truck station 1
rectangle('position',[0 195 20 18],'facecolor','b') %truck station 2
rectangle('position',[0 245 20 18],'facecolor','b') %truck station 3
hold on

human_obst(125,0,5,1)
auto_robot(20,372.5,10,1)
auto_robot(50,372.5,10,1)
auto_robot(80,372.5,10,1)

title('Autonomous Warehouse Environment')

axis([0 400 0 400]);%whole environment area 

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Create Repulsive and attractive potential field forces %
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%Matrix X x Y of  400 by 400
i=1:400;
j=1:400;
[I,J] = meshgrid(i,j);
Z = []; %empty matrix for z-axis plot

for i=1:400 
    for j=1:400 
        pos = [j/10 i/10]; % scaled to fit 400 by 400 matrix
        z=0;    %accumulator
        
        %Obstacles with exponential curve while goal is by vector  
        z=z+exp(-norm(pos-Obs1)/Obpf1)+exp(-norm(pos-Obs2)/Obpf2)+ ...
            exp(-norm(pos-Obs3)/Obpf3)+exp(-norm(pos-Obs4))/Obpf4+ ...
            exp(-norm(pos-Obs5))/Obpf5+ ...
            Gpf1*(norm(Gi1-pos))+ Gpf2*(norm(Gi2-pos))+Gpf3*(norm(Gi3-pos)+...
            Gsf1*(norm(Gs1-pos))+ Gsf2*(norm(Gs2-pos))+Gsf3*(norm(Gs3-pos)));
        % The potential functions chosen are similar to the function 
        % proposed in the document 'apf.pdf'
        Z(i,j) = z; %store in matrix
        
    end
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Path planning for Robot % 
%%%%%%%%%%%%%%%%%%%%%%%%%%%

%Plot current robot motion
t1=(abs(R1(1)-Gi1(1))+abs(R1(2)-Gi1(2))); 
t2=(abs(R2(1)-Gi2(1))+abs(R2(2)-Gi2(2)));
t3=(abs(R3(1)-Gi3(1))+abs(R3(2)-Gi3(2)));

count=1;

while t1>0.5 || t2 >0.5 || t3 > 0.5 %check if the robot had reached the specified target;  
    %stop when reach target
    
    dx1=[0 0];  %accumulation matrix robot1
    dx2=[0 0];  %accumulation matrix robot2
    dx3=[0 0];  %accumulation matrix robot3
    
    %Obstacles Accumulate vector sum  %Goal Accumulate - Robot 1
    dx1= dx1 + (R1-R2)*exp((-norm(R1-R2))/R2pf) + ...
        (R1-R3)*exp((-norm(R1-R3))/R3pf) + ...
        (R1-Obs1)*exp((-norm(R1-Obs1))/Obpf1)+ ...
        (R1-Obs2)*exp((-norm(R1-Obs2))/Obpf2)+ ...
        (R1-Obs3)*exp((-norm(R1-Obs3))/Obpf3)+ ...
        (R1-Obs4)*exp((-norm(R1-Obs4))/Obpf4)+ ...
        (R1-Obs5)*exp((-norm(R1-Obs5))/Obpf5)+ ...
        Gpf1*(Gi1-R1)/norm(Gi1-R1);
    
    
    %Obstacles Accumulate vector sum  %Goal Accumulate - Robot 2
    
    dx2= dx2 + (R2-R1)*exp((-norm(R2-R1))/R2pf) + ...
        (R2-R3)*exp((-norm(R2-R3))/R3pf) + ...
        (R2-Obs1)*exp((-norm(R2-Obs1))/Obpf1)+ ...
        (R2-Obs2)*exp((-norm(R2-Obs2))/Obpf2)+ ...
        (R2-Obs3)*exp((-norm(R2-Obs3))/Obpf3)+ ...
        (R2-Obs4)*exp((-norm(R2-Obs4))/Obpf4)+ ...
        (R2-Obs5)*exp((-norm(R2-Obs5))/Obpf5)+ ...
        Gpf2*(Gi2-R2)/norm(Gi2-R2);
    
    %Obstacles Accumulate vector sum  %Goal Accumulate - Robot 3
    
    dx3= dx3 + (R3-R1)*exp((-norm(R3-R1))/R1pf) + ...
        (R3-R2)*exp((-norm(R3-R2))/R2pf) + ...
        (R3-Obs1)*exp((-norm(R3-Obs1))/Obpf1)+ ...
        (R3-Obs2)*exp((-norm(R3-Obs2))/Obpf2)+ ...
        (R3-Obs3)*exp((-norm(R3-Obs3))/Obpf3)+ ...
        (R3-Obs4)*exp((-norm(R3-Obs4))/Obpf4)+ ...
        (R3-Obs5)*exp((-norm(R3-Obs5))/Obpf5)+ ...
        Gpf3*(Gi3-R3)/norm(Gi3-R3);
    
    R1=R1+dx1;
    R2=R2+dx2;
    R3=R3+dx3;
    
    t1=(abs(R1(1)-Gi1(1))+abs(R1(2)-Gi1(2))); 
    t2=(abs(R2(1)-Gi2(1))+abs(R2(2)-Gi2(2))); 
    t3=(abs(R3(1)-Gi3(1))+abs(R3(2)-Gi3(2))); 
    count=count+1; 
    
    plot3(R1(1,1), R1(1,2), Z(round(R1(1,2)), round(R1(1,1))),'K.','MarkerSize',10);
    plot3(R2(1,1), R2(1,2), Z(round(R2(1,2)), round(R2(1,1))),'R.','MarkerSize',10);
    plot3(R3(1,1), R3(1,2), Z(round(R3(1,2)), round(R3(1,1))),'G.','MarkerSize',10);
    
    
    title(sprintf('Iteration: %d',count));
    refresh;
    drawnow;
end

t4=(abs(R1(1)-Gs1(1))+abs(R1(2)-Gs1(2))); 
t5=(abs(R2(1)-Gs2(1))+abs(R2(2)-Gs2(2)));
t6=(abs(R3(1)-Gs3(1))+abs(R3(2)-Gs3(2)));

while t4>0.6 || t5 >0.6 || t6 > 0.6 %check if the robot had reached the specified target;  
    %stop when reach target
    
    dx4=[0 0];  %accumulation matrix robot1
    dx5=[0 0];  %accumulation matrix robot2
    dx6=[0 0];  %accumulation matrix robot3
    
    %Obstacles Accumulate vector sum  %Goal Accumulate - Robot 1
    dx4= dx4 + (R1-R2)*exp((-norm(R1-R2))/R2pf) + ...
        (R1-R3)*exp((-norm(R1-R3))/R3pf) + ...
        (R1-Obs1)*exp((-norm(R1-Obs1))/Obpf1)+ ...
        (R1-Obs2)*exp((-norm(R1-Obs2))/Obpf2)+ ...
        (R1-Obs3)*exp((-norm(R1-Obs3))/Obpf3)+ ...
        (R1-Obs4)*exp((-norm(R1-Obs4))/Obpf4)+ ...
        (R1-Obs5)*exp((-norm(R1-Obs5))/Obpf5)+ ...
        Gsf1*(Gs1-R1)/norm(Gs1-R1);
    
    
    %Obstacles Accumulate vector sum  %Goal Accumulate - Robot 2
    
    dx5= dx5 + (R2-R1)*exp((-norm(R2-R1))/R2pf) + ...
        (R2-R3)*exp((-norm(R2-R3))/R3pf) + ...
        (R2-Obs1)*exp((-norm(R2-Obs1))/Obpf1)+ ...
        (R2-Obs2)*exp((-norm(R2-Obs2))/Obpf2)+ ...
        (R2-Obs3)*exp((-norm(R2-Obs3))/Obpf3)+ ...
        (R2-Obs4)*exp((-norm(R2-Obs4))/Obpf4)+ ...
        (R2-Obs5)*exp((-norm(R2-Obs5))/Obpf5)+ ...
        Gsf2*(Gs2-R2)/norm(Gs2-R2);
    
    %Obstacles Accumulate vector sum  %Goal Accumulate - Robot 3
    
    dx6= dx6 + (R3-R1)*exp((-norm(R3-R1))/R1pf) + ...
        (R3-R2)*exp((-norm(R3-R2))/R2pf) + ...
        (R3-Obs1)*exp((-norm(R3-Obs1))/Obpf1)+ ...
        (R3-Obs2)*exp((-norm(R3-Obs2))/Obpf2)+ ...
        (R3-Obs3)*exp((-norm(R3-Obs3))/Obpf3)+ ...
        (R3-Obs4)*exp((-norm(R3-Obs4))/Obpf4)+ ...
        (R3-Obs5)*exp((-norm(R3-Obs5))/Obpf5)+ ...
        Gsf3*(Gs3-R3)/norm(Gs3-R3);
    
    R1=R1+dx4;
    R2=R2+dx5;
    R3=R3+dx6;
    
    t4=(abs(R1(1)-Gs1(1))+abs(R1(2)-Gs1(2))); 
    t5=(abs(R2(1)-Gs2(1))+abs(R2(2)-Gs2(2))); 
    t6=(abs(R3(1)-Gs3(1))+abs(R3(2)-Gs3(2))); 
    
    count=count+1; 
    
    plot3(R1(1,1), R1(1,2), Z(round(R1(1,2)), round(R1(1,1))),'K.','MarkerSize',10);
    plot3(R2(1,1), R2(1,2), Z(round(R2(1,2)), round(R2(1,1))),'R.','MarkerSize',10);
    plot3(R3(1,1), R3(1,2), Z(round(R3(1,2)), round(R3(1,1))),'G.','MarkerSize',10);
    
    
    title(sprintf('Iteration: %d',count));
    refresh;
    drawnow;
end

Answer 1

[DGM]

Using round() to generate array indices does not guarantee that the results are within [1 size(A,thisdim)]. In this case, the column subscript is 0.

plot3(R1(1,1), R1(1,2), Z(round(R1(1,2)), round(R1(1,1))),'K.','MarkerSize',10);
plot3(R2(1,1), R2(1,2), Z(round(R2(1,2)), round(R2(1,1))),'R.','MarkerSize',10);
plot3(R3(1,1), R3(1,2), Z(round(R3(1,2)), round(R3(1,1))),'G.','MarkerSize',10);

How you handle this depends on your needs. It may suffice to clamp the values at the edges of the array using min() and max(). Something like this:

% assuming arrays are 400x400
thisr = min(max(round(R1),1),400);
plot3(R1(1,1), R1(1,2), Z(thisr(2),thisr(1)),'K.','MarkerSize',10);
thisr = min(max(round(R2),1),400);
plot3(R2(1,1), R2(1,2), Z(thisr(2),thisr(1)),'R.','MarkerSize',10);
thisr = min(max(round(R3),1),400);
plot3(R3(1,1), R3(1,2), Z(thisr(2),thisr(1)),'G.','MarkerSize',10);