diff --git a/.DS_Store b/.DS_Store new file mode 100644 index 0000000..37e0a4b Binary files /dev/null and b/.DS_Store differ diff --git a/buildPRM.m b/buildPRM.m index d8afab3..9ebfd5e 100644 --- a/buildPRM.m +++ b/buildPRM.m @@ -1,10 +1,16 @@ ## Author: adril ## Created: 2022-12-06 +## For more info: +## Check library matgeom: https://octave.sourceforge.io/matgeom/overview.html %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %function buildPRM (rangeQ1Q2, nbPoints, L1, L2, MapFilename) % % Task: +%The goal of this function is to use the Probabilistic RoadMaps procedure, +%to build a map of both the Cartesian space and the C-space representations, +%with the number of points chosen by the user to establish the different +%options for the path to follow, to get to a specific goal (cartesian point) % % Inputs: % - rangeQ1Q2 : range of values (in degrees) acceptable for joints Q1 and Q2 @@ -13,19 +19,18 @@ % - MapFilename : the name of the file to be saved for the map % % Outputs: -% - None +% - .mat file in the workspace % % Adrien Lasserre (adrien.lasserre@ecam.fr) & Gwenn Durpoix-Espinasson (g.durpoix-espinasson@ecam.fr) % 06/12/2022 % -% Check library matgeom: https://octave.sourceforge.io/matgeom/overview.html %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% function buildPRM (rangeQ1Q2, nbPoints, L1, L2, MapFilename) hold off; - i = 1; - Points=zeros(2, nbPoints); + i = 1; %initialize + Points=zeros(2, nbPoints); %matrix of 2xnbPoints MatrixOfLinks=zeros(nbPoints, nbPoints); alpha=[0;0]; d=[0;0]; @@ -34,6 +39,7 @@ function buildPRM (rangeQ1Q2, nbPoints, L1, L2, MapFilename) figure 1; hold on; b=drawCircle(0, 0, L1+L2); %teacher's functions for drawing circles + ## used for drawing the robot definition hold on; c=drawCircle(0, 0, L2-L1); hold on; @@ -43,11 +49,11 @@ function buildPRM (rangeQ1Q2, nbPoints, L1, L2, MapFilename) drawLine(top_line); hold on; drawLine(bottom_line); + %creates the central box that's prohibited center_box=[L2 L2; -L2 L2; -L2 -L2; L2 -L2]; drawPolygon(center_box); hold on; - poly_a=circleToPolygon([0 0 L2-L1], 32);%create a polygon for matgeom with the circle info (smaller one) poly_b=circleToPolygon([0 0 L1+L2], 32);%bigger one radius=3 @@ -109,7 +115,7 @@ function buildPRM (rangeQ1Q2, nbPoints, L1, L2, MapFilename) endif end - MatrixOfLinks + MatrixOfLinks; Points %display the points vector to check on the graph qGraph (Q_storage, nbPoints, MatrixOfLinks) diff --git a/buildRRT.m b/buildRRT.m new file mode 100644 index 0000000..0380f83 --- /dev/null +++ b/buildRRT.m @@ -0,0 +1,189 @@ +## Author: adril +## Created: 2023-01-08 + +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +%function buildRRT (rangeQ1Q2, nbPoints, L1, L2, MapFilename) +% +% Task: Use the RRT method to reach a desired goal on the cartesian space +% +% Inputs: +% - rangeQ1Q2 : range of values (in degrees) acceptable for joints Q1 and Q2 +% - nbPoints : number of points required +% - L1, L2 : lengths of the links (in m) +% - fixedLength : length of the short link +% - start : starting point +% - goal : ending point +% +% Outputs: +% - None +% +% Adrien Lasserre (adrien.lasserre@ecam.fr) & +% Gwenn Durpoix-Espinasson (g.durpoix-espinasson@ecam.fr) +% 08/01/2023 +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + +function buildRRT(rangeQ1Q2, nbPoints, L1, L2, fixedLength, start, goal) + + hold off; + i = 1; %while + nbPoints=nbPoints+2;%adding the starting point + %Points=zeros(nbPoints, 2); + %MatrixOfLinks=zeros(nbPoints, nbPoints); + alpha=[0;0]; + d=[0;0]; + a=[L1;L2]; + jointNumber=1; + + figure 1; hold on; + b=drawCircle(0, 0, L1+L2); %teacher's functions for drawing circles + hold on; + c=drawCircle(0, 0, L2-L1); + hold on; + %creates the lines defining the prohibited areas + top_line = createLine([0,L1,1,0]); + bottom_line = createLine([0,-L1,1,0]); + drawLine(top_line); + hold on; + drawLine(bottom_line); + center_box=[L2 L2; -L2 L2; -L2 -L2; L2 -L2]; + drawPolygon(center_box); + hold on; + + poly_a=circleToPolygon([0 0 L2-L1], 32);%create a polygon for matgeom with the circle info (smaller one) + poly_b=circleToPolygon([0 0 L1+L2], 32);%bigger one radius=3 + + %set the starting point + Points(1,1:2) = start; + + %draw the start and goal points + drawPoint(start(1, 1), start(1,2)); + drawPoint(goal(1, 1), goal(1,2)); + + while i <= nbPoints + + Q=[rand()*(rangeQ1Q2(1,2)-rangeQ1Q2(1,1))+rangeQ1Q2(1,1);rand()*(rangeQ1Q2(2,2)-rangeQ1Q2(2,1))+rangeQ1Q2(2,1)]; + theta=[Q(1,1);Q(2,1)]; + Q_storage(1:2, i)=Q; + OutOfRange=0; %set the boolean + intersect=0; + + bTee=dh2ForwardKinematics(theta, d, a, alpha, jointNumber); %FW kinematics + jTee=bTee(1:2, 4); %only retrieve the x and y (2D) values + jTee=jTee'; + + index=findClosestPoint(jTee, Points); + dx=jTee(1,1)-Points(index, 1); + dy=jTee(1,2)-Points(index, 2); + E=sqrt(dx^2+dy^2); + dx=(dx)*fixedLength/E; + dy=(dy)*fixedLength/E; + L = createEdge(Points(index, :), [Points(index, 1)+dx, Points(index,2)+dy]); + + if ((Points(index,2)+dy)>=L1) + OutOfRange=1; %is not valid if in that area + elseif ((Points(index,2)+dy)<=-L1) + OutOfRange=1; + elseif (abs((Points(index,1)+dx)) <= L2 && abs((Points(index,2)+dy)) <=L2) + OutOfRange=1; + endif + + if (OutOfRange==0) + + if (isempty(intersectEdgePolygon(L, poly_a))!=1 | isempty(intersectEdgePolygon(L, poly_b))!=1 | isempty(intersectEdgePolygon(L, center_box))!=1) + intersect=1; % intersection happenned + disp('Intersect') + else %if there is no intersection, plot the line-segment and the point and adds it to the list of valid points + hold on;%plotting the line + drawEdge(L); + MatrixOfLinks(i, i)=1; + MatrixOfLinks(i, index)=1; + MatrixOfLinks(index, i)=1; + Points(i, 1:2)=[Points(index, 1)+dx, Points(index,2)+dy]; + hold on; + drawPoint(Points(i, 1), Points(i,2)); %draw the point + intersect=0; + L = createEdge(goal, Points(i, 1:2)); + + if (isempty(intersectEdgePolygon(L, poly_a))!=1 | isempty(intersectEdgePolygon(L, poly_b))!=1 | isempty(intersectEdgePolygon(L, center_box))!=1) + intersect=1; + else + drawEdge(L); + MatrixOfLinks(i+1, i+1)=1; + MatrixOfLinks(i+1,i)=1; + MatrixOfLinks(i,i+1)=1; + i=i+1; + break; + endif + + i=i+1; + endif + endif + endwhile + +## Write the path planning here + +## Start from goal, towards start, going to the previous point each time +## check everytime if the start is not already accessible, and if so draw a line +## prendre le goal, trouver le point auquel il est attach, y aller + + index_goal = columns(MatrixOfLinks); + index = index_goal; + +##Draw the clean figure + figure 2; hold on; + b=drawCircle(0, 0, L1+L2); %teacher's functions for drawing circles + hold on; + c=drawCircle(0, 0, L2-L1); + hold on; + %creates the lines defining the prohibited areas + top_line = createLine([0,L1,1,0]); + bottom_line = createLine([0,-L1,1,0]); + drawLine(top_line); + hold on; + drawLine(bottom_line); + center_box=[L2 L2; -L2 L2; -L2 -L2; L2 -L2]; + drawPolygon(center_box); + hold on; + + poly_a=circleToPolygon([0 0 L2-L1], 32);%create a polygon for matgeom with the circle info (smaller one) + poly_b=circleToPolygon([0 0 L1+L2], 32);%bigger one radius=3 + + %set the starting point + %Points(1,1:2) = start; + + %draw the start and goal points + %drawPoint(start(1, 1), start(1,2)); + Points_2(1, 1:2)=goal; + drawPoint(Points_2(1, 1:2), 'ro'); + z=1; + MatrixOfLinks + while Points_2(z, 1:2)!=Points(1, 1:2) +## Start the path planning +##Check if we can reach the start + L = createEdge(start, Points_2(z, 1:2)); + if (isempty(intersectEdgePolygon(L, poly_a))!=1 | isempty(intersectEdgePolygon(L, poly_b))!=1 | isempty(intersectEdgePolygon(L, center_box))!=1) + intersect=1; + else + drawEdge(L, 'r'); + break; + endif + ##If not, then check the next point + for i=1:index-1 + if MatrixOfLinks(index, i)==1 + disp('found a point connected'); + Points_2(z+1, 1:2)=Points(i, 1:2); + disp('new point added, and drawn'); + drawPoint(Points_2(z+1, 1:2), 'r'); + L=createEdge(Points_2(z+1, 1:2), Points_2(z, 1:2)); + drawEdge(L, 'r'); + index=i; + break; + endif + endfor + z=z+1; + disp('next step') + endwhile + + drawPoint(start, 'ro'); + +endfunction diff --git a/mapTest.mat b/mapTest.mat index 9d98561..13caa76 100644 --- a/mapTest.mat +++ b/mapTest.mat @@ -1,25 +1,25 @@ -# Created by Octave 7.3.0, Tue Jan 10 14:36:10 2023 GMT +# Created by Octave 7.3.0, Thu Jan 19 11:20:48 2023 GMT # name: Points # type: matrix # rows: 2 # columns: 10 - -2.9722694997088999 2.2027074585576791 -2.4163314018998832 -1.563641777935366 1.8247189104154553 -0.17308723248350155 0.24577290552178044 -0.80469834936605633 2.0135592026997866 -0.36560396489119051 - 0.047890118375208762 -0.42713698315704518 -0.30925089631194735 -1.216804326954257 1.3865824550438368 1.3067789455813377 -1.2708472666201218 1.1717992231942889 -0.02269206049426975 1.84133215165838 + 0.19697057332778345 -1.1771718145368819 -2.887110614207359 -1.4285411653803914 -1.6523013408997613 -1.363706683365784 1.4748850427562932 -1.1404559732519735 -2.1736190854336215 2.7723194289404676 + 1.745837315672909 0.75220366971764996 0.5218447246367357 -0.15692440100160332 1.3102147601497778 0.29066752826903497 -0.013557097054121003 -1.4995957486596576 -1.718447568352325 -0.91198013843207804 # name: MatrixOfLinks # type: matrix # rows: 10 # columns: 10 - 1 0 1 1 0 0 0 1 0 1 - 0 1 0 0 1 0 0 0 1 0 - 1 0 1 1 0 0 0 0 0 1 - 1 0 1 1 0 0 1 0 0 0 - 0 1 0 0 1 1 0 1 1 1 - 0 0 0 0 1 1 0 1 0 1 - 0 0 0 1 0 0 1 0 0 0 - 1 0 0 0 1 1 0 1 0 1 - 0 1 0 0 1 0 0 0 1 0 - 1 0 1 0 1 1 0 1 0 1 + 1 0 1 0 1 0 0 0 0 0 + 0 1 1 1 1 1 0 1 1 0 + 1 1 1 1 1 1 0 1 1 0 + 0 1 1 1 1 1 0 1 1 0 + 1 1 1 1 1 1 0 1 1 0 + 0 1 1 1 1 1 0 1 1 0 + 0 0 0 0 0 0 1 0 0 1 + 0 1 1 1 1 1 0 1 1 1 + 0 1 1 1 1 1 0 1 1 1 + 0 0 0 0 0 0 1 1 1 1 diff --git a/planPathPRM.m b/planPathPRM.m index e12fd2c..5b551e0 100644 --- a/planPathPRM.m +++ b/planPathPRM.m @@ -2,21 +2,87 @@ ## Created: 2023-01-08 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% -%function buildPRM (rangeQ1Q2, nbPoints, L1, L2, MapFilename) +%function planPathPRM(mapFilePath, startPoint, endPoint, L1, L2) % -% Task: +% Task: Establish the path from the starting point to the goal, using the points defined with the PRM method +% and using the Dijkstra method. % % Inputs: -% - +% - mapFilePath : the path to get the .mat file +% - startPoint : our starting point for the path planning +% - endPoint : our final goal % % Outputs: -% - +% - None % -% Adrien Lasserre (adrien.lasserre@ecam.fr) & Gwenn Durpoix-Espinasson (g.durpoix-espinasson@ecam.fr) +% Adrien Lasserre (adrien.lasserre@ecam.fr) & +% Gwenn Durpoix-Espinasson (g.durpoix-espinasson@ecam.fr) % 08/01/2023 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% -function planPathPRM () +function planPathPRM (mapFilePath, startPoint, endPoint, L1, L2) + mapFile = load(mapFilePath); + + S = startPoint; %define the start as S + G = endPoint; %define the goal as G + + n = columns(mapFile.MatrixOfLinks); %get the size of the MatrixOfLinks + connectionMatrix = zeros(n+2,n+2); %initialize a new connection matrix, with size n+2 to enter the S and G + connectionMatrix(2:n+1,2:n+1) = mapFile.MatrixOfLinks;%put the MatrixOfLinks inside + + points2Dmap = mapFile.Points'; %get the Points, but transposed (to use the findClosestPoint function) + + indexClosestPointS = findClosestPoint(S,points2Dmap) + 1 %+1 as in the first indexes of the connectionMatrix we put the S + indexClosestPointG = findClosestPoint(G,points2Dmap) + 1 %display those results + + connectionMatrix(1, indexClosestPointS) = 1; %use the previous results to update the connectionMatrix + connectionMatrix(indexClosestPointS, 1) = 1; + connectionMatrix(1, 1) = 1; + connectionMatrix(n+2, indexClosestPointG) = 1; + connectionMatrix(indexClosestPointG, n+2) = 1; + connectionMatrix(n+2, n+2) = 1; + + points2D = [S; points2Dmap; G] %store the points (with S and G) + connectionMatrix %debugging + + [numberOfNodes, visibilityGraph] = createVisibilityGraph(connectionMatrix, points2D) %create the visibilityGraph + + + [distanceToNode, parentOfNode, nodeTrajectory] = dijkstra(numberOfNodes,visibilityGraph); %and finally use the dijkstra algorithm for path planning + + figure 3; hold on; + b=drawCircle(0, 0, L1+L2); %teacher's functions for drawing circles + hold on; + c=drawCircle(0, 0, L2-L1); + hold on; + %creates the lines defining the prohibited areas + top_line = createLine([0,L1,1,0]); + bottom_line = createLine([0,-L1,1,0]); + drawLine(top_line); + hold on; + drawLine(bottom_line); + center_box=[L2 L2; -L2 L2; -L2 -L2; L2 -L2]; + drawPolygon(center_box); + hold on; + + + Points_path=zeros(size(nodeTrajectory(1,2))+1,2); + drawPoint(S, 'ro'); + + for i=1:size(nodeTrajectory, 2) + drawPoint(points2D(nodeTrajectory(1, i), 1:2), 'r'); + Points_path(i, 1:2)=points2D(nodeTrajectory(1, i), 1:2); + endfor + + drawPoint(G, 'ro'); + + for j=2:size(Points_path, 1) + L=createEdge(Points_path(j-1, 1:2), Points_path(j, 1:2)); + drawEdge(L, 'r'); + endfor + indexClosestPointFinal = findClosestPoint(G,Points_path); + L=createEdge(Points_path(indexClosestPointFinal, 1:2), G); + drawEdge(L, 'r'); endfunction diff --git a/qGraph.m b/qGraph.m index ec03b8d..d9c7924 100644 --- a/qGraph.m +++ b/qGraph.m @@ -4,10 +4,14 @@ %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %function buildPRM (rangeQ1Q2, nbPoints, L1, L2, MapFilename) % -% Task: +% Task: Create the C-Space graph for the PRM method, with the selected joint +% values, that weren't outside of bounds. % % Inputs: -% - +% - Q : matrix that stores the different joint values for each point. +% - nbPoints : number of points required +% - mat : the Matrix of links, that stores the information on which point is +% linked to which other one. % % Outputs: % - None @@ -20,16 +24,16 @@ function qGraph (Q, nbPoints, mat) hold off; - figure 2 - for i=1:nbPoints + figure 2 %launch a new figure + for i=1:nbPoints %double for loop to check every point for every link for j=1:nbPoints - drawPoint(Q(1,i), Q(2,i)); - current_point=[Q(1, i), Q(2, i)]; - previous_point=[Q(1, j), Q(2, j)]; - if mat(i, j)==1 & mat(j, i)==1 - L = createEdge(current_point, previous_point); + drawPoint(Q(1,i), Q(2,i)); %draw the point in C-space + current_point=[Q(1, i), Q(2, i)]; %change the current point + previous_point=[Q(1, j), Q(2, j)]; %change the previous point + if mat(i, j)==1 & mat(j, i)==1 %if there is a link between the two + L = createEdge(current_point, previous_point); %then create a line between both hold on; - drawEdge(L); + drawEdge(L); %and draw it on the figure. endif endfor endfor