133 lines
5.3 KiB
Matlab
133 lines
5.3 KiB
Matlab
function [nbNodes, obstacle, points] = buildRRT()
|
|
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
|
%function [nbNodes, obstacle, points] = buildPRM()
|
|
%
|
|
% Task: Implement a code that creates a tree of points that are drawn in a random way in the q1 q2 joint space
|
|
% The tree progresses by a predefined length towards random direction, starting from the S point,
|
|
% until it finds a way to connect the start and goal points
|
|
% that algorithm is called Rapidly-exploring Random Trees algorithm
|
|
%
|
|
%
|
|
% Inputs:
|
|
%
|
|
% Outputs:
|
|
% -nbNodes: number of nodes of the graph excluding the starting and goal points
|
|
% -obstacle: a matrix containing the distance between connected nodes
|
|
% (NaN refers to not connected nodes)
|
|
% The matrix has a size of (nbNodes+2)x(nbNodes+2)
|
|
%-points: a matrix saving the q1 q2 and x y coordinates of each point,
|
|
% the 1st column describing the 1st (start) point
|
|
%
|
|
%
|
|
% Thomas OLIVE (thomas.olive@ecam.fr)
|
|
% 18/12/2021
|
|
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
|
clc
|
|
close all
|
|
|
|
[S, G, randVmin, randVmax, L1, L2] = setParams();
|
|
|
|
[Sq1, Sq2] = MyIK(2,1,S(1),S(2)); %-28.995 104.48
|
|
[Gq1, Gq2] = MyIK(2,1,G(1),G(2)); %151.04 104.48
|
|
|
|
|
|
q1_1st = randVmin + (randVmax - randVmin)* rand(); % first q1 random value
|
|
q2_1st = randVmin + (randVmax - randVmin)* rand(); % first q2 random value
|
|
|
|
|
|
[x, y]=MyFK(L1,L2,q1_1st,q2_1st);
|
|
points = [Sq1 Sq2 S(1) S(2)]'; %flling the first column of the point array with the start point data
|
|
|
|
GapValue = 5;
|
|
distArr = [];
|
|
pdefL = 50;
|
|
|
|
figure 1
|
|
axis([-180 180 -180 180])
|
|
hold on
|
|
plot(points(1, 1), points(2, 1))
|
|
text(points(1, 1), points(2, 1), 'S', 'FontSize', 20); %drawing a 'S' letter at q1 q2
|
|
|
|
minTable = [];
|
|
|
|
n=0; % represents the number of validated points
|
|
WhileCond = 15; % number of desired points in the tree
|
|
|
|
while n<WhileCond
|
|
distArr = []; %resets the distArr array for each point (each iteration)
|
|
|
|
q1_r = randVmin + (randVmax - randVmin)* rand(); % setting a new random point r
|
|
q2_r = randVmin + (randVmax - randVmin)* rand();
|
|
|
|
|
|
[x, y]=MyFK(L1,L2,q1_r,q2_r); % getting the X Y coordinates of r point
|
|
|
|
n = columns(points);
|
|
|
|
minTable(1, n) = inf;
|
|
obstacle(n,n) = NaN; % increases the size of obstacle matrix
|
|
if n==WhileCond % if we have enough points
|
|
q1_r = Gq1; % we do not link a p point with a random one but with the Goal one
|
|
q2_r = Gq2;
|
|
for j=1:n
|
|
q1_p = points(1,j);
|
|
q2_p = points(2,j);
|
|
|
|
pointsTemp = [points [q1_r q2_r 0 0]']; %new point r values are stored in n+1 column of pointsTemp
|
|
distArr = [distArr checkingLine(GapValue, L1, L2, n+1, j, pointsTemp)];
|
|
endfor
|
|
if isnan(min(distArr)) % if the line can not be drawn between the closest and goal point
|
|
WhileCond++; % we keep adding more points
|
|
else
|
|
pdefL = min(distArr); % we do not use the predefined length but the one separing the closest and goal points
|
|
endif
|
|
elseif n > 1 % if is not the first iteration
|
|
for j=1:n % for each point in the tree
|
|
q1_p = points(1,j);
|
|
q2_p = points(2,j);
|
|
|
|
pointsTemp = [points [q1_r q2_r 0 0]']; %new point r values are stored in n+1 column of pointsTemp
|
|
distArr = [distArr checkingLine(GapValue, L1, L2, n+1, j, pointsTemp)]; % we compute the distance between each point in the tree (index j) and the new r one (index n+1)
|
|
endfor
|
|
|
|
else % if it is the first iteration
|
|
q1_p = points(1,1);
|
|
q2_p = points(2,1);
|
|
pointsTemp = [points [q1_r q2_r 0 0]'];
|
|
distArr = [distArr checkingLine(GapValue, L1, L2, 1, 2, pointsTemp)]; % we compute the distance between the first point in the tree (index 1) and the new r one (index n+1)
|
|
|
|
endif
|
|
[minDist, minIndex] = min(distArr); % we save which point is the closest one from the new r point and we note the distance between them
|
|
|
|
if ~isnan(minDist) % if the line between p and r can be drawn (does not intersect an obstacle in X Y cartesian space)
|
|
q1_p = points(1, minIndex);
|
|
q2_p = points(2, minIndex);
|
|
|
|
q1_pdefL = q1_p + (pdefL * (q1_r - q1_p) / minDist); % thanks to some theory coming from the Thales theorem
|
|
q2_pdefL = q2_p + (pdefL * (q2_r - q2_p) / minDist); % we compute the q1 and q2 values of the point that is on the line between p and r, away of pdefL from p
|
|
|
|
points(1, n+1) = q1_pdefL; %we note those two values in the points (= tree) matrix since we consider them as 'validated'
|
|
points(2, n+1) = q2_pdefL;
|
|
|
|
if n+1!=minIndex
|
|
obstacle(n+1, minIndex) = pdefL; % we note the distance between p and r in the obstacle matrix
|
|
obstacle(minIndex, n+1) = pdefL; % which is built as a double entry table (going from S to G)
|
|
endif
|
|
|
|
figure 1
|
|
hold on
|
|
plot(points(1, n), points(2, n)) % we plot the new values and add a description to it
|
|
if pdefL == min(distArr) % this condition is valid only for the last iteration
|
|
text(points(1, n+1), points(2, n+1), 'G', 'FontSize', 20);
|
|
endif
|
|
if n>1 % valid for every iteration except the first and last ones
|
|
text(points(1, n), points(2, n), int2str(n-1), 'FontSize', 20);
|
|
endif
|
|
Xplot = [q1_p, q1_pdefL];
|
|
Yplot = [q2_p, q2_pdefL];
|
|
plot(Xplot, Yplot, 'Color', 'b')
|
|
drawnow
|
|
endif
|
|
endwhile
|
|
nbNodes = WhileCond-1;
|
|
endfunction |