TD5 finished
This commit is contained in:
parent
524192f32f
commit
2e4611af0d
50
buildPRM.m
50
buildPRM.m
|
|
@ -9,6 +9,7 @@ function buildPRM(L1, L2, nbPoints)
|
|||
% -nbVP : Number of Valid Points
|
||||
%
|
||||
%Outputs:
|
||||
% - .mat file with the connectionMap
|
||||
%
|
||||
%Author : Thomas Périn, thomas.perin@ecam.fr
|
||||
%Date/ 22/11/2023
|
||||
|
|
@ -102,10 +103,11 @@ subplot(1,2,2);
|
|||
|
||||
% creates a connection map
|
||||
connectionMap = zeros(nbPoints, nbPoints);
|
||||
xy_valid = [];
|
||||
|
||||
for l=1:size(q1q2_valid,2)
|
||||
for m=1:size(q1q2_valid,2)
|
||||
if (l ~=m)
|
||||
if (l ~= m)
|
||||
% creates the DH table
|
||||
theta = q1q2_valid(:,l);
|
||||
d = [0; 0];
|
||||
|
|
@ -125,11 +127,49 @@ for l=1:size(q1q2_valid,2)
|
|||
B = wTee(1:2,end);
|
||||
|
||||
% computes y =ax + b
|
||||
a = (A(2)-B(2))/(A(1)-B(1));
|
||||
b = A(2) - a * A(1);
|
||||
|
||||
% if no collision
|
||||
%connectionMap(l,m) = 1;
|
||||
% else
|
||||
%connectionMap(l,m) = 0;
|
||||
no_collision = true;
|
||||
% crosses x =-1000 ???
|
||||
if (min(A(1), B(1)) <= -1000 && max(A(1), B(1)) >= -1000)
|
||||
if ((a * (-1000) + b ) < -1000) || ((a * (-1000) + b ) > 1000)
|
||||
no_collision = true;
|
||||
else
|
||||
no_collision = false;
|
||||
end
|
||||
end
|
||||
% crosses x =1000 ???
|
||||
if (min(A(1), B(1)) <= 1000 && max(A(1), B(1)) >= 1000 && no_collision)
|
||||
if ((a * (1000) + b ) < -1000) || ((a * (1000) + b ) > 1000)
|
||||
no_collision = true;
|
||||
else
|
||||
no_collision = false;
|
||||
end
|
||||
end
|
||||
% crosses y =-1000 ???
|
||||
if (min(A(2), B(2)) <= -1000 && max(A(2), B(2)) >= -1000 && no_collision)
|
||||
if (((-1000-b)/a ) < -1000) || (((-1000-b)/a ) > 1000)
|
||||
no_collision = true;
|
||||
else
|
||||
no_collision = false;
|
||||
end
|
||||
end
|
||||
|
||||
if no_collision
|
||||
connectionMap(l,m) = 1;
|
||||
subplot(1,2,1);
|
||||
plot([q1q2_valid(1,l) q1q2_valid(1,m)],[q1q2_valid(2,l) q1q2_valid(2,m)], '-g'); hold on;
|
||||
subplot(1,2,2);
|
||||
plot([A(1) B(1)],[A(2) B(2)], '-g'); hold on;
|
||||
else
|
||||
connectionMap(l,m) = 0;
|
||||
end
|
||||
end
|
||||
end
|
||||
xy_valid = [xy_valid [A(1); A(2)]];
|
||||
end
|
||||
connectionMap
|
||||
xy_valid
|
||||
% Save the matrix to a .mat file
|
||||
save('dataFile.mat', 'connectionMap', 'xy_valid');
|
||||
|
|
|
|||
|
|
@ -0,0 +1,17 @@
|
|||
# Created by Octave 8.4.0, Sun Dec 03 22:49:46 2023 GMT <unknown@LAPTOP-T>
|
||||
# name: connectionMap
|
||||
# type: matrix
|
||||
# rows: 10
|
||||
# columns: 10
|
||||
0 0 0 0 0 0 0 0 1 1
|
||||
0 0 0 1 1 0 0 1 0 0
|
||||
0 0 0 0 0 1 1 0 1 0
|
||||
0 1 0 0 1 0 1 1 0 0
|
||||
0 1 0 1 0 0 0 1 0 0
|
||||
0 0 1 0 0 0 1 0 1 0
|
||||
0 0 1 1 0 1 0 0 1 0
|
||||
0 1 0 1 1 0 0 0 0 0
|
||||
1 0 1 0 0 1 1 0 0 1
|
||||
1 0 0 0 0 0 0 0 1 0
|
||||
|
||||
|
||||
|
|
@ -0,0 +1,25 @@
|
|||
# Created by Octave 8.4.0, Sun Dec 03 23:07:59 2023 GMT <unknown@LAPTOP-T>
|
||||
# name: connectionMap
|
||||
# type: matrix
|
||||
# rows: 10
|
||||
# columns: 10
|
||||
0 0 1 0 1 0 1 1 1 1
|
||||
0 0 0 1 0 1 1 0 0 0
|
||||
1 0 0 0 1 0 1 1 0 0
|
||||
0 1 0 0 1 1 1 0 0 0
|
||||
1 0 1 1 0 0 1 1 0 0
|
||||
0 1 0 1 0 0 1 0 1 0
|
||||
1 1 1 1 1 1 0 1 0 0
|
||||
1 0 1 0 1 0 1 0 1 1
|
||||
1 0 0 0 0 1 0 1 0 1
|
||||
1 0 0 0 0 0 0 1 1 0
|
||||
|
||||
|
||||
# name: xy_valid
|
||||
# type: matrix
|
||||
# rows: 2
|
||||
# columns: 10
|
||||
-1183.1144222084818 1264.4708348085574 -334.39822518231665 2171.4653844739678 655.97776462668855 1073.0376457230295 2337.6632188169524 -2724.1855429400352 -1270.0317481226446 -1301.9713130947187
|
||||
1782.3056782605233 122.44277855170276 1340.3707372349479 245.01665496817861 1581.186555640114 -1823.1319004730015 1824.6138407095095 1199.9716377405398 -1315.9879394455597 -147.11573260990281
|
||||
|
||||
|
||||
|
|
@ -0,0 +1,71 @@
|
|||
function [distanceToNode, parentOfNode, nodeTrajectory] = dijkstra(nbNodes, visibilityGraph)
|
||||
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
||||
%function [distanceToNode, parentOfNode, nodeTrajectory] = dijkstra(nbNodes, visibilityGraph)
|
||||
%
|
||||
% Task: Perform the Dijkstra algorithm on a given visibility graph
|
||||
%
|
||||
% Inputs:
|
||||
% -nbNodes: number of nodes of the graph excluding the starting and goal points
|
||||
% -visibilityGraph: a matrix containing the distance between connected nodes
|
||||
% (NaN refers to not connected nodes)
|
||||
% The matrix has a size of (nbNodes+2)x(nbNodes+2)
|
||||
% The first row/col corresponds to the Starting point, the last row/col to the Goal point.
|
||||
%
|
||||
% Outputs:
|
||||
% - distanceToNode: distance between the current node and its parent
|
||||
% - parentOfNode: index of the parent node for each node
|
||||
% - nodeTrajectory: best trajectory
|
||||
%
|
||||
% Guillaume Gibert (guillaume.gibert@ecam.fr)
|
||||
% 17/03/2021
|
||||
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
||||
|
||||
constantLargeDitance=10000;
|
||||
|
||||
visitedNodes = zeros(1, nbNodes+2);
|
||||
distanceToNode = constantLargeDitance*ones(1, nbNodes+2);
|
||||
distanceToNode(1) = 0;
|
||||
parentOfNode = zeros(1, nbNodes+2);
|
||||
|
||||
fprintf('##Starting Dijkstra''s algorithm...\n')
|
||||
|
||||
while (sum(visitedNodes(:)==0))
|
||||
thresholdDistance = constantLargeDitance+1;
|
||||
for l_node=1:nbNodes+2
|
||||
%l_node
|
||||
if (visitedNodes(l_node)==0 && distanceToNode(l_node) < thresholdDistance)
|
||||
minIndex = l_node;
|
||||
thresholdDistance = distanceToNode(l_node);
|
||||
end
|
||||
end
|
||||
|
||||
fprintf('-->Visiting N%d\n', minIndex-1)
|
||||
|
||||
visitedNodes(minIndex) = 1;
|
||||
for l_node=1:nbNodes+2
|
||||
%l_node
|
||||
if (l_node~=minIndex && ~isnan(visibilityGraph(minIndex, l_node)))
|
||||
distance = distanceToNode(minIndex) + visibilityGraph(minIndex,l_node);
|
||||
if (distance < distanceToNode(l_node))
|
||||
distanceToNode(l_node) = distance;
|
||||
parentOfNode(l_node) = minIndex;
|
||||
end
|
||||
end
|
||||
end
|
||||
end
|
||||
fprintf('##Dijkstra''s algorithm is done!\n')
|
||||
fprintf('##Results\n')
|
||||
fprintf('Minimal distance to target: %d\n', distanceToNode(nbNodes+2))
|
||||
nodeIndex = nbNodes+2;
|
||||
nodeTrajectory = [];
|
||||
while(nodeIndex~=1)
|
||||
nodeIndex = parentOfNode(nodeIndex);
|
||||
nodeTrajectory = [nodeTrajectory nodeIndex];
|
||||
end
|
||||
fprintf('S-->');
|
||||
for l_node=2:length(nodeTrajectory)
|
||||
fprintf('N%d-->', nodeTrajectory(length(nodeTrajectory)-(l_node-1))-1);
|
||||
end
|
||||
fprintf('G\n');
|
||||
fprintf('########\n');
|
||||
|
||||
|
|
@ -0,0 +1,44 @@
|
|||
function retval = planPathPRM ()
|
||||
%%%%%%%%%%%%%%%%%%%
|
||||
%planPathPRM ()
|
||||
%ex. planPathPRM()
|
||||
%
|
||||
%Inputs:
|
||||
%
|
||||
%
|
||||
%Outputs:
|
||||
%
|
||||
%Author : Thomas Périn, thomas.perin@ecam.fr
|
||||
%Date/ 22/11/2023
|
||||
%%%%%%%%%%%%%%%%%%%
|
||||
|
||||
load('dataFile.mat')
|
||||
|
||||
start = [2000; 0];
|
||||
goal = [-2000; 0];
|
||||
|
||||
nbNodes = size(xy_valid,2);
|
||||
|
||||
visibilityGraph = zeros(size(xy_valid,2)+2,size(xy_valid,2)+2);
|
||||
|
||||
for l = 1:(size(visibilityGraph,2)-1)
|
||||
for m = 1:(size(visibilityGraph,2)-1)
|
||||
if l ~= m
|
||||
if l == 1
|
||||
visibilityGraph(l , m) = sqrt((start(1)-xy_valid(1, m-1))^2 + xy_valid(2, m-1)^2);
|
||||
elseif m == 1
|
||||
visibilityGraph(l , m) = sqrt((start(1)-xy_valid(1, l-1))^2 + xy_valid(2, l-1)^2);
|
||||
elseif connectionMap(l-1, m-1) == 1
|
||||
visibilityGraph(l , m) = sqrt((xy_valid(1, m-1)-xy_valid(1, l-1))^2 + (xy_valid(2, m-1)-xy_valid(2, l-1))^2);
|
||||
end
|
||||
end
|
||||
end
|
||||
end
|
||||
for s = 2:(size(visibilityGraph,2)-1)
|
||||
visibilityGraph(s , 12) = sqrt((goal(1)-xy_valid(1, s-1))^2 + xy_valid(2, s-1)^2);
|
||||
visibilityGraph(12 , s) = sqrt((goal(1)-xy_valid(1, s-1))^2 + xy_valid(2, s-1)^2);
|
||||
end
|
||||
visibilityGraph
|
||||
[distanceToNode, parentOfNode, nodeTrajectory] = dijkstra(nbNodes, visibilityGraph);
|
||||
|
||||
endfunction
|
||||
Loading…
Reference in New Issue