67 lines
1.7 KiB
Matlab
67 lines
1.7 KiB
Matlab
function planPathPRM()
|
|
[nbNode, visGraph, points] = buildPRM();
|
|
[distanceToNode, parentOfNode, nodeTrajectory] = dijkstra(nbNode, visGraph);
|
|
visGraph
|
|
addpath("C:/Users/Admin/Documents/ProjectMotionPlanning/motion_planning");
|
|
Q1plot = [];
|
|
Q2plot = [];
|
|
X_plot = [];
|
|
Y_plot = [];
|
|
GapValue = 1;
|
|
nodeTrajectory = [columns(points) nodeTrajectory];
|
|
|
|
for i=1:columns(nodeTrajectory)
|
|
Q1plot = [Q1plot points(1, nodeTrajectory(i))];
|
|
Q2plot = [Q2plot points(2, nodeTrajectory(i))];
|
|
|
|
if i>1
|
|
|
|
|
|
A = (Q2plot(i-1)- Q2plot(i))/(Q1plot(i-1) - Q1plot(i));
|
|
B = Q2plot(i) - A * Q1plot(i);
|
|
|
|
Q2 = @(Q1) A*Q1+B;
|
|
|
|
if Q1plot(i) > Q1plot(i-1)
|
|
gap = -GapValue;
|
|
else
|
|
gap = GapValue;
|
|
endif
|
|
for g=Q1plot(i):gap:Q1plot(i-1)
|
|
Q1test = g;
|
|
Q2test = Q2(g);
|
|
|
|
%Is the end effector colliding with obstacle
|
|
[Xtest, Ytest]=MyFK(2,1,Q1test,Q2test);
|
|
|
|
X_plot = [X_plot Xtest];
|
|
Y_plot = [Y_plot Ytest];
|
|
endfor
|
|
endif
|
|
|
|
endfor
|
|
|
|
figure 1
|
|
axis([-180 180 -180 180]);
|
|
title('q1 q2 Joint Space');
|
|
hold on
|
|
plot(Q1plot, Q2plot, 'Color', 'g', 'LineWidth', 1.5)
|
|
|
|
figure 2
|
|
title('X-Y Cartesian Space');
|
|
axis([-3 3 -3 3]);
|
|
hold all
|
|
text(X_plot, Y_plot, '*', 'FontSize', 10, 'Color', 'g');
|
|
|
|
for i=2:columns(nodeTrajectory)-1
|
|
text(points(3, nodeTrajectory(i)), points(4, nodeTrajectory(i)), int2str(nodeTrajectory(i)-1), 'FontSize', 20);
|
|
endfor
|
|
x = 3*cos(0:0.01*pi:2*pi);
|
|
y = 3*sin(0:0.01*pi:2*pi);
|
|
plot(x,y, 'Color', 'k');
|
|
|
|
x = cos(0:0.01*pi:2*pi);
|
|
y = sin(0:0.01*pi:2*pi);
|
|
plot(x,y, 'Color', 'k');
|
|
|
|
endfunction |