24 lines
732 B
Matlab
24 lines
732 B
Matlab
function planPathPRM()
|
|
% Load the data from the previously generated PRM
|
|
data = load('robot_arm_data.mat');
|
|
q1q2_valid = data.q1q2_valid;
|
|
connectionMap = data.connectionMap;
|
|
|
|
% Parameters for the robot arm (you may need to adjust these)
|
|
L1 = 2; % Length of the first link in mm
|
|
L2 = 1; % Length of the second link in mm
|
|
|
|
% Add Start and Goal points
|
|
S = [2; 0];
|
|
G = [-2; 0];
|
|
|
|
% Create visibility graph including Start and Goal points
|
|
[visibilityGraph, pointMap] = createVisibilityGraph(q1q2_valid, S, G, L1, L2);
|
|
|
|
% Find the shortest path using Dijkstra algorithm
|
|
path = dijkstra(visibilityGraph, pointMap, S, G);
|
|
|
|
% Plot the path
|
|
plotPath(path, q1q2_valid, S, G);
|
|
end
|