from centrex=6, it should move again to 7(as repulsive force becomes zero & only attractive force exists), why does it move to %???????

%It is an example of occurence of a situation in 'potential field method' in
%which the robot is unable to pass through 2 closely spaced obstacles
%the robot is assumed to be square in shape with a width(perpendicular distance between centre and any edge)
%the obstacle is assumed to be rectangular and long SO THAT FORCE EXISTS ALONG THE LATERAL X-DIRECTION ONLY

close all;
clear all;
clc;

%creating the CONFIGURATION SPACE as x-y coordinates
AXIS([-20 20 -20 20]);

%specifications of the constants present in the force equation
Fct=3;
fcr=1;
n=1;
k=1; %specifying the steering angle
v=1;

%ROBOT SPECIFICATION
%start point of the robot which gives the initial centre position
robotix=1;
robotiy=0;
%size and orientation of the robot with respect to x axis
centrex=[];
centrey=[];
centrex(1)=robotix;
centrey(1)=robotiy;
theta=[];
theta(1)=0;
width=1;
robv1x(1)=centrex(1)-(sqrt(2)*width)*cos(theta(1)-pi/4);
robv1y(1)=centrey(1)-(sqrt(2)*width)*sin(theta(1)-pi/4);
%perpendicular distance from centre to any edge of the robot 
range=3; %square region in whiuch obstacles are sensed i.e. ACTIVE REGION

%specifying the target location
targetx=20;
targety=0;

%OBSTACLE SPECIFICATION
ob1=[10,3];
widthOB1=2;
heightOB1=3;
ob2=[10,-5];
widthOB2=3;
heightOB2=2;

for i=1:(2*range+1)
    for j=1:(2*range+1)

          C(i,j)={0};

      end

end

%robx and roby correspond to the active cells of the robot, C{i,j}
%represents their active values depending on the presence of obstacles
i=1;

delta=[];

while(1) 
     for roby=centrey(i)-range:centrey(i)+range

         for robx=centrex(i)-range:centrex(i)+range

            for kk=3:6

                for k=10:12

                   if ((robx==k) && (roby==kk)) 

                      C(robx-centrex(i)+range+1,roby-centrey(i)+range+1)={1};

                   end

                end
            end

             for kk=-5:-3

                for k=10:13

                   if ((robx==k) && (roby==kk)) 

                      C(robx-centrex(i)+range+1,roby-centrey(i)+range+1)={1};

                   end

                end
            end

D(robx-centrex(i)+range+1,roby-centrey(i)+range+1)= {sqrt(((centrex(i)-robx)^2)+((centrey(i)-roby)^2))};

D(range+1,range+1)={1}; %for code completion, else at this point distance is alwayz zero and it renders further calulation undefined!

Frepx(robx-centrex(i)+range+1,roby-centrey(i)+range+1)={(fcr*width*(C{robx-centrex(i)+range+1,roby-centrey(i)+range+1})*(centrex(i)-robx))/((D{robx-centrex(i)+range+1,roby-centrey(i)+range+1})^2)}

Frepy(robx-centrex(i)+range+1,roby-centrey(i)+range+1)={(fcr*width*(C{robx-centrex(i)+range+1,roby-centrey(i)+range+1})*(centrey(i)-roby))/((D{robx-centrex(i)+range+1,roby-centrey(i)+range+1})^2)}

         end

       end

Tfrepx=0;

for k=1:(2*range+1)
    for kk=1:(2*range+1)
      Tfrepx=Tfrepx+Frepx{k,kk};
    end
end

Tfrepy=0;%Actually it is nearly equal to zero in matlab calculations! But mathematically, this is correct! 

Fattx=Fct*(targetx-centrex(i))/(((centrex(i)-targetx)^2)+((centrey(i)-targety)^2))
Fatty=Fct*(targety-centrey(i))/(((centrex(i)-targetx)^2)+((centrey(i)-targety)^2))

Tfrepx
Rx=Tfrepx + Fattx;
Ry=Tfrepy + Fatty;

R=sqrt((Rx^2)+(Ry^2));
delta(i)=atan2(Ry,Rx)

if ((delta(i)<0) && (delta(i)>=pi))
    delta(i)=2*pi+delta(i);
end

robv1x(1)=centrex(i)-(sqrt(2)*width)*cos(theta(i)-pi/4);
robv1y(1)=centrey(i)-(sqrt(2)*width)*sin(theta(i)-pi/4);
robv1x(2)=centrex(i)+(sqrt(2)*width)*cos(theta(i)-pi/4);
robv1y(2)=centrey(i)-(sqrt(2)*width)*sin(theta(i)-pi/4);
robv1x(3)=centrex(i)+(sqrt(2)*width)*cos(theta(i)-pi/4);
robv1y(3)=centrey(i)+(sqrt(2)*width)*sin(theta(i)-pi/4);
robv1x(4)=centrex(i)-(sqrt(2)*width)*cos(theta(i)-pi/4);
robv1y(4)=centrey(i)+(sqrt(2)*width)*sin(theta(i)-pi/4);

patch(robv1x,robv1y,'g');
hold on
plot(centrex(i),centrey(i),'bo');
hold on

theta(i+1)=delta(i)

if (theta(i+1)>=2*pi)
    theta(i+1)=theta(i+1)-2*pi;
end

if (theta(i+1)<0)
    theta(i+1)=theta(i+1)+2*pi;
end

centrex(i+1) = round(centrex(i) + v*(cos(theta(i+1))))
centrey(i+1) = 0;

if (((Rx==0) && (Ry==0))||(i==8))%In this particular iteration ROBOT comes back to its previous position !!!
    display('ROBOT ACTUALLY STOPS HERE !!!!!!!');
    break;
end

z=rectangle('Position',[ob1(1),ob1(2),widthOB1,heightOB1],'Facecolor','c');
z=rectangle('Position',[ob2(1),ob2(2),widthOB2,heightOB2],'Facecolor','y');
plot(targetx,targety,'ro');

i=i+1;
end

[Walter Roberson]

Please add a (clear) question at the top of the body of your message.

Answer 1

[Vivek Bhadouria]

Yeah, Please mention your specific question.

Answer 2

[Twinkle]

Actually, i need to show that in the potential field method, if the obstacles are too near, robot cant go thru the path between the obstacles even when target pulls it in that direction. I suitably chose the values of the constants of the force(repulsive and attractive). So, in this code i have chosen a 'square' range for the robot. Robot assigns a value 1 to those cells/coordinates in which there's an obstacle and zero otherwise.But, attractive force exists all the time. Formula for the force is in the code itself, can check it out. So, at centrex(centre of the robot, which changes as robot moves)=6, only attractive force exists, repulsive force=0 because robot cant sense the obstacles. At centrex=7, both the forces exists, with repulsive being the more dominating one ! So, robot moves away from obstacle to centrex=6.From there it should again move to 7(due to attractive force only), but rather it moves away t0 centrex=5! Note that it should only move to integer coordinates, so i have used the round function.