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)
% ex. buildPRM(2000, 1000, 10)

14
dijkstra.m
View File

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

80
planPathPRM.m
View File

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