Obtaining the inflated obstacle in configuration space of a polygon robot with translational-only motion.

Convex hull

The convex hull of a set of points is defined as the smallest convex polygon, that encloses all of the points in the set. Convex means that the polygon has no corner that is bent inwards.

Matlab command to find convex hull of set of points is 'convhull'

See https://www.mathworks.com/help/matlab/ref/convhull.html

P = [0 0; 1 1; 1.5 0.5; 1.5 -0.5; 1.25 0.3; 1 0; 1.25 -0.3; 1 -1]; % set of points

[k,av] = convhull(P);% finds convexhull

plot(P(:,1),P(:,2),'x')

hold on

plot(P(k,1),P(k,2))

xlim([-1 2])

ylim([-2 2])

Constructing the inflated obstacle

figure
%obstcle is a trapazoid
O=[5 2;   %vertex 1
       15 -1;   %vertex 2
       15 5;    %vertex 3
       8 5;    %vertex 4
       5 2];   %repeating v1
   %robot is a pentagon
   m_o=length(O)-1;  %getting the number of segments
%Now lets draw the obstacle
for i=1:m_o
    plot(O(i,1), O(i,2),'o');
    hold on
    for t=0:0.01:1
        x_segment=(1-t)*O(i,1)+t*O(i+1,1);
        y_segment=(1-t)*O(i,2)+t*O(i+1,2);
        plot(x_segment,y_segment,'.r');
    end
end  
   R=[-1 -1;   %vertex 1
       1 -1;   %vertex 2
       1  1;    %vertex 3
       0 2;      %vertex 4
       -1 1;     %vertex 5
       -1 -1];    %repeating vertex 1
   
  %Now lets draw the Robot
   m_R=length(R)-1;  %getting the number of segments
for i=1:m_R
    plot(R(i,1), R(i,2),'o');
    hold on
    for t=0:0.01:1
        x_segment=(1-t)*R(i,1)+t*R(i+1,1);
        y_segment=(1-t)*R(i,2)+t*R(i+1,2);
        plot(x_segment,y_segment,'.b');
    end
end
plot(0,0,'s','MarkerSize',9) %center point of the robot

Notice here that the center point of the robot is whatever point that has coordinate (0,0) in your space.

Now let's use the algorithm to generate the extreme points of the boundary

Bound_point=[];
l=1;
for j=1:m_o
    for k=1:m_R
        Bound_point(l,:)=O(j,:)-R(k,:);
        l=l+1;
    end
end
plot(Bound_point(:,1),Bound_point(:,2),'kx','MarkerSize',8)

Now lets draw the boundary by calling matlab convex hull function 'convhull'

[k,av] = convhull(Bound_point);% finds convexhull

plot(Bound_point(k,1),Bound_point(k,2))

xlim([-2 18])

ylim([-4 8])

axis square

Non-convex obstacles

Minkowski difference method is for convex obstacles and convex shape robots. You can use this method for non-convex obstacles with convex robots by partitioning the obstacle to a set of non-overlapping convex sub obstacles and use the method for each component. Then, the inflated obstacle will be the union of the inflated subsets.

See the example below.

figure
clear O Bound_point
%obstcle is a trapazoid
O=[13 4;   %vertex 1
       15 -1;   %vertex 2
       15 5;    %vertex 3
       13 4];   %repeating v1
   %robot is a pentagon
   m_o=length(O)-1;  %getting the number of segments
%Now lets draw the obstacle
for i=1:m_o
    plot(O(i,1), O(i,2),'o');
    hold on
    for t=0:0.01:1
        x_segment=(1-t)*O(i,1)+t*O(i+1,1);
        y_segment=(1-t)*O(i,2)+t*O(i+1,2);
        plot(x_segment,y_segment,'.r');
    end
