Skip to content

Instantly share code, notes, and snippets.

Embed
What would you like to do?
a implement of Artificial Potential Field in matlab
%
% PotentialFieldScript.m
%
%% Generate some points
nrows = 400;
ncols = 600;
obstacle = false(nrows, ncols);
[x, y] = meshgrid (1:ncols, 1:nrows);
%% Generate some obstacle
obstacle (300:end, 100:250) = true;
obstacle (150:200, 400:500) = true;
t = ((x - 200).^2 + (y - 50).^2) < 50^2;
obstacle(t) = true;
t = ((x - 400).^2 + (y - 300).^2) < 100^2;
obstacle(t) = true;
%% Compute distance transform
d = bwdist(obstacle);
% Rescale and transform distances
d2 = (d/100) + 1;
d0 = 2;
nu = 800;
repulsive = nu*((1./d2 - 1/d0).^2);
repulsive (d2 > d0) = 0;
%% Display repulsive potential
figure;
m = mesh (repulsive);
m.FaceLighting = 'phong';
axis equal;
title ('Repulsive Potential');
%% Compute attractive force
goal = [400, 50];
xi = 1/700;
attractive = xi * ( (x - goal(1)).^2 + (y - goal(2)).^2 );
figure;
m = mesh (attractive);
m.FaceLighting = 'phong';
axis equal;
title ('Attractive Potential');
%% Display 2D configuration space
figure;
imshow(~obstacle);
hold on;
plot (goal(1), goal(2), 'r.', 'MarkerSize', 25);
hold off;
axis ([0 ncols 0 nrows]);
axis xy;
axis on;
xlabel ('x');
ylabel ('y');
title ('Configuration Space');
%% Combine terms
f = attractive + repulsive;
figure;
m = mesh (f);
m.FaceLighting = 'phong';
axis equal;
title ('Total Potential');
%% Plan route
start = [50, 350];
route = GradientBasedPlanner (f, start, goal, 1000);
%% Plot the energy surface
figure;
m = mesh (f);
axis equal;
%% Plot ball sliding down hill
[sx, sy, sz] = sphere(20);
scale = 20;
sx = scale*sx;
sy = scale*sy;
sz = scale*(sz+1);
hold on;
p = mesh(sx, sy, sz);
p.FaceColor = 'red';
p.EdgeColor = 'none';
p.FaceLighting = 'phong';
hold off;
for i = 1:size(route,1)
P = round(route(i,:));
z = f(P(2), P(1));
p.XData = sx + P(1);
p.YData = sy + P(2);
p.ZData = sz + f(P(2), P(1));
drawnow;
drawnow;
end
%% quiver plot
[gx, gy] = gradient (-f);
skip = 20;
figure;
xidx = 1:skip:ncols;
yidx = 1:skip:nrows;
quiver (x(yidx,xidx), y(yidx,xidx), gx(yidx,xidx), gy(yidx,xidx), 0.4);
axis ([1 ncols 1 nrows]);
hold on;
ps = plot(start(1), start(2), 'r.', 'MarkerSize', 30);
pg = plot(goal(1), goal(2), 'g.', 'MarkerSize', 30);
p3 = plot (route(:,1), route(:,2), 'r', 'LineWidth', 2);
@Francis235

This comment has been minimized.

Copy link

Francis235 commented Jun 14, 2019

can share function GradientBasedPlanner()? thank you very much

@legelabora

This comment has been minimized.

Copy link

legelabora commented Nov 15, 2019

Hello, I'd like to take a look at the function GradientBasedPlanner() too. Can you share it please?

Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
You can’t perform that action at this time.