PRM OK, RRT not OK

buildPRMnew.m

@@ -0,0 +1,78 @@
+function buildPRMnew()
+    % Parameters
+    L1 = 2; % Length of the first link in mm
+    L2 = 1; % Length of the second link in mm
+    nbPoints = 10; % Number of valid points in the roadmap
+
+    q1q2_valid = [];
+    positions = [];
+    connectionMap = zeros(nbPoints, nbPoints);
+
+    % Plotting setup
+    figure;
+
+    % Generating points and checking for obstacles
+    while size(q1q2_valid, 2) < nbPoints
+        q1 = rand() * 360.0;
+        q2 = rand() * 360.0;
+
+        % DH parameters
+        theta = [q1; q2] * pi / 180; % Convert to radians
+        d = [0; 0];
+        a = [L1; L2];
+        alpha = [0; 0];
+
+        % Forward Kinematics
+        wTee = dh2ForwardKinematics(theta, d, a, alpha);
+
+        % Position of the end-effector
+        position_ee = wTee(1:2, 4);
+
+        if ~isHittingObstacle(position_ee, L1, L2)
+            q1q2_valid = [q1q2_valid [q1; q2]];
+            positions = [positions position_ee];
+
+            % Plot in joint space
+            subplot(1, 2, 1);
+            plot(q1, q2, '+g'); hold on;
+            xlabel('q1(deg)');
+            ylabel('q2(deg)');
+            title('Joint space');
+            xlim([0 360]);
+            ylim([0 360]);
+
+            % Plot in cartesian space
+            subplot(1, 2, 2);
+            plot(position_ee(1), position_ee(2), '+g'); hold on;
+            xlabel('x(mm)');
+            ylabel('y(mm)');
+            title('Cartesian space');
+        end
+    end
+
+    % Connect valid points and plot their connections
+    for i = 1:size(q1q2_valid, 2)
+        for j = 1:size(q1q2_valid, 2)
+            if i ~= j && ~isLineIntersectingObstacle(positions(:, i), positions(:, j), L1, L2)
+                connectionMap(i, j) = 1;
+                subplot(1, 2, 2);
+                plot([positions(1, i), positions(1, j)], [positions(2, i), positions(2, j)], 'b-'); hold on;
+            end
+        end
+    end
+
+    % Save data to a .mat file
+    save('robot_arm_data.mat', 'q1q2_valid', 'connectionMap');
+end
+
+% Function to compute the forward kinematics using D-H parameters
+function T = dh2ForwardKinematics(theta, d, a, alpha)
+    T = eye(4);
+    for i = 1:length(theta)
+        T_i = [cos(theta(i)), -sin(theta(i))*cos(alpha(i)), sin(theta(i))*sin(alpha(i)), a(i)*cos(theta(i));
+               sin(theta(i)), cos(theta(i))*cos(alpha(i)), -cos(theta(i))*sin(alpha(i)), a(i)*sin(theta(i));
+               0, sin(alpha(i)), cos(alpha(i)), d(i);
+               0, 0, 0, 1];
+        T = T * T_i;
+    end
+end

buildRRT.m

@@ -0,0 +1,68 @@
+function buildRRT()
+    % 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
+    L1 = 2; % Length of the first link in mm
+    L2 = 1; % Length of the second link in mm
+
+    % Define start and goal points (as column vectors)
+    S = [2; 0];
+    G = [-2; 0];
+
+    % Initialize tree with start point
+    tree = S;
+    reachedGoal = false;
+
+    % Define a threshold for when the goal is considered reached
+    someThreshold = 0.1; % This is an example value; adjust as needed for your application
+
+    % Define the maximum number of iterations to prevent infinite loop
+    maxIterations = 1000;
+
+    % Define the maximum branch length
+    maxBranchLength = 0.5; % This is an example value; adjust as needed for your application
+
+    for i = 1:maxIterations
+        % Sample a random point in C-space
+        r = [randRange(-L1-L2, L1+L2); randRange(-L1-L2, L1+L2)];
+
+        % Find the closest point in the tree to r
+        [p, idx] = findClosestPoint(tree, r);
+
+        % Compute the direction and length to the random point
+        direction = r - p;
+        distance = norm(direction);
+        if distance > maxBranchLength
+            % Scale the branch to the maximum branch length
+            r = p + (direction / distance) * maxBranchLength;
+        end
+
+        % Check if new branch intersects an obstacle
+        if ~isLineIntersectingObstacle(p, r, L1, L2)
+            % Add the branch to the tree
+            tree = [tree, r];
+
+            % Check if we have reached the goal
+            if norm(r - G) < someThreshold
+                reachedGoal = true;
+                break;
+            end
+        end
+    end
+
+    if reachedGoal
+        % Compute path from start to goal
+        path = computePath(tree, idx, G);
+
+        % Add direct lines between any two points if obstacle-free
+        path = optimizePath(path, L1, L2);
+
+        % Plot the tree and the path
+        plotRRT(tree, path, S, G);
+    else
+        error('Failed to reach the goal within the maximum number of iterations.');
+    end
+end

computePath.m

@@ -0,0 +1,8 @@
+function path = computePath(tree, parentIndices, currentIdx)
+    % Backtrack from the goal index to the start index using the parent indices
+    path = tree(:, currentIdx);
+    while parentIndices(currentIdx) ~= 0
+        currentIdx = parentIndices(currentIdx);
+        path = [tree(:, currentIdx), path];
+    end
+end

createVisibilityGraph.m

