thomas.olive
/
MotionPlanning
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity

obstacle threshold and branch shortening for RRT

This commit is contained in:
Thomas OLIVE 2021-12-21 19:43:11 +01:00
parent 42ab8a308b
commit 517777ab02
2 changed files with 8 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

9
buildRRT.m
View File

@ -110,7 +110,10 @@ function [nbNodes, obstacle, points] = buildRRT()
endif 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 [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 min(distArr) < pdefL
pdefL = min(distArr);
disp(minIndex);
endif
if ~isnan(minDist) % if the line between p and r can be drawn (does not intersect an obstacle in X Y cartesian space) 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); q1_p = points(1, minIndex);
q2_p = points(2, minIndex); q2_p = points(2, minIndex);
@ -129,9 +132,6 @@ function [nbNodes, obstacle, points] = buildRRT()
figure 1 figure 1
hold on hold on
plot(points(1, n), points(2, n)) % we plot the new values and add a description to it 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 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); text(points(1, n), points(2, n), int2str(n-1), 'FontSize', 20);
endif endif
@ -141,5 +141,6 @@ function [nbNodes, obstacle, points] = buildRRT()
drawnow drawnow
endif endif
endwhile endwhile
text(points(1, WhileCond+1), points(2, WhileCond+1), 'G', 'FontSize', 20);
nbNodes = WhileCond-1; nbNodes = WhileCond-1;
endfunction endfunction

4
checkingLine.m
View File

@ -67,7 +67,9 @@ function dist = checkingLine(GapValue, L1, L2, n, j, points, threshold)
Xtest = jTee(1,4); Xtest = jTee(1,4);
Ytest = jTee(2,4); Ytest = jTee(2,4);
% if a sampled point is colliding and we haven't already noted it, we do % if a sampled point is colliding and we haven't already noted it, we do
if dist !=NaN && (Ytest >= L1 - threshold || Ytest <= -L1 + threshold || (Xtest>=-L2 - threshold && Xtest<=L2 - threshold && Ytest>=-L2 - threshold && Ytest<=L2 + threshold)) %verifie les obstacles if dist !=NaN && (Ytest >= L1 - threshold || Ytest <= -L1 + threshold || (Xtest>=-L2 - threshold && Xtest<=L2 - threshold && Ytest>=-L2 - threshold && Ytest<=L2 + threshold))
% In order to avoid trajectories hitting the obstacle due to some rounding problems
% we defined the obstacle area using a threshold
dist = NaN; dist = NaN;
endif endif