drawing path
This commit is contained in:
parent
17ed7458bf
commit
7cfd0e5750
|
|
@ -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
|
||||
121
buildRRT.m
121
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
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
|
|
|||
Loading…
Reference in New Issue