From 7cfd0e5750424272c2ca07e331e93563a7781319 Mon Sep 17 00:00:00 2001 From: "lucas.marais" Date: Sun, 3 Dec 2023 18:05:14 +0100 Subject: [PATCH] drawing path --- IsIntersecting.m | 78 ++++++++++++++++++++++++++++++ buildRRT.m | 121 +++++++++++++++++++++++++++++++++++++++++++---- test.m | 5 +- 3 files changed, 195 insertions(+), 9 deletions(-) create mode 100644 IsIntersecting.m diff --git a/IsIntersecting.m b/IsIntersecting.m new file mode 100644 index 0000000..640ef6f --- /dev/null +++ b/IsIntersecting.m @@ -0,0 +1,78 @@ +function intersect = IsIntersecting (L1, L2, closestPoint, newPoint) + %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + % function path = buildRRT(L1, L2, start, finish) + % Task: Determine the 3D transformation matrix corresponding to a set of Denavit-Hartenberg parameters + % + % Inputs: + % - L1: first length + % - L2: second length + % - closestPoint: start point x y + % - newPoint: end point x y + % + % Output: + % -path: Vector of points + % + % author: Marais Lucas + % date: 22/11/2023 + x3 = [-L2 -L2 L2 L2]; + y3 = [-L2 L2 L2 -L2]; + x1 = [-L1-L2 -L1-L2 L1+L2 L1+L2]; + y1 = [-L1-L2 L1+L2 L1+L2 -L1-L2]; + x2 = [-L1-L2 -L1-L2 L1+L2 L1+L2]; + y2 = [-L1 L1 L1 -L1]; + % checks if the path is crossed by an obstacle + crossesObstacle = false; + for i = 1:length(x1) + edge1 = [x1(i), y1(i), x1(mod(i, 4) + 1), y1(mod(i, 4) + 1)]; + edge2 = [x2(i), y2(i), x2(mod(i, 4) + 1), y2(mod(i, 4) + 1)]; + edge3 = [x3(i), y3(i), x3(mod(i, 4) + 1), y3(mod(i, 4) + 1)]; + + % Check if the line intersects with any obstacle edge + if doIntersect(closestPoint, newPoint, edge1(1:2), edge1(3:4)) || ... + doIntersect(closestPoint, newPoint, edge2(1:2), edge2(3:4)) || ... + doIntersect(closestPoint, newPoint, edge3(1:2), edge3(3:4)) + crossesObstacle = true; + break; + end + end + + % Return the result + intersect = crossesObstacle; +endfunction + +function intersects = doIntersect(p1, q1, p2, q2) + % Function to check if two line segments (p1, q1) and (p2, q2) intersect + + if (p1 == q1) || (p2 == q2) + intersects = false; % Degenerate cases, no intersection + return; + end + + % Check if the line segments are not collinear + if orientation(p1, q1, p2) ~= orientation(p1, q1, q2) && ... + orientation(p2, q2, p1) ~= orientation(p2, q2, q1) + intersects = true; + return; + end + + intersects = false; % No intersection +end + + +function o = orientation(p, q, r) + % Function to find the orientation of triplet (p, q, r) + % Returns: + % 0 -> Collinear points + % 1 -> Clockwise points + % 2 -> Counterclockwise points + + val = (q(2) - p(2)) * (r(1) - q(1)) - (q(1) - p(1)) * (r(2) - q(2)); + + if val == 0 + o = 0; % Collinear + elseif val > 0 + o = 1; % Clockwise + else + o = 2; % Counterclockwise + end +end diff --git a/buildRRT.m b/buildRRT.m index 2ca0ac7..0d63289 100644 --- a/buildRRT.m +++ b/buildRRT.m @@ -1,6 +1,6 @@ -function path = buildRRT(L1, L2, x1, y1, x2, y2) +function path = buildRRT(L1, L2, pt1, pt2) %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% - % function path = buildPRM(L1, L2) + % function path = buildRRT(L1, L2, start, finish) % Task: Determine the 3D transformation matrix corresponding to a set of Denavit-Hartenberg parameters % % Inputs: @@ -25,17 +25,23 @@ function path = buildRRT(L1, L2, x1, y1, x2, y2) y1 = [-L1-L2 L1+L2 L1+L2 -L1-L2]; x2 = [-L1-L2 -L1-L2 L1+L2 L1+L2]; y2 = [-L1 L1 L1 -L1]; + xy_valid = [] + xy_valid(end+1,:) = pt1 + q1q2_valid = []; + validLinks = []; + distanceBetweenPoints = 0.1; fill(x1, y1, 'r'); hold on; - - - - - + done = 1; + if ~(IsIntersecting (L1, L2, pt1, pt2)) + xy_valid(end+1,:) = pt2; + validLinks(end+1,:) = [1 2]; + done = 0 + endif t = linspace(0, 2*pi, 100)'; r=L1+L2; @@ -47,8 +53,107 @@ function path = buildRRT(L1, L2, x1, y1, x2, y2) plot(circsx, circsy, 'b'); hold on; - fill(x3, y3, 'b'); + fill(x3, y3, 'r'); hold on; + axis equal; + + while(done == 1) + % samples randomly the joint space + q1 = rand()*360.0; + q2 = rand()*360.0; + + % creates the DH table + theta = [q1; q2]; + d = [0; 0]; + a = [L1; L2]; + alpha = [0; 0]; + + % computes the FK + wTee = dh2ForwardKinematics(theta, d, a, alpha, 1); + + % determines the position of the end-effector + position_ee = wTee(1:2,end); + %determine the closest point + min = 12345678901234567890; + closestPoint = []; + closestPointIdx = 0; + for i=1:size(xy_valid,1) + dist = (position_ee(1)-xy_valid(i,1))^2+ (position_ee(2)-xy_valid(i,2))^2; + if (dist < min) + min = dist; + closestPoint = xy_valid(i, :); + closestPointIdx = i; + endif + endfor + min = 12345678901234567890; + + %place the point at a given length + vectorForce = [position_ee(1)-closestPoint(1,1) position_ee(2)-closestPoint(1,2)]; + % Calculate the Euclidean norm (length) of the vector + vectorNorm = norm(vectorForce); + % Normalize the vector + vectorForce = vectorForce / vectorNorm; + newPoint = closestPoint+vectorForce*distanceBetweenPoints; + + plot(newPoint(1), newPoint(2), 'b'); + % checks if the end-effector is not hitting any obstacle + eeHittingObstacle = 0; + if (newPoint(2) >= L1) + eeHittingObstacle = 1; + end + if (newPoint(2) <= -L1) + eeHittingObstacle = 1; + end + if (newPoint(1) >= -L2 && newPoint(1) <= L2 && newPoint(2) >= -L2 && newPoint(2) <= L2) + eeHittingObstacle = 1; + end + + % If the there is something wrong don't do + if ~(IsIntersecting (L1, L2, closestPoint, newPoint) || eeHittingObstacle == 1) + validLinks(end+1,:) = [closestPointIdx length(xy_valid)+1]; + xy_valid(end+1,:) = newPoint; + q1q2_valid(end+1,:) = theta; + endif + %no more obstacles + if ~(IsIntersecting (L1, L2, newPoint, pt2) || eeHittingObstacle == 1) + done = 0 + xy_valid(end+1,:) = pt2; + validLinks(end+1,:) = [closestPointIdx length(xy_valid)]; + endif + + end + + visibilityGraph = zeros(length(xy_valid)); + + % Add edges to visibility graph based on valid links + for i = 1:length(xy_valid) + for j = i+1:length(xy_valid) + if ~IsIntersecting(L1, L2, xy_valid(i, :), xy_valid(j, :)) + % If the line segment between points i and j does not intersect with obstacles + visibilityGraph(i, j) = norm(xy_valid(i, :) - xy_valid(j, :)); + visibilityGraph(j, i) = visibilityGraph(i, j); % Assuming undirected graph + else + visibilityGraph(i, j) = NaN;% No links + visibilityGraph(j, i) = visibilityGraph(i, j); % Assuming undirected graph + end + end + end + + [distanceToNode, parentOfNode, nodeTrajectory] = dijkstra(length(xy_valid)-2, visibilityGraph); + nodeTrajectory = [1 nodeTrajectory]; + nodeTrajectory(end) = length(xy_valid) + for i=1:length(nodeTrajectory)-1 + x = [xy_valid(nodeTrajectory(i),1) xy_valid(nodeTrajectory(i+1),1)] + y = [xy_valid(nodeTrajectory(i),2) xy_valid(nodeTrajectory(i+1),2)] + plot(x, y) + endfor + + path = nodeTrajectory; + end + + + + diff --git a/test.m b/test.m index 5db6b49..ab1b41f 100644 --- a/test.m +++ b/test.m @@ -1,2 +1,5 @@ -path = buildPRM(2, 1) +path = buildRRT(2, 1, [-1.5 -1.5], [1.5 1.5]) +%test for intersections +intersect = IsIntersecting (2, 1, [-1.5 1.5], [1.5 1.5]) +intersect = IsIntersecting (2, 1, [-1.5 -1.5], [1.5 1.5])