61 lines
2.2 KiB
Matlab
61 lines
2.2 KiB
Matlab
function planPathPRM(L1, L2, nbPoints)
|
|
% Build PRM
|
|
[q1q2_valid, obstacles] = buildPRM(L1, L2, nbPoints);
|
|
|
|
% Define start and goal points
|
|
start_point = [2000; 0];
|
|
goal_point = [-2000; 0];
|
|
|
|
% Convert q1q2_valid into 2D Cartesian coordinates
|
|
points2D = [];
|
|
for i = 1:size(q1q2_valid, 2)
|
|
pos = dh2ForwardKinematics([q1q2_valid(1, i); q1q2_valid(2, i)], [0; 0], [L1; L2], [0; 0], 1)(1:2, end);
|
|
points2D = [points2D; pos'];
|
|
end
|
|
|
|
% Create a connection matrix
|
|
connectionMatrix = zeros(nbPoints, nbPoints);
|
|
|
|
% Calculate distances and create connections
|
|
for i = 1:size(q1q2_valid, 2)
|
|
for j = 1:size(q1q2_valid, 2)
|
|
if i ~= j
|
|
pos1 = dh2ForwardKinematics([q1q2_valid(1, i); q1q2_valid(2, i)], [0; 0], [L1; L2], [0; 0], 1)(1:2, end);
|
|
pos2 = dh2ForwardKinematics([q1q2_valid(1, j); q1q2_valid(2, j)], [0; 0], [L1; L2], [0; 0], 1)(1:2, end);
|
|
if ~isLineIntersectingObstacle(pos1, pos2, obstacles)
|
|
connectionMatrix(i, j) = 1;
|
|
connectionMatrix(j, i) = 1; % Ensure bidirectional connection
|
|
end
|
|
end
|
|
end
|
|
end
|
|
|
|
% Create the visibility graph
|
|
[nbNodes, visibilityGraph] = createVisibilityGraph(connectionMatrix, points2D);
|
|
|
|
% Find the nearest points in q1q2_valid to the start and goal points
|
|
start_index = findNearestPoint(start_point, points2D);
|
|
goal_index = findNearestPoint(goal_point, points2D);
|
|
|
|
% Find the shortest path using Dijkstra's algorithm
|
|
[distanceToNode, parentOfNode, nodeTrajectory] = dijkstra(nbNodes, visibilityGraph);
|
|
|
|
% Plot the path in Cartesian space
|
|
subplot(1, 2, 2);
|
|
hold on;
|
|
for i = 1:length(nodeTrajectory) - 1
|
|
pos1 = points2D(nodeTrajectory(i), :);
|
|
pos2 = points2D(nodeTrajectory(i + 1), :);
|
|
plot([pos1(1), pos2(1)], [pos1(2), pos2(2)], 'b', 'LineWidth', 2); % Plot path in blue
|
|
end
|
|
|
|
% Plot the start and goal points
|
|
plot(start_point(1), start_point(2), 'ro', 'MarkerSize', 10); % Red circle for start
|
|
plot(goal_point(1), goal_point(2), 'go', 'MarkerSize', 10); % Green circle for goal
|
|
|
|
title('Cartesian Space with Path');
|
|
legend('Obstacles', 'Path', 'Start', 'Goal');
|
|
hold off;
|
|
end
|
|
|