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

RRT adds direct line

This commit is contained in:
Thomas OLIVE 2021-12-18 19:19:20 +01:00
parent 94b19538a0
commit 519897d82d
3 changed files with 10 additions and 92 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

36
buildPRM.m
View File

@ -144,11 +144,6 @@ function [nbNode, obstacle, points] = buildPRM()
if obstacle(j, n) !=NaN && (Ytest >= L1 || Ytest <= -L1 || (Xtest>=-L2 && Xtest<=L2 && Ytest>=-L2 && Ytest<=L2)) %verifie les obstacles
obstacle(j,n) = NaN;
obstacle(n,j) = NaN;
%{
X_space = [X_space Xtest];
Y_space = [Y_space Ytest];
%}
endif
endfor
@ -170,35 +165,4 @@ function [nbNode, obstacle, points] = buildPRM()
endif
endwhile
nbNode = WhileCond-2;
%points
%{
figure 2
axis([-3 3 -3 3]);
hold all
for i=1:columns(X_space)
if i==1
text(X_space(i), Y_space(i), 'S', 'FontSize', 20);
elseif i==2
text(X_space(i), Y_space(i), 'G', 'FontSize', 20);
else
text(X_space(i), Y_space(i), '*', 'FontSize', 20);
endif
%drawnow
endfor
figure 2
hold all
xL = 3*cos(0:0.01*pi:2*pi);
yL = 3*sin(0:0.01*pi:2*pi);
cL = [0 0 0];
plot(xL,yL,cL);
x = cos(0:0.01*pi:2*pi);
y = sin(0:0.01*pi:2*pi);
c = [0 0 0];
plot(x,y,c);
DrawObstacles
%}
endfunction

54
buildRRT.m
View File

@ -109,58 +109,4 @@ function [nbNode, obstacle, points] = buildRRT()
drawnow
endif
endwhile
## disp('avant')
## obstacle
## for i=1:columns(obstacle)
## for j=1:columns(obstacle)
## if ~isnan(checkingLine(GapValue, L1, L2, i, j, points))
## obstacle(i,j) = checkingLine(GapValue, L1, L2, i, j, points);
##
## q1 = points(1,1);
## q2 = points(2,1);
## q1_ = points(1,j);
## q2_ = points(2,j);
##
## Xplot = [q1, q1_];
## Yplot = [q2, q2_];
## plot(Xplot, Yplot, 'Color', 'b')
## drawnow
##
## endif
## endfor
## endfor
## disp('apres')
## obstacle
nbNode = WhileCond-1;
%{
for i=1:columns(obstacle)
[minTab(i,1) minTab(i,2)] = min(obstacle(1:i,i));
endfor
for i=1:rows(minTab)
if ~isnan(minTab(i,1))
q1_p = points(1,minTab(i,2));
q2_p = points(2,minTab(i,2));
q1_r = points(1, i);
q2_r = points(2, i);
q1_pdefL = pdefL * (q1_r - q1_p) / minTab(i,1)
q2_pdefL = pdefL * (q2_r - q2_p) / minTab(i,1) % from thales theorem
q1_pdefL = q1_r;
q2_pdefL = q2_r;
Xplot = [q1_p, q1_pdefL];
Yplot = [q2_p, q2_pdefL];
figure 1
plot(Xplot, Yplot, 'Color', 'b')
drawnow
endif
endfor
%}
endfunction

12
planPathRRT.m
View File

@ -3,6 +3,7 @@ function planPathRRT
GapValue=5;
L1=2;
L2=1;
nodeTrajCut = [];
[nbNode, visGraph, points] = buildRRT();
for i=1:columns(visGraph)
for j=1:columns(visGraph)
@ -16,14 +17,21 @@ function planPathRRT
Q1plot = [];
Q2plot = [];
nodeTrajectory = [columns(points) nodeTrajectory];
nodeTrajectory = [columns(points) nodeTrajectory]
for i=columns(nodeTrajectory):-1:1
i
if ~isnan(checkingLine(GapValue, L1, L2, 1, nodeTrajectory(i), points))
nodeTrajCut = [nodeTrajCut nodeTrajectory(i)];
endif
endfor
nodeTrajCut
nodeTrajectory = [16 max(nodeTrajCut) 1]
for i=1:columns(nodeTrajectory)
Q1plot = [Q1plot points(1, nodeTrajectory(i))];
Q2plot = [Q2plot points(2, nodeTrajectory(i))];
endfor
figure 1
axis([-180 180 -180 180]);
title('q1 q2 Joint Space');