Merge branch 'Gwenn'
This commit is contained in:
commit
20d20cdf11
18
buildPRM.m
18
buildPRM.m
|
|
@ -1,10 +1,16 @@
|
|||
## Author: adril <adril@LAPTOP-EJ1AIJHT>
|
||||
## 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)
|
||||
|
||||
|
|
|
|||
|
|
@ -0,0 +1,189 @@
|
|||
## Author: adril <adril@LAPTOP-EJ1AIJHT>
|
||||
## 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
|
||||
26
mapTest.mat
26
mapTest.mat
|
|
@ -1,25 +1,25 @@
|
|||
# Created by Octave 7.3.0, Tue Jan 10 14:36:10 2023 GMT <unknown@LAPTOP-EJ1AIJHT>
|
||||
# Created by Octave 7.3.0, Thu Jan 19 11:20:48 2023 GMT <unknown@LAPTOP-EJ1AIJHT>
|
||||
# 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
|
||||
|
||||
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
24
qGraph.m
24
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
|
||||
|
|
|
|||
Loading…
Reference in New Issue