21 lines
657 B
Matlab
21 lines
657 B
Matlab
function plotPath(path, q1q2_valid, S, G)
|
|
% Plot the PRM points and the start and goal points
|
|
figure;
|
|
plot(q1q2_valid(1, :), q1q2_valid(2, :), 'bo'); % PRM points
|
|
hold on;
|
|
plot(S(1), S(2), 'go', 'MarkerSize', 10, 'MarkerFaceColor', 'g'); % Start point
|
|
plot(G(1), G(2), 'ro', 'MarkerSize', 10, 'MarkerFaceColor', 'r'); % Goal point
|
|
|
|
% Plot the path
|
|
for i = 1:size(path, 2) - 1
|
|
plot([path(1, i), path(1, i+1)], [path(2, i), path(2, i+1)], 'k-', 'LineWidth', 2);
|
|
end
|
|
|
|
% Set plot attributes
|
|
xlabel('x(mm)');
|
|
ylabel('y(mm)');
|
|
title('Shortest Path in Cartesian Space');
|
|
grid on;
|
|
axis equal;
|
|
end
|