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

finished with original function instead of fast computing one

This commit is contained in:
Thomas OLIVE 2021-12-20 00:31:21 +01:00
parent ba91853223
commit 4c9e2ee655
4 changed files with 45 additions and 24 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

7
buildPRM.m
View File

@ -26,6 +26,7 @@ function [nbNodes, obstacle, points] = buildPRM()
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
clc
close all
addpath("C:/Users/Admin/Documents/ProjectMotionPlanning/motion_planning");
[S, G, randVmin, randVmax, L1, L2] = setParams();
@ -44,8 +45,10 @@ function [nbNodes, obstacle, points] = buildPRM()
q1 = randVmin + (randVmax - randVmin)* rand();% setting a new random point r
q2 = randVmin + (randVmax - randVmin)* rand();
[x, y]=MyFK(L1,L2,q1,q2);
%[x, y]=MyFK(L1,L2,q1,q2); remove the '%' on that line and add it on the next ones for faster computation
jTee=dh2ForwardKinematics([q1; q2], [0; 0], [L1; L2], [0; 0], 1);
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))

13
buildRRT.m
View File

@ -35,7 +35,10 @@ function [nbNodes, obstacle, points] = buildRRT()
q2_1st = randVmin + (randVmax - randVmin)* rand(); % first q2 random value
[x, y]=MyFK(L1,L2,q1_1st,q2_1st);
%[x, y]=MyFK(L1,L2,q1_1st,q2_1st); remove the % on that line and add it on the next ones for faster computation
jTee=dh2ForwardKinematics([q1_1st; q2_1st], [0; 0], [L1; L2], [0; 0], 1);
x= jTee(1,4);
y= jTee(2,4);
points = [Sq1 Sq2 S(1) S(2)]'; %flling the first column of the point array with the start point data
GapValue = 5;
@ -51,7 +54,7 @@ function [nbNodes, obstacle, points] = buildRRT()
minTable = [];
n=0; % represents the number of validated points
WhileCond = 15; % number of desired points in the tree
WhileCond = 20; % number of desired points in the tree
while n<WhileCond
distArr = []; %resets the distArr array for each point (each iteration)
@ -60,7 +63,11 @@ function [nbNodes, obstacle, points] = buildRRT()
q2_r = randVmin + (randVmax - randVmin)* rand();
[x, y]=MyFK(L1,L2,q1_r,q2_r); % getting the X Y coordinates of r point
%[x, y]=MyFK(L1,L2,q1_r,q2_r); remove the % on that line and add it on the next ones for faster computation
jTee=dh2ForwardKinematics([q1_r; q2_r], [0; 0], [L1; L2], [0; 0], 1); % getting the X Y coordinates of r point
x= jTee(1,4);
y= jTee(2,4);
n = columns(points);

5
checkingLine.m
View File

@ -62,8 +62,11 @@ function dist = checkingLine(GapValue, L1, L2, n, j, points)
Q2test = Q2(g);
%Is each sampled point from the line colliding with an obstacle in X Y cartesian space
[Xtest, Ytest]=MyFK(L1,L2,Q1test,Q2test);
%[Xtest, Ytest]=MyFK(L1,L2,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 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
dist = NaN;

44
planPathRRT.m
View File

@ -11,7 +11,7 @@ function planPathRRT
% Outputs:
%
% Thomas OLIVE (thomas.olive@ecam.fr)
% 18/12/2021
% 19/12/2021
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
addpath("C:/Users/Admin/Documents/ProjectMotionPlanning/motion_planning"); % gets access to the scripts that are stored in that folder
@ -19,7 +19,6 @@ function planPathRRT
[S, G, randVmin, randVmax, L1, L2] = setParams();
GapValue=1;
nodeTrajCut = [];
[nbNode, visGraph, points] = buildRRT();
for i=1:columns(visGraph)
@ -38,19 +37,28 @@ function planPathRRT
Y_plot = [];
nodeTrajectory = [columns(points) nodeTrajectory]; % we add the index of the last (goal) point to that array
for i=columns(nodeTrajectory):-1:1 %runs through the nodeTrajectory array from the end to the beginning
if ~isnan(checkingLine(GapValue, L1, L2, 1, nodeTrajectory(i), points)) % if a line can be drawn between the start point and the one of index i
nodeTrajCut = [nodeTrajCut nodeTrajectory(i)]; % we note the distance between those points
maxIndex = i;
shortcutPath = [1]; % we create an array similar to nodeTrajectory but without the nodes that can be avoided with shortcuts
loopAgain = 1; %boolean value indicating wether or not we keep running the loop looking for shortcuts
prevMaxIndex = columns(nodeTrajectory); % the index of the previous shortcut node we have in memory, the start point being at the end of nodeTrajectory
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
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
endfor
prevMaxIndex = maxIndex;
if shortcutPath(end) == columns(points) || maxShortcut == 0 % if we reached the goal point with the shortcuts
loopAgain = 0; % we stop looping
else
loopAgain = 1;
shortcutPath = [shortcutPath maxShortcut];
endif
endfor
nodeTrajectory = [nodeTrajectory(1:maxIndex-1) max(nodeTrajCut) 1]; % as we want to travel the maximal distance from the start point, withouth going through the first branches, we take the maximum value of the nodeTrajCut array
for i=1:columns(nodeTrajectory) % running through the new nodeTrajectory array
Q1plot = [Q1plot points(1, nodeTrajectory(i))]; %we note the q1 and q2 values that are to be plot as the shortest path
Q2plot = [Q2plot points(2, nodeTrajectory(i))];
endwhile
for i=1:columns(shortcutPath) % running through the new nodeTrajectory array
Q1plot = [Q1plot points(1, shortcutPath(i))]; %we note the q1 and q2 values that are to be plot as the shortest path
Q2plot = [Q2plot points(2, shortcutPath(i))];
if i>1 % for the points that are not the start one
@ -73,8 +81,8 @@ function planPathRRT
%we compute the x y coordinates that are matching the sample points
[Xtest, Ytest]=MyFK(2,1,Q1test,Q2test);
if g==Q1plot(i) % as the sampling starts from a node that condition means that we are computing about a node
points(3, nodeTrajectory(i)) = Xtest; % we note the x and y values of the nodes
points(4, nodeTrajectory(i)) = Ytest;
points(3, shortcutPath(i)) = Xtest; % we note the x and y values of the nodes
points(4, shortcutPath(i)) = Ytest;
endif
X_plot = [X_plot Xtest]; % we note the x and y values of the sample points
@ -95,9 +103,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(nodeTrajectory)
for i=2:columns(shortcutPath)
if nodeTrajectory(i) != 1
text(points(3, nodeTrajectory(i)), points(4, nodeTrajectory(i)), int2str(nodeTrajectory(i)-1), 'FontSize', 20); % we add the description of each node
text(points(3, shortcutPath(i)), points(4, shortcutPath(i)), int2str(shortcutPath(i)-1), 'FontSize', 20); % we add the description of each node
endif
endfor