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

added a threshold in the obstacle detection

This commit is contained in:
Thomas OLIVE 2021-12-21 00:11:47 +01:00
parent 118adb38c4
commit 0b5c6e2633
7 changed files with 49 additions and 24 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
DrawObstacles.m
View File

@ -32,6 +32,9 @@ function DrawObstacles()
% saving the computed coordinates
q1 = qi(j,1);
q2 = qi(j,2);
if q2 > 180
q2 = q2-360;
endif
q1_plot = [q1_plot q1];
q2_plot = [q2_plot q2];

21
buildPRM.m
View File

@ -28,10 +28,19 @@ function [nbNodes, obstacle, points] = buildPRM()
close all
addpath("C:/Users/Admin/Documents/ProjectMotionPlanning/motion_planning");
[S, G, randVmin, randVmax, L1, L2] = setParams();
[S, G, randVmin, randVmax, L1, L2, threshold] = setParams();
threshold = 0.000001;
[nbSol, qi] = solveIK2LinkPlanarRobot(2, 1, 2, 0);
Sq1 = qi(1,1)
Sq2 = qi(1,2)
[nbSol, qi] = solveIK2LinkPlanarRobot(2, 1, -2, 0);
Gq1 = qi(1,1)
Gq2 = qi(1,2)
[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
points = [Sq1 Sq2 S(1) S(2)]';
obstacle = [];
GapValue = 10;
@ -50,7 +59,7 @@ function [nbNodes, obstacle, points] = buildPRM()
x= jTee(1,4);
y= jTee(2,4);
%Checking if the new random point (end effector) is part of a known obstacle in X Y cartesian space
if not(y >= L1 || y <= -L1 || (x>=-L2 && x<=L2 && y>=-L2 && y<=L2))
if not((y >= L1 - threshold || y <= -L1 + threshold || (x>=-L2 - threshold && x<=L2 - threshold && y>=-L2 - threshold && y<=L2 + threshold))
n=columns(points);
@ -105,8 +114,8 @@ function [nbNodes, obstacle, points] = buildPRM()
q2_stored = points(2,j);
obstacle(j,n) = checkingLine(GapValue, L1, L2, n, j, points); % we store the distance (NaN if invalid) between the points in a matrix
obstacle(n,j) = checkingLine(GapValue, L1, L2, n, j, points); % that is designed as a double entry table
obstacle(j,n) = checkingLine(GapValue, L1, L2, n, j, points, threshold); % we store the distance (NaN if invalid) between the points in a matrix
obstacle(n,j) = checkingLine(GapValue, L1, L2, n, j, points, threshold); % that is designed as a double entry table
if ~isnan(obstacle(j, n)) && n!=j % if the points of index n and j can be linked
Xplot = [q1, q1_stored];

19
buildRRT.m
View File

@ -25,12 +25,17 @@ function [nbNodes, obstacle, points] = buildRRT()
clc
close all
[S, G, randVmin, randVmax, L1, L2] = setParams();
[S, G, randVmin, randVmax, L1, L2, threshold] = 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
[nbSol, qi] = solveIK2LinkPlanarRobot(2, 1, 2, 0);
Sq1 = qi(1,1);
Sq2 = qi(1,2);
[nbSol, qi] = solveIK2LinkPlanarRobot(2, 1, -2, 0);
Gq1 = qi(1,1);
Gq2 = qi(1,2);
q1_1st = randVmin + (randVmax - randVmin)* rand(); % first q1 random value
q2_1st = randVmin + (randVmax - randVmin)* rand(); % first q2 random value
@ -54,7 +59,7 @@ function [nbNodes, obstacle, points] = buildRRT()
minTable = [];
n=0; % represents the number of validated points
WhileCond = 20; % number of desired points in the tree
WhileCond = 15; % number of desired points in the tree
while n<WhileCond
distArr = []; %resets the distArr array for each point (each iteration)
@ -81,7 +86,7 @@ function [nbNodes, obstacle, points] = buildRRT()
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)];
distArr = [distArr checkingLine(GapValue, L1, L2, n+1, j, pointsTemp, threshold)];
endfor
if isnan(min(distArr)) % if the line can not be drawn between the closest and goal point
WhileCond++; % we keep adding more points
@ -94,14 +99,14 @@ function [nbNodes, obstacle, points] = buildRRT()
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)
distArr = [distArr checkingLine(GapValue, L1, L2, n+1, j, pointsTemp, threshold)]; % 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)
distArr = [distArr checkingLine(GapValue, L1, L2, 1, 2, pointsTemp, threshold)]; % 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

5
checkingLine.m
View File

