planPathPRM functions

src/motion_planning/createVisibilityGraph.m

@@ -0,0 +1,21 @@
+function visibility_graph = createVisibilityGraph(position_valid, L1, L2)
+%%%%%%%%%%%
+% Create a visibility graph connecting valid points that have unobstructed line of sight
+% Author: Adrien COMBE
+% Date: 27/11/2023
+% Last Updated date : 01/12/2023
+%%%%%%%%%%%
+
+    num_points = size(position_valid, 1);
+    visibility_graph = zeros(num_points);
+
+    for i = 1:num_points
+        for j = i+1:num_points
+            if ~segmentsIntersect(position_valid(i, 1), position_valid(i, 2), ...
+                                  position_valid(j, 1), position_valid(j, 2), L1, L2)
+                visibility_graph(i, j) = norm(position_valid(i, :) - position_valid(j, :));
+                visibility_graph(j, i) = visibility_graph(i, j);
+            end
+        end
+    end
+end

src/motion_planning/dijkstra.m

@@ -0,0 +1,45 @@
+function [distances, path_indices] = dijkstra(graph, start, goal)
+%%%%%%%%%%%
+% Dijkstra's algorithm to find the shortest path in a graph
+% Author: Adrien COMBE
+% Date: 27/11/2023
+% Last Updated date : 01/12/2023
+%%%%%%%%%%
+
+    num_nodes = size(graph, 1);
+    visited = false(1, num_nodes);
+    distances = inf(1, num_nodes);
+    previous = zeros(1, num_nodes);
+    distances(start) = 0;
+
+    for i = 1:num_nodes
+        current = find(~visited, 1, 'first');
+        for j = find(~visited)
+            if distances(j) < distances(current)
+                current = j;
+            end
+        end
+
+        if distances(current) == inf
+            break;
+        end
+
+        visited(current) = true;
+
+        for j = 1:num_nodes
+            if ~visited(j) && graph(current, j) > 0
+                alt = distances(current) + graph(current, j);
+                if alt < distances(j)
+                    distances(j) = alt;
+                    previous(j) = current;
+                end
+            end
+        end
+    end
+
+    % Reconstruct the path
+    path_indices = goal;
+    while previous(path_indices(1)) ~= 0
+        path_indices = [previous(path_indices(1)), path_indices];
+    end
+end

src/motion_planning/planPathPRM.m

@@ -0,0 +1,67 @@
+function [path, path_length] = planPathPRM(L1, L2, nbPoints, start, goal)
+%%%%%%%%%%%%%%%
+% function planPathPRM(L1, L2, nbPoints, start, goal)
+% ex. [path, path_length] = planPathPRM(2000, 1000, 10, [2000, 0], [-2000, 0])
+%
+% Task:
+%   Plan a path from the start point to the goal point using the PRM approach.
+%
+% Inputs:
+%   - L1: length of the first link (in mm)
+%   - L2: length of the second link (in mm)
+%   - nbPoints: the number of valid points in the roadmap
+%   - start: starting point [x, y]
+%   - goal: goal point [x, y]
+%
+% Outputs:
+%   - path: array containing the waypoints of the planned path
+%   - path_length: total length of the planned path
+%
+% Author: Adrien COMBE
+% Date: 27/11/2023
+% Last Updated date : 01/12/2023
+%%%%%%%%%%%%%%%
+
+% Build the PRM
+[q1q2_valid, position_valid] = buildPRM(L1, L2, nbPoints);
+
+% Add start and goal points to the PRM
+start_valid = findNearestValidPoint(start, position_valid);
+goal_valid = findNearestValidPoint(goal, position_valid);
+q1q2_valid = [q1q2_valid; start_valid'; goal_valid'];
+position_valid = [position_valid; start; goal];
+
+% Create visibility graph
+visibility_graph = createVisibilityGraph(position_valid, L1, L2);
+
+% Use Dijkstra's algorithm to find the shortest path
+[start_idx, goal_idx] = deal(size(position_valid, 1) - 1, size(position_valid, 1));
+[~, path_indices] = dijkstra(visibility_graph, start_idx, goal_idx);
+
+% Extract the waypoints of the planned path
+path = position_valid(path_indices, :);
+path_length = computePathLength(path);
+
+% Plot the PRM and planned path
+%plotPRM(position_valid, L1, L2);
+hold on;
+plot(path(:, 1), path(:, 2), 'r-', 'LineWidth', 2);
+scatter(start(1), start(2), 'b', 'filled');
+scatter(goal(1), goal(2), 'b', 'filled');
+xlabel('X-axis');
+ylabel('Y-axis');
+title('PRM and Planned Path');
+
+end
+
+function distance = computePathLength(path)
+    % Compute the total length of the path
+    distance = sum(sqrt(sum(diff(path, 1, 1).^2, 2)));
+end
+
+function nearest_valid_point = findNearestValidPoint(point, position_valid)
+    % Find the nearest valid point to the given point
+    distances = sqrt(sum((position_valid - point).^2, 2));
+    [~, idx] = min(distances);
+    nearest_valid_point = position_valid(idx, :);
+end