From 4c9e2ee65567fe3a6cdb15518f2172fcad01b1af Mon Sep 17 00:00:00 2001 From: Thomas Olive Date: Mon, 20 Dec 2021 00:31:21 +0100 Subject: [PATCH] finished with original function instead of fast computing one --- buildPRM.m | 7 +++++-- buildRRT.m | 13 ++++++++++--- checkingLine.m | 5 ++++- planPathRRT.m | 44 ++++++++++++++++++++++++++------------------ 4 files changed, 45 insertions(+), 24 deletions(-) diff --git a/buildPRM.m b/buildPRM.m index f5baff4..571cf3b 100644 --- a/buildPRM.m +++ b/buildPRM.m @@ -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)) diff --git a/buildRRT.m b/buildRRT.m index 19c5549..8338d79 100644 --- a/buildRRT.m +++ b/buildRRT.m @@ -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= L1 || Ytest <= -L1 || (Xtest>=-L2 && Xtest<=L2 && Ytest>=-L2 && Ytest<=L2)) %verifie les obstacles dist = NaN; diff --git a/planPathRRT.m b/planPathRRT.m index 85e823b..fabaaf5 100644 --- a/planPathRRT.m +++ b/planPathRRT.m @@ -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