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