85 lines
2.6 KiB
Matlab
85 lines
2.6 KiB
Matlab
function planPathPRM()
|
|
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
|
%function planPathPRM()
|
|
%
|
|
% Task: Finding the shortest path between the start and goal points (using dijkstra algorithm)
|
|
% in a map of linked random points that explores the q1 q2 space (using Probabilistic RoadMaps algorithm)
|
|
%
|
|
%
|
|
% Inputs:
|
|
%
|
|
% Outputs:
|
|
%
|
|
% Thomas OLIVE (thomas.olive@ecam.fr)
|
|
% 18/12/2021
|
|
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
|
|
|
[nbNode, visGraph, points] = buildPRM();
|
|
[distanceToNode, parentOfNode, nodeTrajectory] = dijkstra(nbNode, visGraph);
|
|
addpath("C:/Users/Admin/Documents/ProjectMotionPlanning/motion_planning");
|
|
Q1plot = [];
|
|
Q2plot = [];
|
|
X_plot = [];
|
|
Y_plot = [];
|
|
GapValue = 1;
|
|
nodeTrajectory = [columns(points) nodeTrajectory]; % we add the index of the last (goal) point to that array
|
|
|
|
for i=1:columns(nodeTrajectory) %runs through the nodeTrajectory array
|
|
Q1plot = [Q1plot points(1, nodeTrajectory(i))]; %we note the q1 and q2 values that are to be plot as the shortest path
|
|
Q2plot = [Q2plot points(2, nodeTrajectory(i))];
|
|
|
|
% we define the lines composing the shortest path using the usual Ax+B method
|
|
if i>1
|
|
|
|
|
|
A = (Q2plot(i-1)- Q2plot(i))/(Q1plot(i-1) - Q1plot(i));
|
|
B = Q2plot(i) - A * Q1plot(i);
|
|
|
|
Q2 = @(Q1) A*Q1+B;
|
|
|
|
% we sample the line
|
|
if Q1plot(i) > Q1plot(i-1)
|
|
gap = -GapValue;
|
|
else
|
|
gap = GapValue;
|
|
endif
|
|
for g=Q1plot(i):gap:Q1plot(i-1)
|
|
Q1test = g;
|
|
Q2test = Q2(g);
|
|
|
|
%we compute the x y coordinates that are matching the sample points
|
|
[Xtest, Ytest]=MyFK(2,1,Q1test,Q2test);
|
|
|
|
X_plot = [X_plot Xtest];% we note the x and y values of the sample points
|
|
Y_plot = [Y_plot Ytest];
|
|
endfor
|
|
endif
|
|
|
|
endfor
|
|
|
|
figure 1
|
|
axis([-180 180 -180 180]);
|
|
title('q1 q2 Joint Space');
|
|
hold on
|
|
plot(Q1plot, Q2plot, 'Color', 'g', 'LineWidth', 1.5)% we plot the shortest path in the q1 q2 joint space
|
|
|
|
figure 2
|
|
title('X-Y Cartesian Space');
|
|
axis([-3 3 -3 3]);
|
|
hold all
|
|
text(X_plot, Y_plot, '*', 'FontSize', 10, 'Color', 'g');% we plot the shortest path in the x y cartesian space
|
|
|
|
for i=2:columns(nodeTrajectory)-1
|
|
text(points(3, nodeTrajectory(i)), points(4, nodeTrajectory(i)), int2str(nodeTrajectory(i)-1), 'FontSize', 20); % we add the description of each node
|
|
endfor
|
|
|
|
% we draw the two circles defining the workspace
|
|
x = 3*cos(0:0.01*pi:2*pi);
|
|
y = 3*sin(0:0.01*pi:2*pi);
|
|
plot(x,y, 'Color', 'k');
|
|
|
|
x = cos(0:0.01*pi:2*pi);
|
|
y = sin(0:0.01*pi:2*pi);
|
|
plot(x,y, 'Color', 'k');
|
|
|
|
endfunction |