Some preliminary functions for implementing a PRM-based motion planning algorithm

For this demonstration we will use Matlab's

union function: https://www.mathworks.com/help/matlab/ref/double.union.html

setdiff function: https://www.mathworks.com/help/matlab/ref/double.setdiff.html

Specify your workspace W, number of samples n_samples, neighborhood connectivity radius r, and start point start_point and goal point goal_point.

Take a note of path_length and how it is changing when you change the number of samples or each time you run the program.

Generating Samples

Generate n_samples samples uniformly distributed over the workspace of W=[1 14]x[0 8] .

We will create the Samples as a cell data in Matlab.

Notice that since you are generating the samples randomly. Everytime you run the algorithm your roadmap will change.

clear all
fig_samples=figure;
n_samples=60; %specify this value
W={[1 14],[0 8]}; %specify this value
Samples(:,1) = W{1}(1,1) + (W{1}(1,2)-W{1}(1,1))*rand(n_samples,1); % x coordiante
Samples(:,2) = W{2}(1,1) + (W{2}(1,2)-W{2}(1,1))*rand(n_samples,1); % y coordiante
plot(Samples(:,1),Samples(:,2),'o')
xlim([W{1}(1,1)-1,W{1}(1,2)+1])
ylim([W{2}(1,1)-1,W{2}(1,2)+1])
axis square

Some programming tricks, for copy content of one figure to another. we use fig_PRM to plot the roadmap

fig_PRM=figure();

fig_samplesChil = fig_samples.Children;

copyobj(fig_samplesChil, fig_PRM);

Connecting the samples to create the roadmap

Now let us connect the points using r-radius rule, where we set the r to be 2.

We will save the edge lengths between any two nodes in a matrix called edge_length (this matrix is going to be symmertric by construct).

hold on

edge_length=-1*ones(n_samples+2,n_samples+2);%we initialize this matrix -1; at the end of process if an element is -1 it means that the corresponding elements are not connected.

%edge_length has two extra rows and columns, as place holder for start and goal points (nodes)

r=2; %neighborhood radius (specify this value)

Adj_table={};

node_set=(1:1:n_samples);

for i=1:n_samples

Adj_table{i}=[];%Adj_table of node i is initialized empty

for j=setdiff(node_set,[i])

if ((Samples(i,1)-Samples(j,1))^2+(Samples(i,2)-Samples(j,2))^2<=r^2)

line([Samples(i,1) Samples(j,1)],[Samples(i,2) Samples(j,2)],'Color','r')

Adj_table{i}=union(Adj_table{i},[j]);

edge_length(i,j)=sqrt((Samples(i,1)-Samples(j,1))^2+(Samples(i,2)-Samples(j,2))^2);

end

clear temp n_samples_minus_i j

end

Some programming tricks, for copy content of one figure to another. we use fig_PRM to plot the roadmap

fig_start_goal_connected=figure();
fig_PRMChil = fig_PRM.Children; 
copyobj(fig_PRMChil, fig_start_goal_connected);

Joining the start and goal points to the roadmap

We add the start and goal points as the last two nodes on the PRM graph

start_point=[2,2];

goal_point=[11, 2];

plot(start_point(1,1),start_point(1,2),'m>')

plot(goal_point(1,1),goal_point(1,2),'gs')

Samples(n_samples+1,:)=[2,2];

Samples(n_samples+2,:)=[11, 2];

Adj_table{n_samples+1}=[];%Adj_table of start node is initialized empty

Adj_table{n_samples+2}=[];%Adj_table of goal node is initialized empty

%connecting to the nodes in r radius of the points

Colour(n_samples+1)='m';

Colour(n_samples+2)='g';

for i=[n_samples+1, n_samples+2]

for j=1:n_samples

if ((Samples(i,1)-Samples(j,1))^2+(Samples(i,2)-Samples(j,2))^2<=r^2)

line([Samples(i,1) Samples(j,1)],[Samples(i,2) Samples(j,2)],'Color',Colour(i),'LineWidth',3)

edge_length(i,j)=sqrt((Samples(i,1)-Samples(j,1))^2+(Samples(i,2)-Samples(j,2))^2);

edge_length(j,i)=edge_length(i,j);

Adj_table{i}=union(Adj_table{i},[j]);

Adj_table{j}=union(Adj_table{j},[i]); %i is also a neighbor of j

end

clear temp n_samples_minus_i

end

Some programming tricks, for copy content of one figure to another. we use fig_PRM to plot the roadmap

fig_shortestpath=figure();
fig_start_goal_connectedChil = fig_start_goal_connected.Children; 
copyobj(fig_start_goal_connectedChil, fig_shortestpath);

Plotting the shortest path

[parent,dist]=Dijkstra_search(n_samples+2,Adj_table,edge_length,n_samples+1,n_samples+2);

path=extract_path(parent,n_samples+2)

path = 1×8

    61    16    43    52    36    49    57    62

path_length=dist(n_samples+2)

path_length = 10.2522

if length(path)~=0

for k=1:length(path)-1

line([Samples(path(k),1),Samples(path(k+1),1)],[Samples(path(k),2),Samples(path(k+1),2)],'Color','Y','LineWidth',4)

hold on

end

Dijkstra'a algorithm

function [parent,dist]=Dijkstra_search(n,Adj_table,weight,v_start,v_goal)
%n is the number of the nodes in the graph
%Adj_table adjacency table of the graph (assumes this table is defined as a matlab cell)
%weight: efge weight matrix
%v_start: start node label
%v_goal: goal node label
for i=1:n   %(line 1 on pseudo code)
    dist(i)=inf;%(line 2 on pseudo code)
    parent(i)=-1; %(line 3 on pseudo code) %-1 is equivalent of none, it means that node i does not have a parent
end
dist(v_start)=0; %(line 4 on pseudo code)
parent(v_start)=v_start; %(line 5 on pseudo code)
Q=(1:1:n); %(line 6 on pseudo code)
while length(Q)~=0 %(line 7 on pseudo code)
    dist_Q=dist(Q);
    [min_dist,index_min]= min(dist_Q);
    v=Q(index_min); %(line 8 on pseudo code) 
    dist_Q=[]; Q(index_min)=[]; %(line 9 on pseudo code)
    for w=Adj_table{v} %(line 10 on pseudo code)
        if dist(w)>dist(v)+weight(v,w) %(line 11 on pseudo code)
            dist(w)=dist(v)+weight(v,w);  %(line 12 on pseudo code)
            parent(w)=v;%(line 12 on pseudo code)
        end 
    end
end
end

Extract path

function P=extract_path(parent,v_goal)
P=[v_goal]; %(line 1 on pseudo code)
u=v_goal; %(line 2 on pseudo code)
while parent(u)~=u  %(line 3 on pseudo code)
    u=parent(u);  %(line 4 on pseudo code)
    P=[u P]; %(line 5 on pseudo code)
    if u==-1 %for discounected graphs
        P=[];
        break
    end
end
end

@ Solmaz Kia

May 14, 2023

Kia Cooeprative Systems Lab

University of California Irvine