@@ -0,0 +1,28 @@
+function [visibilityGraph, pointMap] = createVisibilityGraph(q1q2_valid, S, G, L1, L2)
+    % Combine the PRM points with the Start and Goal points
+    allPoints = [S, G, q1q2_valid];
+    numPoints = size(allPoints, 2);
+
+    % Create a map for point indexing
+    pointMap = containers.Map('KeyType', 'double', 'ValueType', 'any');
+    for i = 1:numPoints
+        pointMap(i) = allPoints(:, i);
+    end
+
+    % Initialize the visibility graph
+    visibilityGraph = zeros(numPoints, numPoints);
+
+    % Check visibility between each pair of points
+    for i = 1:numPoints
+        for j = i+1:numPoints
+            if i ~= j
+                A = allPoints(:, i);
+                B = allPoints(:, j);
+                if ~isLineIntersectingObstacle(A, B, L1, L2)
+                    visibilityGraph(i, j) = norm(A - B); % Distance between A and B
+                    visibilityGraph(j, i) = visibilityGraph(i, j); % The graph is undirected
+                end
+            end
+        end
+    end
+end

dijkstra.m

@@ -0,0 +1,46 @@
+function path = dijkstra(visibilityGraph, pointMap, S, G)
+    numPoints = size(visibilityGraph, 1);
+    
+    % Find indices for Start and Goal in the point map
+    startIndex = findPointIndex(pointMap, S);
+    goalIndex = findPointIndex(pointMap, G);
+
+    % Initialize distance and previous node arrays
+    dist = inf(numPoints, 1);
+    dist(startIndex) = 0;
+    prev = -ones(numPoints, 1);
+
+    % Create a priority queue and add the start index
+    priorityQueue = startIndex;
+
+    while ~isempty(priorityQueue)
+        % Find the point with the smallest distance in the queue
+        [~, uIdx] = min(dist(priorityQueue));
+        u = priorityQueue(uIdx);
+        priorityQueue(uIdx) = []; % Remove u from the queue
+
+        % For each neighbor v of u
+        for v = 1:numPoints
+            if visibilityGraph(u, v) > 0
+                alt = dist(u) + visibilityGraph(u, v);
+                if alt < dist(v)
+                    dist(v) = alt;
+                    prev(v) = u;
+                    if isempty(find(priorityQueue == v, 1))
+                        priorityQueue(end + 1) = v; % Add v to the queue
+                    end
+                end
+            end
+        end
+    end
+
+    % Reconstruct the shortest path
+    path = [];
+    u = goalIndex;
+    if prev(u) ~= -1 || u == startIndex
+        while u ~= -1
+            path = [pointMap(u) path];
+            u = prev(u);
+        end
+    end
+end

findClosestPoint.m

@@ -0,0 +1,6 @@
+function [closestPoint, closestIdx] = findClosestPoint(tree, point)
+    % Finds the closest point in the tree to a given random point
+    distances = sqrt(sum((tree - point).^2, 1));
+    [minDistance, closestIdx] = min(distances);
+    closestPoint = tree(:, closestIdx);
+end

findPointIndex.m

@@ -0,0 +1,12 @@
+function idx = findPointIndex(pointMap, point)
+    % Find the index of a point in the pointMap
+    keys = cell2mat(pointMap.keys);
+    values = cell2mat(pointMap.values);
+    for i = 1:length(keys)
+        if isequal(values(:, i), point)
+            idx = keys(i);
+            return;
+        end
+    end
+    error('Point not found in the map.');
+end

isHittingObstacle.m

@@ -0,0 +1,4 @@
+function result = isHittingObstacle(position, L1, L2)
+    result = position(2) >= L1 || position(2) <= -L1 || ...
+             (-L2 <= position(1) && position(1) <= L2 && -L2 <= position(2) && position(2) <= L2);
+end

isLineIntersectingObstacle.m

@@ -0,0 +1,4 @@
+function result = isLineIntersectingObstacle(A, B, L1, L2)
+    % Implement logic to check if the line segment AB intersects any of the obstacles
+    result = false; % Placeholder, assuming no intersection for simplicity
+end

main.m

@@ -0,0 +1,5 @@
+function main()
+    % Call the buildPRM function
+    buildPRMnew();
+    planPathPRM();
+end

optimizePath.m

@@ -0,0 +1,14 @@
+function optimizedPath = optimizePath(path, L1, L2)
+    % Shortens the path by connecting non-consecutive nodes directly if there's no obstacle
+    optimizedPath = path(:, 1);
+    skip = 0;
+    for i = 1:size(path, 2)-1
+        for j = size(path, 2):-1:i+1+skip
+            if ~isLineIntersectingObstacle(path(:, i), path(:, j), L1, L2)
+                optimizedPath = [optimizedPath, path(:, j)];
+                skip = j - i - 1;
+                break;
+            end
+        end
+    end
+end

planPathPRM.m

@@ -0,0 +1,23 @@
+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

plotPath.m

@@ -0,0 +1,20 @@
+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

plotRRT.m

@@ -0,0 +1,17 @@
+function plotRRT(tree, path, S, G)
+    % Visualizes the tree and the path
+    figure;
+    plot(tree(1, :), tree(2, :), 'bo', 'MarkerFaceColor', 'b'); % Tree nodes
+    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
+    for i = 1:length(tree)-1
+        line([tree(1, i), tree(1, i+1)], [tree(2, i), tree(2, i+1)], 'Color', 'b'); % Tree branches
+    end
+    line(path(1, :), path(2, :), 'Color', 'r', 'LineWidth', 2); % Path
+    title('RRT Path Planning');
+    xlabel('x');
+    ylabel('y');
+    grid on;
+    axis equal;
+end

randRange.m

@@ -0,0 +1,4 @@
+function val = randRange(minVal, maxVal)
+    % Generates a random number within a specified range
+    val = (maxVal - minVal).*rand() + minVal;
+end