From d6688e6b1c24d29bb9a69e62e6f6547a1af99e4c Mon Sep 17 00:00:00 2001 From: Cagdas Aras Ciblak Date: Mon, 4 Dec 2023 00:13:47 +0100 Subject: [PATCH] PRM OK, RRT not OK --- buildPRMnew.m | 78 +++++++++++++++++++++++++++++++++++ buildRRT.m | 68 ++++++++++++++++++++++++++++++ computePath.m | 8 ++++ createVisibilityGraph.m | 28 +++++++++++++ dijkstra.m | 46 +++++++++++++++++++++ findClosestPoint.m | 6 +++ findPointIndex.m | 12 ++++++ isHittingObstacle.m | 4 ++ isLineIntersectingObstacle.m | 4 ++ main.m | 5 +++ optimizePath.m | 14 +++++++ planPathPRM.m | 23 +++++++++++ plotPath.m | 20 +++++++++ plotRRT.m | 17 ++++++++ randRange.m | 4 ++ robot_arm_data.mat | Bin 0 -> 417 bytes 16 files changed, 337 insertions(+) create mode 100644 buildPRMnew.m create mode 100644 buildRRT.m create mode 100644 computePath.m create mode 100644 createVisibilityGraph.m create mode 100644 dijkstra.m create mode 100644 findClosestPoint.m create mode 100644 findPointIndex.m create mode 100644 isHittingObstacle.m create mode 100644 isLineIntersectingObstacle.m create mode 100644 main.m create mode 100644 optimizePath.m create mode 100644 planPathPRM.m create mode 100644 plotPath.m create mode 100644 plotRRT.m create mode 100644 randRange.m create mode 100644 robot_arm_data.mat diff --git a/buildPRMnew.m b/buildPRMnew.m new file mode 100644 index 0000000..8759e5e --- /dev/null +++ b/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 diff --git a/buildRRT.m b/buildRRT.m new file mode 100644 index 0000000..2fe35d7 --- /dev/null +++ b/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 diff --git a/computePath.m b/computePath.m new file mode 100644 index 0000000..1748156 --- /dev/null +++ b/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 \ No newline at end of file diff --git a/createVisibilityGraph.m b/createVisibilityGraph.m new file mode 100644 index 0000000..7e9b093 --- /dev/null +++ b/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 diff --git a/dijkstra.m b/dijkstra.m new file mode 100644 index 0000000..1def563 --- /dev/null +++ b/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 \ No newline at end of file diff --git a/findClosestPoint.m b/findClosestPoint.m new file mode 100644 index 0000000..8ce5170 --- /dev/null +++ b/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 \ No newline at end of file diff --git a/findPointIndex.m b/findPointIndex.m new file mode 100644 index 0000000..f0368f3 --- /dev/null +++ b/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 \ No newline at end of file diff --git a/isHittingObstacle.m b/isHittingObstacle.m new file mode 100644 index 0000000..d32bf4f --- /dev/null +++ b/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 \ No newline at end of file diff --git a/isLineIntersectingObstacle.m b/isLineIntersectingObstacle.m new file mode 100644 index 0000000..8bc5276 --- /dev/null +++ b/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 \ No newline at end of file diff --git a/main.m b/main.m new file mode 100644 index 0000000..506fbcc --- /dev/null +++ b/main.m @@ -0,0 +1,5 @@ +function main() + % Call the buildPRM function + buildPRMnew(); + planPathPRM(); +end \ No newline at end of file diff --git a/optimizePath.m b/optimizePath.m new file mode 100644 index 0000000..9dfd453 --- /dev/null +++ b/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 \ No newline at end of file diff --git a/planPathPRM.m b/planPathPRM.m new file mode 100644 index 0000000..057892f --- /dev/null +++ b/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 diff --git a/plotPath.m b/plotPath.m new file mode 100644 index 0000000..422790a --- /dev/null +++ b/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 diff --git a/plotRRT.m b/plotRRT.m new file mode 100644 index 0000000..8f538d8 --- /dev/null +++ b/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 diff --git a/randRange.m b/randRange.m new file mode 100644 index 0000000..d3df990 --- /dev/null +++ b/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 \ No newline at end of file diff --git a/robot_arm_data.mat b/robot_arm_data.mat new file mode 100644 index 0000000000000000000000000000000000000000..f6471f407efb77426565e85f3196f1e164e558d8 GIT binary patch literal 417 zcmeZu4DoSvQZUssQ1EpO(M`+DN!3vZ$Vn_o%P-2cQV4Jk_w+L}(NSF;y@!Ffvvk5-`93qo*%FkbND9E9N{-PDrR=IFjRV zhHh+1+vvKeYQlop5f8O?!}LY+S44QoiV)$=k2#bQSL? zF!|l(9x=_i14Fq}53huoKMuhjCHzm0#JY#+I