84 lines
2.8 KiB
Matlab
84 lines
2.8 KiB
Matlab
function path = planPathPRM(prmDataFile, startPoint, goalPoint)
|
|
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
|
% function path = planPathPRM(prmDataFile, startPoint, goalPoint)
|
|
%
|
|
% Task:
|
|
% Plan a path from start point S to goal point G using the PRM approach using a mat file.
|
|
%
|
|
% Inputs:
|
|
% - prmDataFile: MAT file containing PRM data (e.g., 'prm_data.mat')
|
|
% - start: Start point coordinates in cartesian space [x_start, y_start]
|
|
% - goal: Goal point coordinates in cartesian space [x_goal, y_goal]
|
|
%
|
|
% Outputs:
|
|
% - path: The planned path as a sequence of waypoints
|
|
%
|
|
% author: Camille CONJAT, camille.conjat@ecam.fr
|
|
% date: 03/12/2023
|
|
%
|
|
% Does not work correctly(an issue with the function inverseKinematics)
|
|
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
|
% Load PRM data from the MAT file
|
|
load('prm_data.mat', 'q1q2_valid', 'connectionMap', 'L1', 'L2', 'nbPoints');
|
|
|
|
% Add start and goal points to the map
|
|
startNode = inverseKinematics(startPoint(1), startPoint(2), L1, L2);
|
|
goalNode = inverseKinematics(goalPoint(1), goalPoint(2), L1, L2);
|
|
|
|
% Append start and goal nodes to the PRM map
|
|
q1q2_valid = [q1q2_valid startNode goalNode];
|
|
|
|
% Create visibility graph
|
|
visibilityGraph = zeros(nbPoints + 2, nbPoints + 2);
|
|
|
|
% Connect start and goal nodes to the existing nodes in the map
|
|
for i = 1:nbPoints + 2
|
|
% Connect start node
|
|
collisionStart = checkCollisionAlongLine(startNode, q1q2_valid(:, i), L1, L2);
|
|
if ~collisionStart
|
|
visibilityGraph(nbPoints + 1, i) = 1;
|
|
visibilityGraph(i, nbPoints + 1) = 1;
|
|
end
|
|
|
|
% Connect goal node
|
|
collisionGoal = checkCollisionAlongLine(goalNode, q1q2_valid(:, i), L1, L2);
|
|
if ~collisionGoal
|
|
visibilityGraph(nbPoints + 2, i) = 1;
|
|
visibilityGraph(i, nbPoints + 2) = 1;
|
|
end
|
|
|
|
% Connect other nodes in the map
|
|
for j = 1:nbPoints + 2
|
|
if i ~= j && connectionMap(i, j)
|
|
visibilityGraph(i, j) = 1;
|
|
visibilityGraph(j, i) = 1;
|
|
end
|
|
end
|
|
end
|
|
|
|
% Use Dijkstra algorithm to find the shortest path
|
|
[~, pathIndices] = dijkstra(visibilityGraph, nbPoints + 1, nbPoints + 2);
|
|
|
|
% Display the resulting path on the graph
|
|
subplot(1, 2, 2);
|
|
for i = 1:length(pathIndices) - 1
|
|
nodeA = pathIndices(i);
|
|
nodeB = pathIndices(i + 1);
|
|
pointA = q1q2_valid(1:2, nodeA);
|
|
pointB = q1q2_valid(1:2, nodeB);
|
|
plot([pointA(1), pointB(1)], [pointA(2), pointB(2)], 'b', 'LineWidth', 2); hold on;
|
|
end
|
|
|
|
% Display start and goal points
|
|
plot(startPoint(1), startPoint(2), 'mo', 'MarkerSize', 10, 'LineWidth', 2);
|
|
plot(goalPoint(1), goalPoint(2), 'co', 'MarkerSize', 10, 'LineWidth', 2);
|
|
|
|
% Add legend and title
|
|
legend('Obstacles', 'Valid points', 'Path', 'Start', 'Goal', 'Location', 'Best');
|
|
title('Path Planning using PRM');
|
|
|
|
% Update axis limits
|
|
xlim([-(L1 + L2) (L1 + L2)]);
|
|
ylim([-(L1 + L2) (L1 + L2)]);
|
|
end
|