corresction for points in CSPACE
This commit is contained in:
parent
8c65f17168
commit
5216d74996
66
buildRRT.m
66
buildRRT.m
|
|
@ -1,4 +1,4 @@
|
|||
function path = buildRRT(L1, L2, pt1, pt2)
|
||||
function [path, q1q2Path, xyPath] = buildRRT(L1, L2, pt1, pt2)
|
||||
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
||||
% function path = buildRRT(L1, L2, start, finish)
|
||||
% Task: Determine the 3D transformation matrix corresponding to a set of Denavit-Hartenberg parameters
|
||||
|
|
@ -6,13 +6,13 @@ function path = buildRRT(L1, L2, pt1, pt2)
|
|||
% Inputs:
|
||||
% - L1: first length
|
||||
% - L2: second length
|
||||
% - x1: start point x
|
||||
% - y1: first point y
|
||||
% - x2: end point x
|
||||
% - y2: end point y
|
||||
% - pt1: start xy
|
||||
% - pt2: end xy
|
||||
%
|
||||
% Output:
|
||||
% -path: Vector of points
|
||||
% - q1q2Path: List of all the points created in C-Space
|
||||
% - xyPath: List of all the points created in Cardinal space
|
||||
%
|
||||
% author: Marais Lucas
|
||||
% date: 22/11/2023
|
||||
|
|
@ -26,19 +26,37 @@ function path = buildRRT(L1, L2, pt1, pt2)
|
|||
x2 = [-L1-L2 -L1-L2 L1+L2 L1+L2];
|
||||
y2 = [-L1 L1 L1 -L1];
|
||||
xy_valid = []
|
||||
xy_valid(end+1,:) = pt1
|
||||
d = [0; 0];
|
||||
a = [L1; L2];
|
||||
alpha = [0; 0];
|
||||
|
||||
% computes the FK
|
||||
wTee = dh2ForwardKinematics(pt1', d, a, alpha, 1);
|
||||
|
||||
% determines the position of the end-effector
|
||||
position_ee = wTee(1:2,end);
|
||||
xy_valid(end+1,:) = position_ee;
|
||||
q1q2_valid = [];
|
||||
q1q2_valid(end+1,:) = pt1;
|
||||
validLinks = [];
|
||||
|
||||
distanceBetweenPoints = 0.1;
|
||||
distanceBetweenPoints = 3;
|
||||
|
||||
fill(x1, y1, 'r');
|
||||
|
||||
hold on;
|
||||
|
||||
done = 1;
|
||||
if ~(IsIntersecting (L1, L2, pt1, pt2))
|
||||
xy_valid(end+1,:) = pt2;
|
||||
|
||||
pt1Card = position_ee';
|
||||
% computes the FK
|
||||
wTee = dh2ForwardKinematics(pt2', d, a, alpha, 1);
|
||||
|
||||
% determines the position of the end-effector
|
||||
pt2Card = wTee(1:2,end)';
|
||||
if ~(IsIntersecting (L1, L2, pt1Card, pt2Card))
|
||||
xy_valid(end+1,:) = pt2Card;
|
||||
q1q2_valid(end+1,:) = pt2;
|
||||
validLinks(end+1,:) = [1 2];
|
||||
done = 0
|
||||
endif
|
||||
|
|
@ -55,7 +73,8 @@ function path = buildRRT(L1, L2, pt1, pt2)
|
|||
hold on;
|
||||
fill(x3, y3, 'r');
|
||||
hold on;
|
||||
|
||||
plot(pt1Card(1), pt1Card(2), 'g');
|
||||
plot(pt2Card(1), pt2Card(2), 'g');
|
||||
axis equal;
|
||||
|
||||
while(done == 1)
|
||||
|
|
@ -79,22 +98,30 @@ function path = buildRRT(L1, L2, pt1, pt2)
|
|||
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;
|
||||
dist = (theta(1)-q1q2_valid(i,1))^2+ (theta(2)-q1q2_valid(i,2))^2;
|
||||
if (dist < min)
|
||||
min = dist;
|
||||
closestPoint = xy_valid(i, :);
|
||||
closestPointC = q1q2_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)];
|
||||
vectorForce = [theta(1)-closestPointC(1,1) theta(2)-closestPointC(1,2)];
|
||||
% Calculate the Euclidean norm (length) of the vector
|
||||
vectorNorm = norm(vectorForce);
|
||||
% Normalize the vector
|
||||
vectorForce = vectorForce / vectorNorm;
|
||||
newPoint = closestPoint+vectorForce*distanceBetweenPoints;
|
||||
closestPointC
|
||||
newPointC = closestPointC+vectorForce*distanceBetweenPoints;
|
||||
|
||||
% computes the FK
|
||||
wTee = dh2ForwardKinematics(newPointC', d, a, alpha, 1);
|
||||
|
||||
% determines the position of the end-effector
|
||||
newPoint = wTee(1:2,end)';
|
||||
|
||||
plot(newPoint(1), newPoint(2), 'b');
|
||||
% checks if the end-effector is not hitting any obstacle
|
||||
|
|
@ -113,12 +140,13 @@ function path = buildRRT(L1, L2, pt1, pt2)
|
|||
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;
|
||||
q1q2_valid(end+1,:) = newPointC;
|
||||
endif
|
||||
%no more obstacles
|
||||
if ~(IsIntersecting (L1, L2, newPoint, pt2) || eeHittingObstacle == 1)
|
||||
if ~(IsIntersecting (L1, L2, newPoint, pt2Card) || eeHittingObstacle == 1)
|
||||
done = 0
|
||||
xy_valid(end+1,:) = pt2;
|
||||
q1q2_valid(end+1,:) = pt2;
|
||||
xy_valid(end+1,:) = pt2Card;
|
||||
validLinks(end+1,:) = [closestPointIdx length(xy_valid)];
|
||||
endif
|
||||
|
||||
|
|
@ -126,12 +154,12 @@ function path = buildRRT(L1, L2, pt1, pt2)
|
|||
|
||||
visibilityGraph = zeros(length(xy_valid));
|
||||
|
||||
% Add edges to visibility graph based on valid links
|
||||
% Add edges to visibility graph based on valid links // can be used with q1q2_valid or xy_valid
|
||||
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(i, j) = norm(q1q2_valid(i, :) - q1q2_valid(j, :));% here can be change
|
||||
visibilityGraph(j, i) = visibilityGraph(i, j); % Assuming undirected graph
|
||||
else
|
||||
visibilityGraph(i, j) = NaN;% No links
|
||||
|
|
@ -150,6 +178,8 @@ function path = buildRRT(L1, L2, pt1, pt2)
|
|||
endfor
|
||||
|
||||
path = nodeTrajectory;
|
||||
q1q2Path = q1q2_valid;
|
||||
xyPath = xy_valid;
|
||||
end
|
||||
|
||||
|
||||
|
|
|
|||
Loading…
Reference in New Issue