end  
   R=[-1 -1;   %vertex 1
       1 -1;   %vertex 2
       1  1;    %vertex 3
       0 2;      %vertex 4
       -1 1;     %vertex 5
       -1 -1];    %repeating vertex 1
   
  %Now lets draw the Robot
   m_R=length(R)-1;  %getting the number of segments
for i=1:m_R
    plot(R(i,1), R(i,2),'o');
    hold on
    for t=0:0.01:1
        x_segment=(1-t)*R(i,1)+t*R(i+1,1);
        y_segment=(1-t)*R(i,2)+t*R(i+1,2);
        plot(x_segment,y_segment,'.b');
    end
end
plot(0,0,'s','MarkerSize',9) %center point of the robot

Bound_point=[];
l=1;
for j=1:m_o
    for k=1:m_R
        Bound_point(l,:)=O(j,:)-R(k,:);
        l=l+1;
    end
end
plot(Bound_point(:,1),Bound_point(:,2),'kx','MarkerSize',8)
hold on

Now lets draw the boundary by calling matlab convex hull function 'convhull'

[k,av] = convhull(Bound_point);% finds convexhull
plot(Bound_point(k,1),Bound_point(k,2))
xlim([-2 18])
ylim([-4 8])
clear O Bound_point
%obstcle is a trapazoid
O=[13 4;   %vertex 1
       15 5;    %vertex 3
       8 5;    %vertex 4
       13 4];   %repeating v1
   %robot is a pentagon
   m_o=length(O)-1;  %getting the number of segments
%Now lets draw the obstacle
for i=1:m_o
    plot(O(i,1), O(i,2),'o');
    hold on
    for t=0:0.01:1
        x_segment=(1-t)*O(i,1)+t*O(i+1,1);
        y_segment=(1-t)*O(i,2)+t*O(i+1,2);
        plot(x_segment,y_segment,'.r');
    end
end  
   R=[-1 -1;   %vertex 1
       1 -1;   %vertex 2
       1  1;    %vertex 3
       0 2;      %vertex 4
       -1 1;     %vertex 5
       -1 -1];    %repeating vertex 1
   
  %Now lets draw the Robot
   m_R=length(R)-1;  %getting the number of segments
for i=1:m_R
    plot(R(i,1), R(i,2),'o');
    hold on
    for t=0:0.01:1
        x_segment=(1-t)*R(i,1)+t*R(i+1,1);
        y_segment=(1-t)*R(i,2)+t*R(i+1,2);
        plot(x_segment,y_segment,'.b');
    end
end
plot(0,0,'s','MarkerSize',9) %center point of the robot

Bound_point=[];
l=1;
for j=1:m_o
    for k=1:m_R
        Bound_point(l,:)=O(j,:)-R(k,:);
        l=l+1;
    end
end
plot(Bound_point(:,1),Bound_point(:,2),'kx','MarkerSize',8)

Now lets draw the boundary by calling matlab convex hull function 'convhull'

[k,av] = convhull(Bound_point);% finds convexhull

plot(Bound_point(k,1),Bound_point(k,2))

xlim([-2 18])

ylim([-4 8])

plot(12.35, 2.135,'s');

for i=1:m_R

plot(R(i,1)+12.35, R(i,2)+2.135,'o');

hold on

for t=0:0.01:1

x_segment=(1-t)*(R(i,1)+12.35)+t*(R(i+1,1)+12.35);

y_segment=(1-t)*(R(i,2)+2.135)+t*(R(i+1,2)+2.135);

plot(x_segment,y_segment,'.m');

end

x_off=12.75;

y_off=1.025;

plot(x_off, y_off,'s');

for i=1:m_R

plot(R(i,1)+x_off, R(i,2)+y_off,'o');

hold on

for t=0:0.01:1

x_segment=(1-t)*(R(i,1)+x_off)+t*(R(i+1,1)+x_off);

y_segment=(1-t)*(R(i,2)+y_off)+t*(R(i+1,2)+y_off);

plot(x_segment,y_segment,'.m');

end