123 lines
4.5 KiB
Matlab
123 lines
4.5 KiB
Matlab
function planPathRRT
|
|
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
|
%function planPathPRM()
|
|
%
|
|
% Task: Finding the shortest path between the start and goal points (using dijkstra algorithm)
|
|
% in a tree of linked random points that explores the q1 q2 space (using Rapidly-exploring Random Trees algorithm)
|
|
%
|
|
%
|
|
% Inputs:
|
|
%
|
|
% Outputs:
|
|
%
|
|
% Thomas OLIVE (thomas.olive@ecam.fr)
|
|
% 19/12/2021
|
|
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
|
|
|
addpath("C:/Users/Admin/Documents/ProjectMotionPlanning/motion_planning"); % gets access to the scripts that are stored in that folder
|
|
|
|
[S, G, randVmin, randVmax, L1, L2] = setParams();
|
|
|
|
GapValue=1;
|
|
[nbNode, visGraph, points] = buildRRT();
|
|
|
|
for i=1:columns(visGraph)
|
|
for j=1:columns(visGraph)
|
|
if visGraph(i,j)==0 % if a link has not been noted with the distance
|
|
visGraph(i,j) = NaN; % we set it as invalid
|
|
endif
|
|
endfor
|
|
endfor
|
|
|
|
[distanceToNode, parentOfNode, nodeTrajectory] = dijkstra(nbNode, visGraph);
|
|
|
|
Q1plot = [];
|
|
Q2plot = [];
|
|
X_plot = [];
|
|
Y_plot = [];
|
|
|
|
nodeTrajectory = [columns(points) nodeTrajectory]; % we add the index of the last (goal) point to that array
|
|
shortcutPath = [1]; % we create an array similar to nodeTrajectory but without the nodes that can be avoided with shortcuts
|
|
loopAgain = 1; %boolean value indicating wether or not we keep running the loop looking for shortcuts
|
|
prevMaxIndex = columns(nodeTrajectory); % the index of the previous shortcut node we have in memory, the start point being at the end of nodeTrajectory
|
|
while loopAgain
|
|
maxShortcut = 0; % we reset the shortcut point we have in memory
|
|
for i=prevMaxIndex:-1:1 % we loop from the last saved shortcut to the goal point
|
|
if ~isnan(checkingLine(GapValue, L1, L2, nodeTrajectory(prevMaxIndex), nodeTrajectory(i), points)) % if a line can be drawn between the last saved shortcut and the node of index i
|
|
maxShortcut = nodeTrajectory(i); % we update that point of index i as the maximum shortcut reachable
|
|
maxIndex = i; % we save the index of that point of index i that will be the next shortcut
|
|
endif
|
|
endfor
|
|
prevMaxIndex = maxIndex;
|
|
if shortcutPath(end) == columns(points) || maxShortcut == 0 % if we reached the goal point with the shortcuts
|
|
loopAgain = 0; % we stop looping
|
|
else
|
|
loopAgain = 1;
|
|
shortcutPath = [shortcutPath maxShortcut];
|
|
endif
|
|
endwhile
|
|
for i=1:columns(shortcutPath) % running through the new nodeTrajectory array
|
|
Q1plot = [Q1plot points(1, shortcutPath(i))]; %we note the q1 and q2 values that are to be plot as the shortest path
|
|
Q2plot = [Q2plot points(2, shortcutPath(i))];
|
|
|
|
if i>1 % for the points that are not the start one
|
|
|
|
% we define the lines composing the shortest path using the usual Ax+B method
|
|
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);
|
|
if g==Q1plot(i) % as the sampling starts from a node that condition means that we are computing about a node
|
|
points(3, shortcutPath(i)) = Xtest; % we note the x and y values of the nodes
|
|
points(4, shortcutPath(i)) = Ytest;
|
|
endif
|
|
|
|
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(shortcutPath)
|
|
if nodeTrajectory(i) != 1
|
|
text(points(3, shortcutPath(i)), points(4, shortcutPath(i)), int2str(shortcutPath(i)-1), 'FontSize', 20); % we add the description of each node
|
|
endif
|
|
endfor
|
|
|
|
text(S(1), S(2), 'S', 'FontSize', 20);
|
|
text(G(1), G(2), 'G', 'FontSize', 20);
|
|
|
|
% 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 |