@ -1,4 +1,4 @@
function dist = checkingLine(GapValue, L1, L2, n, j, points)
function dist = checkingLine(GapValue, L1, L2, n, j, points, threshold)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%function dist = checkingLine(GapValue, L1, L2, n, j, points)
@ -36,7 +36,6 @@ function dist = checkingLine(GapValue, L1, L2, n, j, points)
q1_ = points(1,n);
q2_ = points(2,n);
dist = 0; % 0 value, if not modified, will represent that we didn't note about an invalid link between the n and j points
if n==j
dist = NaN;
@ -68,7 +67,7 @@ function dist = checkingLine(GapValue, L1, L2, n, j, points)
Xtest = jTee(1,4);
Ytest = jTee(2,4);
% if a sampled point is colliding and we haven't already noted it, we do
if dist !=NaN && (Ytest >= L1 || Ytest <= -L1 || (Xtest>=-L2 && Xtest<=L2 && Ytest>=-L2 && Ytest<=L2)) %verifie les obstacles
if dist !=NaN && (Ytest >= L1 - threshold || Ytest <= -L1 + threshold || (Xtest>=-L2 - threshold && Xtest<=L2 - threshold && Ytest>=-L2 - threshold && Ytest<=L2 + threshold)) %verifie les obstacles
dist = NaN;
endif

7
planPathPRM.m
View File

@ -13,6 +13,7 @@ function planPathPRM()
% Thomas OLIVE (thomas.olive@ecam.fr)
% 18/12/2021
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
[S, G, randVmin, randVmax, L1, L2, threshold] = setParams()
[nbNode, visGraph, points] = buildPRM();
[distanceToNode, parentOfNode, nodeTrajectory] = dijkstra(nbNode, visGraph);
@ -48,7 +49,11 @@ function planPathPRM()
Q2test = Q2(g);
%we compute the x y coordinates that are matching the sample points
[Xtest, Ytest]=MyFK(2,1,Q1test,Q2test);
% [Xtest, Ytest]=MyFK(2,1,Q1test,Q2test); remove the % on that line and add it on the next ones for faster computation
jTee=dh2ForwardKinematics([Q1test; Q2test], [0; 0], [L1; L2], [0; 0], 1);
Xtest = jTee(1,4);
Ytest = jTee(2,4);
X_plot = [X_plot Xtest];% we note the x and y values of the sample points
Y_plot = [Y_plot Ytest];

15
planPathRRT.m
View File

@ -16,7 +16,7 @@ function planPathRRT
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();
[S, G, randVmin, randVmax, L1, L2, threshold] = setParams();
GapValue=1;
[nbNode, visGraph, points] = buildRRT();
@ -43,7 +43,7 @@ function planPathRRT
while loopAgain
maxShortcut = 0; % we reset the shortcut point we have in memory
for i=prevMaxIndex:-1:1 % we loop from the last saved shortcut to the goal point
if ~isnan(checkingLine(GapValue, L1, L2, nodeTrajectory(prevMaxIndex), nodeTrajectory(i), points)) % if a line can be drawn between the last saved shortcut and the node of index i
if ~isnan(checkingLine(GapValue, L1, L2, nodeTrajectory(prevMaxIndex), nodeTrajectory(i), points, threshold)) % if a line can be drawn between the last saved shortcut and the node of index i
maxShortcut = nodeTrajectory(i); % we update that point of index i as the maximum shortcut reachable
maxIndex = i; % we save the index of that point of index i that will be the next shortcut
endif
@ -79,7 +79,11 @@ function planPathRRT
Q2test = Q2(g);
%we compute the x y coordinates that are matching the sample points
[Xtest, Ytest]=MyFK(2,1,Q1test,Q2test);
% [Xtest, Ytest]=MyFK(2,1,Q1test,Q2test); remove the % on that line and add it on the next ones for faster computation
jTee=dh2ForwardKinematics([Q1test; Q2test], [0; 0], [L1; L2], [0; 0], 1);
Xtest = jTee(1,4);
Ytest = jTee(2,4);
if g==Q1plot(i) % as the sampling starts from a node that condition means that we are computing about a node
points(3, shortcutPath(i)) = Xtest; % we note the x and y values of the nodes
points(4, shortcutPath(i)) = Ytest;
@ -103,10 +107,9 @@ function planPathRRT
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(shortcutPath)
if nodeTrajectory(i) != 1
for i=2:columns(shortcutPath)-1
text(points(3, shortcutPath(i)), points(4, shortcutPath(i)), int2str(shortcutPath(i)-1), 'FontSize', 20); % we add the description of each node
endif
%endif
endfor
text(S(1), S(2), 'S', 'FontSize', 20);

3
setParams.m
View File

@ -1,4 +1,4 @@
function [S, G, randVmin, randVmax, L1, L2] = setParams()
function [S, G, randVmin, randVmax, L1, L2, threshold] = setParams()
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%function [S, G, randVmin, randVmax, L1, L2] = setParams()
%
@ -28,4 +28,5 @@ function [S, G, randVmin, randVmax, L1, L2] = setParams()
randVmax = 180;
L1 = 2;
L2 = 1;
threshold = 0.000001;
endfunction