40 lines
1.0 KiB
Matlab
40 lines
1.0 KiB
Matlab
function planPathRRT
|
|
addpath("C:/Users/Admin/Documents/ProjectMotionPlanning/motion_planning");
|
|
GapValue=5;
|
|
L1=2;
|
|
L2=1;
|
|
nodeTrajCut = [];
|
|
[nbNode, visGraph, points] = buildRRT();
|
|
for i=1:columns(visGraph)
|
|
for j=1:columns(visGraph)
|
|
if visGraph(i,j)==0
|
|
visGraph(i,j) = NaN;
|
|
endif
|
|
endfor
|
|
endfor
|
|
[distanceToNode, parentOfNode, nodeTrajectory] = dijkstra(nbNode, visGraph);
|
|
|
|
Q1plot = [];
|
|
Q2plot = [];
|
|
|
|
nodeTrajectory = [columns(points) nodeTrajectory]
|
|
|
|
for i=columns(nodeTrajectory):-1:1
|
|
i
|
|
if ~isnan(checkingLine(GapValue, L1, L2, 1, nodeTrajectory(i), points))
|
|
nodeTrajCut = [nodeTrajCut nodeTrajectory(i)];
|
|
endif
|
|
endfor
|
|
nodeTrajCut
|
|
nodeTrajectory = [16 max(nodeTrajCut) 1]
|
|
for i=1:columns(nodeTrajectory)
|
|
Q1plot = [Q1plot points(1, nodeTrajectory(i))];
|
|
Q2plot = [Q2plot points(2, nodeTrajectory(i))];
|
|
endfor
|
|
|
|
figure 1
|
|
axis([-180 180 -180 180]);
|
|
title('q1 q2 Joint Space');
|
|
hold on
|
|
plot(Q1plot, Q2plot, 'Color', 'g', 'LineWidth', 1.5)
|
|
endfunction |