112 lines
2.8 KiB
Mathematica
112 lines
2.8 KiB
Mathematica
function [nbNode, obstacle, points] = buildRRT()
|
|
clc
|
|
close all
|
|
|
|
S = [2, 0];
|
|
G = [-2, 0];
|
|
[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
|
|
randVmin = -180;
|
|
randVmax = 180;
|
|
L1 = 2;
|
|
L2 = 1;
|
|
|
|
q1_1st = randVmin + (randVmax - randVmin)* rand();
|
|
q2_1st = randVmin + (randVmax - randVmin)* rand();
|
|
|
|
|
|
[x, y]=MyFK(L1,L2,q1_1st,q2_1st);
|
|
points = [Sq1 Sq2 S(1) S(2)]';
|
|
|
|
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);
|
|
|
|
minTable = [];
|
|
|
|
n=0;
|
|
WhileCond = 15;
|
|
|
|
while n<WhileCond
|
|
distArr = [];
|
|
|
|
q1_r = randVmin + (randVmax - randVmin)* rand();
|
|
q2_r = randVmin + (randVmax - randVmin)* rand();
|
|
|
|
|
|
[x, y]=MyFK(L1,L2,q1_r,q2_r);
|
|
|
|
n = columns(points);
|
|
|
|
minTable(1, n) = inf;
|
|
obstacle(n,n) = NaN;
|
|
if n==WhileCond
|
|
q1_r = Gq1;
|
|
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))
|
|
WhileCond++;
|
|
else
|
|
pdefL = min(distArr);
|
|
endif
|
|
elseif n > 1
|
|
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
|
|
|
|
else
|
|
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)];
|
|
|
|
endif
|
|
[minDist, minIndex] = min(distArr); %validé
|
|
|
|
if ~isnan(minDist)
|
|
q1_p = points(1, minIndex);
|
|
q2_p = points(2, minIndex);
|
|
|
|
q1_pdefL = q1_p + (pdefL * (q1_r - q1_p) / minDist);
|
|
q2_pdefL = q2_p + (pdefL * (q2_r - q2_p) / minDist); % from thales theorem
|
|
|
|
points(1, n+1) = q1_pdefL;
|
|
points(2, n+1) = q2_pdefL;
|
|
|
|
if n+1!=minIndex
|
|
obstacle(n+1, minIndex) = pdefL;
|
|
obstacle(minIndex, n+1) = pdefL;
|
|
endif
|
|
|
|
figure 1
|
|
hold on
|
|
plot(points(1, n), points(2, n))
|
|
if pdefL == min(distArr) %which occurs only at the last iteration
|
|
text(points(1, n+1), points(2, n+1), 'G', 'FontSize', 20);
|
|
endif
|
|
if n>1
|
|
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
|
|
endfunction |