estevan.biau-loyer
/
Tuto5_Motion_Planning
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

last update

This commit is contained in:
theyatokami 2023-12-04 00:06:38 +01:00
parent 17b962dbb3
commit 5cd994fe0a
3 changed files with 46 additions and 56 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
buildPRM.m
View File

@ -1,4 +1,4 @@
function [q1q2_valid, obstacles] = buildPRM(L1, L2, nbPoints) function [q1q2_valid, obstacles, connectionMap] = buildPRM(L1, L2, nbPoints)
%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%
%function buildPRM(L1, L2, nbPoints) %function buildPRM(L1, L2, nbPoints)
% ex. buildPRM(2000, 1000, 10) % ex. buildPRM(2000, 1000, 10)

8
dijkstra.m
View File

@ -59,13 +59,13 @@ fprintf('Minimal distance to target: %d\n', distanceToNode(nbNodes+2))
nodeIndex = nbNodes+2; nodeIndex = nbNodes+2;
nodeTrajectory = []; nodeTrajectory = [];
while (nodeIndex ~= 1) while (nodeIndex ~= 1)
if nodeIndex <= 0 || nodeIndex > length(parentOfNode)
error('Invalid node index encountered. Path reconstruction failed.');
end
nodeIndex = parentOfNode(nodeIndex); nodeIndex = parentOfNode(nodeIndex);
nodeTrajectory = [nodeTrajectory nodeIndex]; nodeTrajectory = [nodeTrajectory nodeIndex];
end end
fprintf('S-->');
for l_node=2:length(nodeTrajectory)
fprintf('N%d-->', nodeTrajectory(length(nodeTrajectory)-(l_node-1))-1);
end
fprintf('G\n'); fprintf('G\n');
fprintf('########\n'); fprintf('########\n');

78
planPathPRM.m
View File

@ -1,60 +1,50 @@
function planPathPRM(L1, L2, nbPoints) function path = planPathPRM(L1, L2, nbPoints)
% Build PRM % Run buildPRM to get the PRM
[q1q2_valid, obstacles] = buildPRM(L1, L2, nbPoints); [q1q2_valid, obstacles, connectionMap] = buildPRM(L1, L2, nbPoints);
% Define start and goal points % Convert q1q2_valid to Cartesian coordinates
start_point = [2000; 0]; q1q2_2d = []; % Initialize the array for 2D Cartesian coordinates
goal_point = [-2000; 0];
% Convert q1q2_valid into 2D Cartesian coordinates
points2D = [];
for i = 1:size(q1q2_valid, 2) for i = 1:size(q1q2_valid, 2)
pos = dh2ForwardKinematics([q1q2_valid(1, i); q1q2_valid(2, i)], [0; 0], [L1; L2], [0; 0], 1)(1:2, end); pos = dh2ForwardKinematics([q1q2_valid(1, i); q1q2_valid(2, i)], [0; 0], [L1; L2], [0; 0], 1)(1:2, end);
points2D = [points2D; pos']; q1q2_2d = [q1q2_2d, pos];
end end
% Create a connection matrix % Add start and goal points to the set of points
connectionMatrix = zeros(nbPoints, nbPoints); start_point = [2000, 0];
goal_point = [-2000, 0];
points2D = [start_point; q1q2_2d'; goal_point];
% Calculate distances and create connections % Find nearest neighbors for start and goal points
for i = 1:size(q1q2_valid, 2) [start_nearest_idx, start_nearest_dist] = findNearestNeighbor(start_point, q1q2_2d');
for j = 1:size(q1q2_valid, 2) [goal_nearest_idx, goal_nearest_dist] = findNearestNeighbor(goal_point, q1q2_2d');
if i ~= j
pos1 = dh2ForwardKinematics([q1q2_valid(1, i); q1q2_valid(2, i)], [0; 0], [L1; L2], [0; 0], 1)(1:2, end); % Update connectionMap for start and goal points
pos2 = dh2ForwardKinematics([q1q2_valid(1, j); q1q2_valid(2, j)], [0; 0], [L1; L2], [0; 0], 1)(1:2, end); connectionMap = [connectionMap; zeros(1, size(connectionMap, 2))];
if ~isLineIntersectingObstacle(pos1, pos2, obstacles) connectionMap = [connectionMap, zeros(size(connectionMap, 1), 1)];
connectionMatrix(i, j) = 1; connectionMap(1, start_nearest_idx+1) = 1; % +1 to account for start point being the first row
connectionMatrix(j, i) = 1; % Ensure bidirectional connection connectionMap(start_nearest_idx+1, 1) = 1;
end connectionMap(end, goal_nearest_idx+1) = 1; % end row is the goal point
end connectionMap(goal_nearest_idx+1, end) = 1;
end
end
% Create the visibility graph % Create the visibility graph
[nbNodes, visibilityGraph] = createVisibilityGraph(connectionMatrix, points2D); [nbNodes, visibilityGraph] = createVisibilityGraph(connectionMap, points2D);
% Find the nearest points in q1q2_valid to the start and goal points % Plan the path using Dijkstra's algorithm
start_index = findNearestPoint(start_point, points2D);
goal_index = findNearestPoint(goal_point, points2D);
% Find the shortest path using Dijkstra's algorithm
[distanceToNode, parentOfNode, nodeTrajectory] = dijkstra(nbNodes, visibilityGraph); [distanceToNode, parentOfNode, nodeTrajectory] = dijkstra(nbNodes, visibilityGraph);
if isempty(nodeTrajectory)
error('No valid path found');
end
% Extract the path in Cartesian coordinates
path = points2D(nodeTrajectory, :);
% Plot the path in Cartesian space % Plot the path in Cartesian space
subplot(1, 2, 2); % ... [plotting code here] ...
hold on;
for i = 1:length(nodeTrajectory) - 1 return;
pos1 = points2D(nodeTrajectory(i), :);
pos2 = points2D(nodeTrajectory(i + 1), :);
plot([pos1(1), pos2(1)], [pos1(2), pos2(2)], 'b', 'LineWidth', 2); % Plot path in blue
end end
% Plot the start and goal points function [nearest_idx, nearest_dist] = findNearestNeighbor(point, points)
plot(start_point(1), start_point(2), 'ro', 'MarkerSize', 10); % Red circle for start distances = sqrt(sum((points - point).^2, 2));
plot(goal_point(1), goal_point(2), 'go', 'MarkerSize', 10); % Green circle for goal [nearest_dist, nearest_idx] = min(distances);
title('Cartesian Space with Path');
legend('Obstacles', 'Path', 'Start', 'Goal');
hold off;
end end