113 lines
3.9 KiB
Matlab
113 lines
3.9 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)
|
|
% 18/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;
|
|
nodeTrajCut = [];
|
|
[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
|
|
|
|
for i=columns(nodeTrajectory):-1:1 %runs through the nodeTrajectory array from the end to the beginning
|
|
if ~isnan(checkingLine(GapValue, L1, L2, 1, nodeTrajectory(i), points)) % if a line can be drawn between the start point and the one of index i
|
|
nodeTrajCut = [nodeTrajCut nodeTrajectory(i)]; % we note the distance between those points
|
|
maxIndex = i;
|
|
endif
|
|
endfor
|
|
|
|
nodeTrajectory = [nodeTrajectory(1:maxIndex) max(nodeTrajCut) 1]; % as we want to travel the maximal distance from the start point, withouth going through the first branches, we take the maximum value of the nodeTrajCut array
|
|
|
|
for i=1:columns(nodeTrajectory) % running through the new 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))];
|
|
|
|
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, nodeTrajectory(i)) = Xtest; % we note the x and y values of the nodes
|
|
points(4, nodeTrajectory(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(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
|
|
|
|
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 |