69 lines
2.1 KiB
Matlab
69 lines
2.1 KiB
Matlab
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
|