├── Potential Fields
    ├── Code
    │   ├── GoalDelta.m
    │   ├── ObsDelta.m
    │   ├── PotentialFields.m
    │   ├── PotentialFields_Q2.m
    │   ├── README.md
    │   └── circles.m
    ├── Plots
    │   ├── 1obstacle.png
    │   ├── 2obstacle.png
    │   ├── README.md
    │   └── potentialFields.png
    └── README.md
├── README.md
├── RRT
    ├── Code
    │   ├── CheckIntersection.m
    │   ├── EuclDist.m
    │   ├── IfOnEdge.m
    │   ├── NearestNode.m
    │   ├── NewPointonLine.m
    │   ├── NodeToPoint.m
    │   ├── PointToNode.m
    │   ├── README.md
    │   ├── RRT.m
    │   ├── RandomPoint.m
    │   └── circles.m
    ├── Plots
    │   ├── README.md
    │   ├── RRT_Conf1.png
    │   ├── RRT_Conf2.png
    │   └── RRT_Conf3.png
    └── README.md
├── RRT_Star
    ├── Code
    │   ├── CheckIntersection.m
    │   ├── EuclDist.m
    │   ├── IfOnEdge.m
    │   ├── MinCostNode.m
    │   ├── NearestNode.m
    │   ├── NewPointonLine.m
    │   ├── NodeToPoint.m
    │   ├── PointToNode.m
    │   ├── README.md
    │   ├── RRTStar.m
    │   ├── RandomPoint.m
    │   └── circles.m
    ├── Plots
    │   ├── README.md
    │   ├── RRTstar_Conf1.png
    │   ├── RRTstar_Conf2.png
    │   └── RRTstar_Conf3.png
    └── README.md
├── Report_PDF.pdf
└── Visibility Graph
    ├── Code
        ├── EuclDist.m
        ├── README.md
        ├── circles.m
        └── visible_graph.m
    ├── Plots
        ├── README.md
        ├── VisGraph_Conf1.png
        ├── VisGraph_Conf2.png
        └── VisGraph_Conf3.png
    └── README.md


/Potential Fields/Code/GoalDelta.m:
--------------------------------------------------------------------------------
 1 | function [delXG, delYG] = GoalDelta(vx, vy, gx, gy, goalR, goalS, alpha)
 2 | % This function gives delX, delY of attraction caused by the goal point
 3 | 
 4 | dGoal = sqrt((gx-vx)^2 + (gy-vy)^2); % distance bw goal and current position
 5 | thetaG = atan2((gy-vy),(gx-vx));     % angle between goal and current position
 6 | % delXG = 0; delYG = 0;
 7 | 
 8 | if dGoal<goalR
 9 |     delXG = 0; delYG = 0;
10 | elseif ((goalS + goalR) >= dGoal) && (dGoal >= goalR)
11 |     delXG = alpha*(dGoal - goalR)*cos(thetaG);
12 |     delYG = alpha*(dGoal - goalR)*sin(thetaG);
13 | else
14 |     delXG = alpha*goalS*cos(thetaG);
15 |     delYG = alpha*goalS*sin(thetaG);
16 | end
17 | 
18 | end


--------------------------------------------------------------------------------
/Potential Fields/Code/ObsDelta.m:
--------------------------------------------------------------------------------
 1 | function [delXO, delYO] = ObsDelta(vx, vy, ox, oy, obsRad, obsS, beta)
 2 | % This function gives delX, delY of repulsion caused by the obstacle
 3 | 
 4 | inf = 10;
 5 | dObs = sqrt((ox-vx)^2 + (oy-vy)^2); % distance bw goal and current position
 6 | thetaO = atan2((oy-vy),(ox-vx));     % angle between goal and current position
 7 | % delXO = 0; delYO = 0;
 8 | 
 9 | if dObs<obsRad
10 |     delXO = -(sign(cos(thetaO)))*inf;
11 |     delYO = -(sign(sin(thetaO)))*inf;
12 | elseif (dObs < (obsS + obsRad)) && (dObs>=obsRad)
13 |     delXO = -beta*(obsS + obsRad - dObs)*cos(thetaO);
14 |     delYO = -beta*(obsS + obsRad - dObs)*sin(thetaO);
15 | else 
16 |     delXO = 0;
17 |     delYO = 0;
18 | end
19 | 
20 | end


--------------------------------------------------------------------------------
/Potential Fields/Code/PotentialFields.m:
--------------------------------------------------------------------------------
 1 | %%% Author: ADITYA JAIN %%%%
 2 | %%% Date: 20th January, 2018 %%%
 3 | %%% Topic: Potential Fields Robotics Assignment %%%
 4 | 
 5 | clc
 6 | close all
 7 | clear 
 8 | %% Defining environment variables
 9 | startPos = [5,5];
10 | goalPos = [90, 95];
11 | obs1Pos = [50, 50];
12 | obsRad = 10;
13 | goalR = 0.2; % The radius of the goal
14 | goalS = 20;  % The spread of attraction of the goal
15 | obsS = 30;   % The spread of repulsion of the obstacle
16 | alpha = 0.8; % Strength of attraction
17 | beta = 0.6;  % Strength of repulsion
18 | 
19 | 
20 | %% Doing the Potential Field Math here
21 | 
22 | u = zeros(100, 100);
23 | v = zeros(100, 100);
24 | testu = zeros(100, 100);
25 | testv = zeros(100, 100);
26 | 
27 | 
28 | for x = 1:1:100
29 |     for y = 1:1:100
30 |         [uG, vG] = GoalDelta(x, y, goalPos(1), goalPos(2), goalR, goalS, alpha);
31 |         [uO, vO] = ObsDelta(x, y, obs1Pos(2), obs1Pos(1), obsRad, obsS, beta);
32 |         xnet = uG + uO;
33 |         ynet = vG + vO;
34 |         vspeed = sqrt(xnet^2 + ynet^2);
35 |         theta = atan2(ynet,xnet);
36 |         u(x,y) = vspeed*cos(theta);
37 |         v(x,y) = vspeed*sin(theta);
38 | %         hold on
39 |         
40 |     end
41 | end
42 | %%
43 | [X,Y] = meshgrid(1:1:100,1:1:100);
44 | figure
45 | quiver(X, Y, u, v, 3)
46 | 
47 | 
48 | %% Defining the grid
49 | 
50 | % Plotting the obstacles
51 | circles(obs1Pos(1),obs1Pos(2),obsRad, 'facecolor','red')
52 | axis square
53 | 
54 | hold on % Plotting start position
55 | circles(startPos(1),startPos(2),2, 'facecolor','green')
56 | 
57 | hold on % Plotting goal position
58 | circles(goalPos(1),goalPos(2),2, 'facecolor','yellow')
59 | 
60 | %% Priting of the path
61 | currentPos = startPos;
62 | x = 0;
63 | 
64 | while sqrt((goalPos(1)-currentPos(1))^2 + (goalPos(2)-currentPos(2))^2) > 1
65 |     tempPos = currentPos + [u(currentPos(1),currentPos(2)), v(currentPos(1),currentPos(2))]
66 |     currentPos = round(tempPos)
67 |     hold on
68 |     plot(currentPos(1),currentPos(2),'-o', 'MarkerFaceColor', 'black')
69 |     pause(0.5)
70 | end
71 | 


--------------------------------------------------------------------------------
/Potential Fields/Code/PotentialFields_Q2.m:
--------------------------------------------------------------------------------
 1 | %%% Author: ADITYA JAIN %%%%
 2 | %%% Date: 20th January, 2018 %%%
 3 | %%% Topic: Potential Fields Robotics Assignment %%%
 4 | 
 5 | clc
 6 | close all
 7 | clear 
 8 | %% Defining environment variables
 9 | startPos = [5,15];
10 | goalPos = [90, 95];
11 | obs1Pos = [50, 50];
12 | obs2Pos = [30, 80];
13 | obsRad = 10;
14 | goalR = 0.2; % The radius of the goal
15 | goalS = 20;  % The spread of attraction of the goal
16 | obsS = 20;   % The spread of repulsion of the obstacle
17 | alpha = 0.8; % Strength of attraction
18 | beta = 0.9;  % Strength of repulsion
19 | 
20 | 
21 | %% Doing the Potential Field Math here
22 | 
23 | u = zeros(100, 100);
24 | v = zeros(100, 100);
25 | testu = zeros(100, 100);
26 | testv = zeros(100, 100);
27 | 
28 | 
29 | for x = 1:1:100
30 |     for y = 1:1:100
31 |         [uG, vG] = GoalDelta(x, y, goalPos(1), goalPos(2), goalR, goalS, alpha);
32 |         [uO, vO] = ObsDelta(x, y, obs1Pos(2), obs1Pos(1), obsRad, obsS, beta);
33 |         [uO2, vO2] = ObsDelta(x, y, obs2Pos(2), obs2Pos(1), obsRad, obsS, beta);
34 |         xnet = uG + uO + uO2;
35 |         ynet = vG + vO + vO2;
36 |         vspeed = sqrt(xnet^2 + ynet^2);
37 |         theta = atan2(ynet,xnet);
38 |         u(x,y) = vspeed*cos(theta);
39 |         v(x,y) = vspeed*sin(theta);
40 | %         hold on
41 |         
42 |     end
43 | end
44 | %%
45 | [X,Y] = meshgrid(1:1:100,1:1:100);
46 | figure
47 | quiver(X, Y, u, v, 3)
48 | 
49 | 
50 | %% Defining the grid
51 | 
52 | % Plotting the obstacles
53 | circles(obs1Pos(1),obs1Pos(2),obsRad, 'facecolor','red')
54 | axis square
55 | 
56 | hold on
57 | circles(obs2Pos(1),obs2Pos(2),obsRad, 'facecolor','red')
58 | 
59 | hold on % Plotting start position
60 | circles(startPos(1),startPos(2),2, 'facecolor','green')
61 | 
62 | hold on % Plotting goal position
63 | circles(goalPos(1),goalPos(2),2, 'facecolor','yellow')
64 | 
65 | %% Priting of the path
66 | currentPos = startPos;
67 | x = 0;
68 | 
69 | while sqrt((goalPos(1)-currentPos(1))^2 + (goalPos(2)-currentPos(2))^2) > 1
70 |     tempPos = currentPos + [u(currentPos(1),currentPos(2)), v(currentPos(1),currentPos(2))]
71 |     currentPos = round(tempPos)
72 |     hold on
73 |     plot(currentPos(1),currentPos(2),'o', 'MarkerFaceColor', 'black')
74 |     pause(0.5)
75 | end
76 | 


--------------------------------------------------------------------------------
/Potential Fields/Code/README.md:
--------------------------------------------------------------------------------
1 | # Running the code
2 | 
3 | 1. Navigate to the codes folder
4 | 2. Run the "PotentialFields.m" file for 1 obstacle environment and "PotentialFields_Q2.m" for 2 obstacle environment.
5 | 
6 | Note: other files are helper files for the above main scripts
7 | 


--------------------------------------------------------------------------------
/Potential Fields/Code/circles.m:
--------------------------------------------------------------------------------
  1 | function [ h ] = circles(x,y,r,varargin)
  2 | % h = circles(x,y,r,varargin) plots circles of radius r at points x and y. 
  3 | % x, y, and r can be scalars or N-D arrays.  
  4 | % 
  5 | % Chad Greene, March 2014. Updated August 2014. 
  6 | % University of Texas Institute for Geophysics. 
  7 | % 
  8 | %% Syntax 
  9 | %  circles(x,y,r)
 10 | %  circles(...,'points',numberOfPoints)
 11 | %  circles(...,'rotation',degreesRotation)
 12 | %  circles(...,'ColorProperty',ColorValue)
 13 | %  circles(...,'LineProperty',LineValue)
 14 | %  h = circles(...)
 15 | % 
 16 | %% Description
 17 | % 
 18 | % circles(x,y,r) plots circle(s) of radius or radii r centered at points given by 
 19 | % x and y.  Inputs x, y, and r may be any combination of scalar,
 20 | % vector, or 2D matrix, but dimensions of all nonscalar inputs must agree. 
 21 | % 
 22 | % circles(...,'points',numberOfPoints) allows specification of how many points to use 
 23 | % for the outline of each circle. Default value is 1000, but this may be
 24 | % increased to increase plotting resolution.  Or you may specify a small
 25 | % number (e.g. 4 to plot a square, 5 to plot a pentagon, etc.). 
 26 | % 
 27 | % circles(...,'rotation',degreesRotation) rotates the shape by a given
 28 | % degreesRotation, which can be a scalar or a matrix. This is useless for
 29 | % circles, but may be desired for polygons with a discernible number of corner points. 
 30 | % 
 31 | % circles(...,'ColorProperty',ColorValue) allows declaration of
 32 | % 'facecolor' or 'facealpha'
 33 | % as name-value pairs. Try declaring any fill property as name-value pairs. 
 34 | %
 35 | % circles(...,'LineProperty',LineValue) allows declaration of 'edgecolor', 
 36 | % 'linewidth', etc.
 37 | %
 38 | % h = circles(...) returns the handle(s) h of the plotted object(s). 
 39 | % 
 40 | % 
 41 | %% EXAMPLES: 
 42 | %
 43 | % Example 1: 
 44 | % circles(5,10,3)
 45 | % 
 46 | % % Example 2: 
 47 | % x = 2:7;
 48 | % y = [5,15,12,25,3,18]; 
 49 | % r = [3 4 5 5 7 3]; 
 50 | % figure
 51 | % circles(x,y,r)
 52 | % 
 53 | % % Example 3: 
 54 | % figure
 55 | % circles(1:10,5,2)
 56 | % 
 57 | % % Example 4: 
 58 | % figure
 59 | % circles(5,15,1:5,'facecolor','none')
 60 | % 
 61 | % % Example 5: 
 62 | % figure 
 63 | % circles(5,10,3,'facecolor','green')
 64 | % 
 65 | % % Example 6: 
 66 | % figure
 67 | % h = circles(5,10,3,'edgecolor',[.5 .2 .9])
 68 | % 
 69 | % % Example 7: 
 70 | % lat = repmat((10:-1:1)',1,10); 
 71 | % lon = repmat(1:10,10,1); 
 72 | % r = .4; 
 73 | % figure
 74 | % h1 = circles(lon,lat,r,'linewidth',4,'edgecolor','m','facecolor',[.6 .4 .8]);
 75 | % hold on;
 76 | % h2 = circles(1:.5:10,((1:.5:10).^2)/10,.12,'edgecolor','k','facecolor','none');
 77 | % axis equal 
 78 | % 
 79 | % % Example 8: Circles have corners
 80 | % This script approximates circles with 1000 points. If all those points
 81 | % are too complex for your Pentium-II, you can reduce the number of points
 82 | % used to make each circle.  If 1000 points is not high enough resolution,
 83 | % you can increase the number of points.  Or if you'd like to draw
 84 | % triangles or squares, or pentagons, you can significantly reduce the
 85 | % number of points. Let's try drawing a stop sign: 
 86 | % 
 87 | % figure
 88 | % h = circles(1,1,10,'points',8,'color','red'); 
 89 | % axis equal
 90 | % % and we see that our stop sign needs to be rotated a little bit, so we'll
 91 | % % delete the one we drew and try again: 
 92 | % delete(h)
 93 | % h = circles(1,1,10,'points',8,'color','red','rot',45/2); 
 94 | % text(1,1,'STOP','fontname','helvetica CY',...
 95 | %     'horizontalalignment','center','fontsize',140,...
 96 | %     'color','w','fontweight','bold')
 97 | % 
 98 | % figure
 99 | % circles([1 3 5],2,1,'points',4,'rot',[0 45 35])
100 | % 
101 | %
102 | % TIPS: 
103 | % 1. Include the name-value pair 'facecolor','none' to draw outlines
104 | % (non-filled) circles. 
105 | % 
106 | % 2. Follow the circles command with axis equal to fix distorted circles. 
107 | %
108 | % See also: fill, patch, and scatter. 
109 | 
110 | %% Check inputs: 
111 | 
112 | assert(isnumeric(x),'Input x must be numeric.') 
113 | assert(isnumeric(y),'Input y must be numeric.') 
114 | assert(isnumeric(r),'Input r must be numeric.') 
115 | 
116 | if ~isscalar(x) && ~isscalar(y)
117 |     assert(numel(x)==numel(y),'If neither x nor y is a scalar, their dimensions must match.')
118 | end
119 | if ~isscalar(x) && ~isscalar(r)
120 |     assert(numel(x)==numel(r),'If neither x nor r is a scalar, their dimensions must match.')
121 | end
122 | if ~isscalar(r) && ~isscalar(y)
123 |     assert(numel(r)==numel(y),'If neither y nor r is a scalar, their dimensions must match.')
124 | end
125 | 
126 | %% Parse inputs: 
127 | 
128 | % Define number of points per circle: 
129 | tmp = strcmpi(varargin,'points')|strcmpi(varargin,'NOP')|strcmpi(varargin,'corners')|...
130 |     strncmpi(varargin,'vert',4); 
131 | if any(tmp)
132 |     NOP = varargin{find(tmp)+1}; 
133 |     tmp(find(tmp)+1)=1; 
134 |     varargin = varargin(~tmp); 
135 | else
136 |     NOP = 1000; % 1000 points on periphery by default 
137 | end
138 | 
139 | % Define rotation
140 | tmp = strncmpi(varargin,'rot',3);
141 | if any(tmp)
142 |     rotation = varargin{find(tmp)+1}; 
143 |     assert(isnumeric(rotation)==1,'Rotation must be numeric.')
144 |     rotation = rotation*pi/180; % converts to radians
145 |     tmp(find(tmp)+1)=1; 
146 |     varargin = varargin(~tmp); 
147 | else
148 |     rotation = 0; % no rotation by default.
149 | end
150 | 
151 | % Be forgiving if the user enters "color" instead of "facecolor"
152 | tmp = strcmpi(varargin,'color');
153 | if any(tmp)
154 |     varargin{tmp} = 'facecolor'; 
155 | end
156 | 
157 | %% Begin operations:
158 | 
159 | % Make inputs column vectors: 
160 | x = x(:); 
161 | y = y(:);
162 | r = r(:); 
163 | rotation = rotation(:); 
164 | 
165 | % Determine how many circles to plot: 
166 | numcircles = max([length(x) length(y) length(r) length(rotation)]); 
167 | 
168 | % Create redundant arrays to make the plotting loop easy: 
169 | if length(x)<numcircles
170 |     x(1:numcircles) = x; 
171 | end
172 | 
173 | if length(y)<numcircles
174 |     y(1:numcircles) = y; 
175 | end
176 | 
177 | if length(r)<numcircles
178 |     r(1:numcircles) = r; 
179 | end
180 | 
181 | if length(rotation)<numcircles
182 |     rotation(1:numcircles) = rotation; 
183 | end
184 | 
185 | % Define an independent variable for drawing circle(s):
186 | t = 2*pi/NOP*(1:NOP); 
187 | 
188 | % Query original hold state:
189 | holdState = ishold; 
190 | hold on; 
191 | 
192 | % Preallocate object handle: 
193 | h = NaN(size(x)); 
194 | 
195 | % Plot circles singly: 
196 | for n = 1:numcircles
197 |     h(n) = fill(x(n)+r(n).*cos(t+rotation(n)), y(n)+r(n).*sin(t+rotation(n)),'',varargin{:});
198 | end
199 | 
200 | % Return to original hold state: 
201 | if ~holdState
202 |     hold off
203 | end
204 | 
205 | % Delete object handles if not requested by user: 
206 | if nargout==0 
207 |     clear h 
208 | end
209 | 
210 | end
211 | 
212 | 


--------------------------------------------------------------------------------
/Potential Fields/Plots/1obstacle.png:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/adityajain07/Path-Planning-Algorithms/48c3db87d330033e40407ad9021b10981584cef1/Potential Fields/Plots/1obstacle.png


--------------------------------------------------------------------------------
/Potential Fields/Plots/2obstacle.png:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/adityajain07/Path-Planning-Algorithms/48c3db87d330033e40407ad9021b10981584cef1/Potential Fields/Plots/2obstacle.png


--------------------------------------------------------------------------------
/Potential Fields/Plots/README.md:
--------------------------------------------------------------------------------
1 | # About
2 | This folder has all the plots in raw form.
3 | 


--------------------------------------------------------------------------------
/Potential Fields/Plots/potentialFields.png:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/adityajain07/Path-Planning-Algorithms/48c3db87d330033e40407ad9021b10981584cef1/Potential Fields/Plots/potentialFields.png


--------------------------------------------------------------------------------
/Potential Fields/README.md:
--------------------------------------------------------------------------------
 1 | 
 2 | # About
 3 | This folder has code for potential fields. The strength of the attraction (by goal) and repulsion (by obstacles) can be tweaked in the code. 
 4 | 
 5 | ## Environment
 6 | The below plot shows the net potential field in the environment due to a goal and an obstacle:<br />
 7 | Green circle: start position<br />
 8 | Red circle: obstacle(s)<br />
 9 | Yellow circle: goal position<br />
10 | ![alt text](https://github.com/adityajain07/Path-Planning-Algorithms/blob/master/Potential%20Fields/Plots/potentialFields.png)
11 | 
12 | 
13 | ## Path: 1-Obstacle Environment
14 | ![alt text](https://github.com/adityajain07/Path-Planning-Algorithms/blob/master/Potential%20Fields/Plots/1obstacle.png)
15 | 
16 | ## Path: 2-Obstacle Environment
17 | ![alt text](https://github.com/adityajain07/Path-Planning-Algorithms/blob/master/Potential%20Fields/Plots/2obstacle.png)
18 | 


--------------------------------------------------------------------------------
/README.md:
--------------------------------------------------------------------------------
1 | # Path-Planning-Algorithms
2 | 
3 | This repository contains MATLAB codes for popular path planning algorithms like potential fields, visibility graph, RRT and RRT*
4 | 


--------------------------------------------------------------------------------
/RRT/Code/CheckIntersection.m:
--------------------------------------------------------------------------------
 1 | % This script checks if two line segments intersect
 2 | % Used to check intersection between qnew and qnear with all obstacle edges
 3 | % returns 1 if no intersection
 4 | function t = CheckIntersection(qnear,qnewtemp,edges)
 5 | 
 6 |     ledgeSize = size(edges);
 7 |     noEdges = ledgeSize(1);
 8 |     
 9 |        
10 |                 
11 |         % find equation of line between qnear and qnewtemp
12 |         p1 = qnear;
13 |         q1 = qnewtemp;
14 |         m1 = (q1(2)-p1(2))/(q1(1)-p1(1));
15 |         c1 = p1(2) - m1*(p1(1));
16 |         %%%%%%%%%%%%
17 |     
18 |         t = 1;    % flag to check if the edge has any intersection with any other edge;
19 |                      % '1' means no intersection
20 |         % need to compare with the edges   
21 |         for k = 1:noEdges            
22 |             ed = edges(k,:);            
23 |             m2 = (ed(4) - ed(3))/(ed(2) - ed(1));
24 |             if(ed(2)==ed(1))
25 |                 m2 = 1e+10;
26 |             end
27 |             c2 = ed(3) - m2*ed(1);  
28 |             
29 |             if m1==m2 %% ignoring 
30 |                 t = 1;
31 |             else
32 |                 
33 |                 %%%%%%
34 |                 temp1 = ed(3) - m1*ed(1) - c1;
35 |                 temp2 = ed(4) - m1*ed(2) - c1;
36 |                 
37 |                 temp3 = p1(2) - m2*p1(1) - c2;
38 |                 temp4 = q1(2) - m2*q1(1) - c2;
39 |                 
40 |                 if (sign(temp1) ~= sign(temp2)) &&  sign(temp1)~=0 && sign(temp2)~=0 && (sign(temp3) ~= sign(temp4)) &&  sign(temp3)~=0 && sign(temp4)~=0
41 |                     t = 0;
42 |                     break
43 |                 end
44 |                 %%%%%%
45 |             end
46 | 
47 |         end     
48 | end


--------------------------------------------------------------------------------
/RRT/Code/EuclDist.m:
--------------------------------------------------------------------------------
1 | % This scripts returns the euclidean distance between two points
2 | function dis = EuclDist(p,q)
3 |     dis  = sqrt(sum((p - q) .^ 2));        
4 | end


--------------------------------------------------------------------------------
/RRT/Code/IfOnEdge.m:
--------------------------------------------------------------------------------
 1 | % This script checks if the new node to be added is not lying on any of
 2 | % the obstacle edges
 3 | function res = IfOnEdge(p,edges)
 4 | 
 5 |         ledgeSize = size(edges);
 6 |         noEdges = ledgeSize(1);
 7 |         
 8 |         for k = 1:noEdges            
 9 |             ed = edges(k,:);            
10 |             m = (ed(4) - ed(3))/(ed(2) - ed(1));
11 |             if(ed(2)==ed(1))
12 |                 m = 1e+10;
13 |             end
14 |             c = ed(3) - m*ed(1); 
15 |             
16 |             res = p(1,2) - m*p(1,1) - c;
17 |             
18 |             if (res == 0)
19 |                 break
20 |             end
21 |             
22 |         end
23 | 
24 | end


--------------------------------------------------------------------------------
/RRT/Code/NearestNode.m:
--------------------------------------------------------------------------------
 1 | % Given input a graph and a random point in the grid, this script returns
 2 | % the nearest node in the graph to that point
 3 | function n = NearestNode(G,q)
 4 |        sizeG = size(G.Nodes);       
 5 |        dmin = 1e+10;         % min distance
 6 |        
 7 |        for i=1:sizeG(1,1)    % length of number of nodes in the graph
 8 |         temp = G.Nodes{i,1};    
 9 |         point = NodeToPoint(temp);
10 | %         D  = sqrt(sum((q - point) .^ 2));
11 |         D = EuclDist(q,point);
12 |         
13 |         if (D<dmin)
14 |             dmin = D;
15 |             n = point;            
16 |         end
17 |         
18 |        end
19 | end


--------------------------------------------------------------------------------
/RRT/Code/NewPointonLine.m:
--------------------------------------------------------------------------------
1 | % This function returns the new point along the line joining qnearest and
2 | % qrandom, with step size away from qnear
3 | 
4 | function p = NewPointonLine(qnear,qrand,step)
5 |         v = qrand - qnear;
6 |         u = v/norm(v);
7 |         p = qnear + step*u;
8 | end


--------------------------------------------------------------------------------
/RRT/Code/NodeToPoint.m:
--------------------------------------------------------------------------------
 1 | % This function returns the graph node (which is in cell format) to a
 2 | % number array to be used for distance calculations and stuff
 3 | 
 4 | function p = NodeToPoint(n)    
 5 |     temp = char(n);
 6 |     temp2 = strsplit(temp,'\');
 7 |     tempx = temp2{1,1};
 8 |     tempy = temp2{1,2};
 9 |     p = [str2num(tempx), str2num(tempy)];
10 | end


--------------------------------------------------------------------------------
/RRT/Code/PointToNode.m:
--------------------------------------------------------------------------------
1 | % This function converts an (x,y) coordinate point in an array to a string which can be
2 | % then added as a node in the graph for RRT
3 | function n = PointToNode(p)    
4 |     temp1 = num2str(p(1,1));
5 |     temp2 = num2str(p(1,2));
6 |     n = strcat(temp1,'\',temp2);   % & is to differentiate between the x and y coordinate
7 | end


--------------------------------------------------------------------------------
/RRT/Code/README.md:
--------------------------------------------------------------------------------
1 | # Running the code
2 | 
3 | 1. Navigate to the code folder
4 | 2. Run "RRT.m" file
5 | 
6 | Note:
7 | - other files are helper files for the above main scripts
8 | - To switch between the three environment configurations, comment the others in RRT.m (well documented and commented code)
9 | 


--------------------------------------------------------------------------------
/RRT/Code/RRT.m:
--------------------------------------------------------------------------------
  1 | %%% Author: Aditya Jain %%%
  2 | %%% Topic: RRT for Static Obstacles
  3 | %%% Date: 10th February, 2018 %%%
  4 | clc
  5 | close all
  6 | clear
  7 | tic
  8 | 
  9 | %% Defining environment variables
 10 | start = [4,4];     % start position
 11 | goal = [90, 85];   % goal position
 12 | n = 2;             % no. of obstacles
 13 | 
 14 | %% Defining the grid
 15 | 
 16 | % %%%% 1st Config %%%%%%
 17 | figure
 18 | rectangle('Position',[20 10 40 30], 'FaceColor',[0 .5 .5])
 19 | axis([0 100 0 100])
 20 | axis square
 21 | title('RRT Path Planning Algorithm')
 22 | 
 23 | % hold on
 24 | rectangle('Position',[50 60 20 20], 'FaceColor',[0 .5 .5])
 25 | %%%%%%%%%%%%%%%%%%%%%
 26 | 
 27 | 
 28 | % 2nd Config
 29 | % figure
 30 | % rectangle('Position',[20 10 40 20], 'FaceColor',[0 .5 .5])
 31 | % axis([0 100 0 100])
 32 | % axis square
 33 | % title('RRT Path Planning Algorithm')
 34 | % 
 35 | % rectangle('Position',[70 10 20 40], 'FaceColor',[0 .5 .5])
 36 | % rectangle('Position',[10 40 40 20], 'FaceColor',[0 .5 .5])
 37 | % rectangle('Position',[20 70 60 20], 'FaceColor',[0 .5 .5])
 38 | %%%%%%%%%%%%
 39 | 
 40 | 
 41 | %%% 3rd Config
 42 | % rectangle('Position',[20 10 20 20], 'FaceColor',[0 .5 .5])
 43 | % axis([0 100 0 100])
 44 | % axis square
 45 | % 
 46 | % rectangle('Position',[70 10 20 20], 'FaceColor',[0 .5 .5])
 47 | % rectangle('Position',[10 40 20 20], 'FaceColor',[0 .5 .5])
 48 | % rectangle('Position',[20 70 20 20], 'FaceColor',[0 .5 .5])
 49 | % rectangle('Position',[50 5 15 20], 'FaceColor',[0 .5 .5])
 50 | % rectangle('Position',[40 40 35 20], 'FaceColor',[0 .5 .5])
 51 | % rectangle('Position',[10 70 5 20], 'FaceColor',[0 .5 .5])
 52 | % rectangle('Position',[50 65 20 25], 'FaceColor',[0 .5 .5])
 53 | % rectangle('Position',[80 35 15 40], 'FaceColor',[0 .5 .5])
 54 | %%%%%%%%%%%%%
 55 | 
 56 | % Plotting start position
 57 | circles(start(1), start(2),2, 'facecolor','green')
 58 | 
 59 | % Plotting goal position
 60 | circles(goal(1), goal(2),2, 'facecolor','yellow')
 61 | 
 62 | %% initialising the hash map
 63 | %%% 1st Config
 64 | keys = {'a', 'b', 'c', 'd', 'e', 'f', 'g', 'h', 'i', 'j'};
 65 | values = {start, [20,10], [20,40], [60,40], [60,10], [50,60], [50,80], [70,80], [70,60], goal};
 66 | %%%%%%%%%%%%%%%%%
 67 | 
 68 | %%% 2nd Config
 69 | % keys = {'a', 'b', 'c', 'd', 'e', 'f', 'g', 'h', 'i', 'j', 'k', 'l', 'm', 'n', 'o', 'p', 'q', 'r'};
 70 | % values = {start, [20,10], [20,30], [60,30], [60,10], [10,40], [10,60], [50,60], [50,40], [70,10], [70,50], [90,50], [90,10], [20,70], [20,90], [80,90], [80,70], goal};
 71 | % %%%%%%%%%%%%%%%%%
 72 | 
 73 | %%% 3rd Config
 74 | % keys = {'a', 'b', 'c', 'd', 'e', 'f', 'g', 'h', 'i', 'j', 'k', 'l', 'm', 'n', 'o', 'p', 'q', 'r', 's', 't', 'u', 'v', 'w', 'x', 'y', 'z', 'aa', 'ab', 'ac', 'ad', 'ae', 'af', 'ag', 'ah', 'ai', 'aj', 'ak', 'al'};
 75 | % values = {start, [20,10], [20,30], [40,30], [40,10], [50,5], [50,25], [65,25], [65,5], [70,10], [70,30], [90,30], [90,10], [10,40], [10,60], [30,60], [30,40], [40,40], [40,60], [75,60], [75,40], [80,35], [80,75], [95,75], [95,35], [10,70], [10,90], [15,90], [15,70], [20,70], [20,90], [40,90], [40,70], [50,65], [50,90], [70,90], [70,65], goal};
 76 | %%%%%%%%%%%%%%%%%
 77 | 
 78 | Map = containers.Map(keys, values);
 79 | len = Map.values;
 80 | Map('j');
 81 | 
 82 | 
 83 | 
 84 | %% Building all the obstacle edges
 85 | length = size(keys);    % this contains the number of nodes
 86 | 
 87 | edges = [];
 88 | 
 89 | for i = 2:(length(2)-2)   
 90 |         temp = [values{1,i}(1), values{1,i+1}(1), values{1,i}(2), values{1,i+1}(2)];
 91 |         edges = vertcat(edges, temp);                
 92 | end
 93 | 
 94 | 
 95 | % Removing edges which are not obstacle edges
 96 | sizeEdges = size(edges);
 97 | i = 4;
 98 | while i < (sizeEdges(1)-1)
 99 |     [edges,ps] = removerows(edges,'ind',i);
100 |     sizeEdges = size(edges);
101 |     i = i + 3;
102 | end
103 | 
104 | % Adding 1 edge pair in each obstacle which were not added in the earlier
105 | % for loop
106 | i = 2;
107 | while i < length(2)-2    
108 |     temp = [values{1,i}(1), values{1,i+3}(1), values{1,i}(2), values{1,i+3}(2)];
109 |     edges = vertcat(edges, temp);
110 |     i = i + 4;    
111 | end
112 | %% RRT Algorithm
113 | G = graph;                      % initialising the graph
114 | step = 2;                       % step size for the RRT algo
115 | qinit = start;                  % initialising the first node of the graph
116 | qnew = PointToNode(qinit);      % latest added node in the graph
117 | G = addnode(G, qnew);           % adding the node to the graph
118 | flag = 1;                       % flag to terminate RRT
119 | ranG = 7;                       % every ranGth point will be the goal position in the random function script
120 | i = 0;                          % iterator used to pick every ranGth point as goal position
121 | 
122 | while (flag==1)
123 |     i = i + 1;
124 |     
125 |     qrand = RandomPoint(ranG,i,goal);             % generating the random point on the grid
126 |     qnear = NearestNode(G,qrand);                 % nearest node to qrand in the form of array
127 |     qnewtemp = round(NewPointonLine(qnear,qrand,step));  % the new point along the line with a step distance
128 |     ch = IfOnEdge(qnewtemp,edges);
129 |     
130 |     if (ch ~= 0)  % If not lying on any of the edges, then proceed further
131 |      check = CheckIntersection(qnear,qnewtemp,edges);    % 1 means no intersection
132 |     
133 |     if (check==1)
134 |         
135 |         if (findnode(G,PointToNode(qnewtemp))) == 0   % to check if node already exists in the graph
136 |             G = addnode(G, PointToNode(qnewtemp));
137 |         end
138 |         
139 |         if (findedge(G,PointToNode(qnear),PointToNode(qnewtemp)))==0  % to check if edge exists in the graph
140 |             G = addedge(G,PointToNode(qnear), PointToNode(qnewtemp));            
141 |         end
142 |         
143 |         qnew = qnewtemp;
144 |         % plot the edge
145 |         xpoints = [qnear(1,1), qnew(1,1)];
146 |         ypoints = [qnear(1,2), qnew(1,2)];        
147 |         hold on
148 |         plot(xpoints, ypoints, 'b')
149 |         %%%
150 |         
151 |         % checking distance with the goal position
152 |         if (EuclDist(qnew,goal) <= step)
153 |             G = addedge(G,PointToNode(qnew), PointToNode(goal));
154 |             flag = 0;   % terminating the expansion of tree
155 |         end
156 |         
157 |     end
158 |     
159 |     end
160 |     
161 | %     break
162 | end
163 | 
164 | %% find the shortest path from the graph generated from RRT
165 | 
166 | path = shortestpath(G, PointToNode(start), PointToNode(goal));
167 | pathSize = size(path);
168 | 
169 | totalDis = 0;
170 | 
171 | for i=1:pathSize(2)-1
172 |     t1 = path{i};
173 |     t2 = path{i+1};
174 |     
175 |     p1 = NodeToPoint(t1);
176 |     p2 = NodeToPoint(t2);
177 |     totalDis = totalDis + EuclDist(p1,p2);
178 |     
179 |     xpoints = [p1(1,1), p2(1,1)];
180 |     ypoints = [p1(1,2), p2(1,2)];
181 |     hold on
182 |     plot(xpoints, ypoints, 'k', 'LineWidth', 3) 
183 |     title('RRT Path Planning Algorithm')
184 | end
185 | totalDis
186 | toc
187 | %%%%%%%%%%%%%% end %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
188 | 


--------------------------------------------------------------------------------
/RRT/Code/RandomPoint.m:
--------------------------------------------------------------------------------
 1 | % This function generates the random point in the grid with the nth random
 2 | % point to be always the goal position
 3 | 
 4 | function p = RandomPoint(g,i,goalPos)
 5 |         % g is the gth point generated will be the goal position
 6 |         % i is the iterator to check the above
 7 |         % goalPos has the goal position to be returned
 8 |         if (mod(i,g)==0)
 9 |             p = goalPos;
10 |         else
11 |             p = randi([1 100],1,2);
12 |         end
13 | 
14 | end


--------------------------------------------------------------------------------
/RRT/Code/circles.m:
--------------------------------------------------------------------------------
  1 | function [ h ] = circles(x,y,r,varargin)
  2 | % h = circles(x,y,r,varargin) plots circles of radius r at points x and y. 
  3 | % x, y, and r can be scalars or N-D arrays.  
  4 | % 
  5 | % Chad Greene, March 2014. Updated August 2014. 
  6 | % University of Texas Institute for Geophysics. 
  7 | % 
  8 | %% Syntax 
  9 | %  circles(x,y,r)
 10 | %  circles(...,'points',numberOfPoints)
 11 | %  circles(...,'rotation',degreesRotation)
 12 | %  circles(...,'ColorProperty',ColorValue)
 13 | %  circles(...,'LineProperty',LineValue)
 14 | %  h = circles(...)
 15 | % 
 16 | %% Description
 17 | % 
 18 | % circles(x,y,r) plots circle(s) of radius or radii r centered at points given by 
 19 | % x and y.  Inputs x, y, and r may be any combination of scalar,
 20 | % vector, or 2D matrix, but dimensions of all nonscalar inputs must agree. 
 21 | % 
 22 | % circles(...,'points',numberOfPoints) allows specification of how many points to use 
 23 | % for the outline of each circle. Default value is 1000, but this may be
 24 | % increased to increase plotting resolution.  Or you may specify a small
 25 | % number (e.g. 4 to plot a square, 5 to plot a pentagon, etc.). 
 26 | % 
 27 | % circles(...,'rotation',degreesRotation) rotates the shape by a given
 28 | % degreesRotation, which can be a scalar or a matrix. This is useless for
 29 | % circles, but may be desired for polygons with a discernible number of corner points. 
 30 | % 
 31 | % circles(...,'ColorProperty',ColorValue) allows declaration of
 32 | % 'facecolor' or 'facealpha'
 33 | % as name-value pairs. Try declaring any fill property as name-value pairs. 
 34 | %
 35 | % circles(...,'LineProperty',LineValue) allows declaration of 'edgecolor', 
 36 | % 'linewidth', etc.
 37 | %
 38 | % h = circles(...) returns the handle(s) h of the plotted object(s). 
 39 | % 
 40 | % 
 41 | %% EXAMPLES: 
 42 | %
 43 | % Example 1: 
 44 | % circles(5,10,3)
 45 | % 
 46 | % % Example 2: 
 47 | % x = 2:7;
 48 | % y = [5,15,12,25,3,18]; 
 49 | % r = [3 4 5 5 7 3]; 
 50 | % figure
 51 | % circles(x,y,r)
 52 | % 
 53 | % % Example 3: 
 54 | % figure
 55 | % circles(1:10,5,2)
 56 | % 
 57 | % % Example 4: 
 58 | % figure
 59 | % circles(5,15,1:5,'facecolor','none')
 60 | % 
 61 | % % Example 5: 
 62 | % figure 
 63 | % circles(5,10,3,'facecolor','green')
 64 | % 
 65 | % % Example 6: 
 66 | % figure
 67 | % h = circles(5,10,3,'edgecolor',[.5 .2 .9])
 68 | % 
 69 | % % Example 7: 
 70 | % lat = repmat((10:-1:1)',1,10); 
 71 | % lon = repmat(1:10,10,1); 
 72 | % r = .4; 
 73 | % figure
 74 | % h1 = circles(lon,lat,r,'linewidth',4,'edgecolor','m','facecolor',[.6 .4 .8]);
 75 | % hold on;
 76 | % h2 = circles(1:.5:10,((1:.5:10).^2)/10,.12,'edgecolor','k','facecolor','none');
 77 | % axis equal 
 78 | % 
 79 | % % Example 8: Circles have corners
 80 | % This script approximates circles with 1000 points. If all those points
 81 | % are too complex for your Pentium-II, you can reduce the number of points
 82 | % used to make each circle.  If 1000 points is not high enough resolution,
 83 | % you can increase the number of points.  Or if you'd like to draw
 84 | % triangles or squares, or pentagons, you can significantly reduce the
 85 | % number of points. Let's try drawing a stop sign: 
 86 | % 
 87 | % figure
 88 | % h = circles(1,1,10,'points',8,'color','red'); 
 89 | % axis equal
 90 | % % and we see that our stop sign needs to be rotated a little bit, so we'll
 91 | % % delete the one we drew and try again: 
 92 | % delete(h)
 93 | % h = circles(1,1,10,'points',8,'color','red','rot',45/2); 
 94 | % text(1,1,'STOP','fontname','helvetica CY',...
 95 | %     'horizontalalignment','center','fontsize',140,...
 96 | %     'color','w','fontweight','bold')
 97 | % 
 98 | % figure
 99 | % circles([1 3 5],2,1,'points',4,'rot',[0 45 35])
100 | % 
101 | %
102 | % TIPS: 
103 | % 1. Include the name-value pair 'facecolor','none' to draw outlines
104 | % (non-filled) circles. 
105 | % 
106 | % 2. Follow the circles command with axis equal to fix distorted circles. 
107 | %
108 | % See also: fill, patch, and scatter. 
109 | 
110 | %% Check inputs: 
111 | 
112 | assert(isnumeric(x),'Input x must be numeric.') 
113 | assert(isnumeric(y),'Input y must be numeric.') 
114 | assert(isnumeric(r),'Input r must be numeric.') 
115 | 
116 | if ~isscalar(x) && ~isscalar(y)
117 |     assert(numel(x)==numel(y),'If neither x nor y is a scalar, their dimensions must match.')
118 | end
119 | if ~isscalar(x) && ~isscalar(r)
120 |     assert(numel(x)==numel(r),'If neither x nor r is a scalar, their dimensions must match.')
121 | end
122 | if ~isscalar(r) && ~isscalar(y)
123 |     assert(numel(r)==numel(y),'If neither y nor r is a scalar, their dimensions must match.')
124 | end
125 | 
126 | %% Parse inputs: 
127 | 
128 | % Define number of points per circle: 
129 | tmp = strcmpi(varargin,'points')|strcmpi(varargin,'NOP')|strcmpi(varargin,'corners')|...
130 |     strncmpi(varargin,'vert',4); 
131 | if any(tmp)
132 |     NOP = varargin{find(tmp)+1}; 
133 |     tmp(find(tmp)+1)=1; 
134 |     varargin = varargin(~tmp); 
135 | else
136 |     NOP = 1000; % 1000 points on periphery by default 
137 | end
138 | 
139 | % Define rotation
140 | tmp = strncmpi(varargin,'rot',3);
141 | if any(tmp)
142 |     rotation = varargin{find(tmp)+1}; 
143 |     assert(isnumeric(rotation)==1,'Rotation must be numeric.')
144 |     rotation = rotation*pi/180; % converts to radians
145 |     tmp(find(tmp)+1)=1; 
146 |     varargin = varargin(~tmp); 
147 | else
148 |     rotation = 0; % no rotation by default.
149 | end
150 | 
151 | % Be forgiving if the user enters "color" instead of "facecolor"
152 | tmp = strcmpi(varargin,'color');
153 | if any(tmp)
154 |     varargin{tmp} = 'facecolor'; 
155 | end
156 | 
157 | %% Begin operations:
158 | 
159 | % Make inputs column vectors: 
160 | x = x(:); 
161 | y = y(:);
162 | r = r(:); 
163 | rotation = rotation(:); 
164 | 
165 | % Determine how many circles to plot: 
166 | numcircles = max([length(x) length(y) length(r) length(rotation)]); 
167 | 
168 | % Create redundant arrays to make the plotting loop easy: 
169 | if length(x)<numcircles
170 |     x(1:numcircles) = x; 
171 | end
172 | 
173 | if length(y)<numcircles
174 |     y(1:numcircles) = y; 
175 | end
176 | 
177 | if length(r)<numcircles
178 |     r(1:numcircles) = r; 
179 | end
180 | 
181 | if length(rotation)<numcircles
182 |     rotation(1:numcircles) = rotation; 
183 | end
184 | 
185 | % Define an independent variable for drawing circle(s):
186 | t = 2*pi/NOP*(1:NOP); 
187 | 
188 | % Query original hold state:
189 | holdState = ishold; 
190 | hold on; 
191 | 
192 | % Preallocate object handle: 
193 | h = NaN(size(x)); 
194 | 
195 | % Plot circles singly: 
196 | for n = 1:numcircles
197 |     h(n) = fill(x(n)+r(n).*cos(t+rotation(n)), y(n)+r(n).*sin(t+rotation(n)),'',varargin{:});
198 | end
199 | 
200 | % Return to original hold state: 
201 | if ~holdState
202 |     hold off
203 | end
204 | 
205 | % Delete object handles if not requested by user: 
206 | if nargout==0 
207 |     clear h 
208 | end
209 | 
210 | end
211 | 
212 | 


--------------------------------------------------------------------------------
/RRT/Plots/README.md:
--------------------------------------------------------------------------------
1 | # About
2 | This has all the plots in raw format.
3 | 


--------------------------------------------------------------------------------
/RRT/Plots/RRT_Conf1.png:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/adityajain07/Path-Planning-Algorithms/48c3db87d330033e40407ad9021b10981584cef1/RRT/Plots/RRT_Conf1.png


--------------------------------------------------------------------------------
/RRT/Plots/RRT_Conf2.png:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/adityajain07/Path-Planning-Algorithms/48c3db87d330033e40407ad9021b10981584cef1/RRT/Plots/RRT_Conf2.png


--------------------------------------------------------------------------------
/RRT/Plots/RRT_Conf3.png:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/adityajain07/Path-Planning-Algorithms/48c3db87d330033e40407ad9021b10981584cef1/RRT/Plots/RRT_Conf3.png


--------------------------------------------------------------------------------
/RRT/README.md:
--------------------------------------------------------------------------------
 1 | # About 
 2 | This project implements the algorithm of RRT.
 3 | 
 4 | ## Points to Note
 5 | 1. The obstacles have been deterministically placed in the environment
 6 | 2. Shortest path in the graph is calculated using BFS
 7 | 3. The code also outputs total path length in unit distance and time taken to execute the code
 8 | 
 9 | 
10 | ## Path: 1st Environment
11 | Green circle: start position  <br />
12 | Yellow circle: goal position  <br />
13 | ![alt text](https://github.com/adityajain07/Path-Planning-Algorithms/blob/master/RRT/Plots/RRT_Conf1.png)
14 | 
15 | 
16 | 
17 | 
18 | ## Path: 2nd Environment
19 | ![alt text](https://github.com/adityajain07/Path-Planning-Algorithms/blob/master/RRT/Plots/RRT_Conf2.png)
20 | 
21 | 
22 | 
23 | 
24 | ## Path: 3rd Environment
25 | ![alt text](https://github.com/adityajain07/Path-Planning-Algorithms/blob/master/RRT/Plots/RRT_Conf3.png)
26 | 


--------------------------------------------------------------------------------
/RRT_Star/Code/CheckIntersection.m:
--------------------------------------------------------------------------------
 1 | % This script checks if two line segments intersect
 2 | % Used to check intersection between qnew and qnear with all obstacle edges
 3 | % returns 1 if no intersection
 4 | function t = CheckIntersection(qnear,qnewtemp,edges)
 5 | 
 6 |     ledgeSize = size(edges);
 7 |     noEdges = ledgeSize(1);
 8 |     
 9 |        
10 |                 
11 |         % find equation of line between qnear and qnewtemp
12 |         p1 = qnear;
13 |         q1 = qnewtemp;
14 |         m1 = (q1(2)-p1(2))/(q1(1)-p1(1));
15 |         c1 = p1(2) - m1*(p1(1));
16 |         %%%%%%%%%%%%
17 |     
18 |         t = 1;    % flag to check if the edge has any intersection with any other edge;
19 |                      % '1' means no intersection
20 |         % need to compare with the edges   
21 |         for k = 1:noEdges            
22 |             ed = edges(k,:);            
23 |             m2 = (ed(4) - ed(3))/(ed(2) - ed(1));
24 |             if(ed(2)==ed(1))
25 |                 m2 = 1e+10;
26 |             end
27 |             c2 = ed(3) - m2*ed(1);  
28 |             
29 |             if m1==m2 %% ignoring 
30 |                 t = 1;
31 |             else
32 |                 
33 |                 %%%%%%
34 |                 temp1 = ed(3) - m1*ed(1) - c1;
35 |                 temp2 = ed(4) - m1*ed(2) - c1;
36 |                 
37 |                 temp3 = p1(2) - m2*p1(1) - c2;
38 |                 temp4 = q1(2) - m2*q1(1) - c2;
39 |                 
40 |                 if (sign(temp1) ~= sign(temp2)) &&  sign(temp1)~=0 && sign(temp2)~=0 && (sign(temp3) ~= sign(temp4)) &&  sign(temp3)~=0 && sign(temp4)~=0
41 |                     t = 0;
42 |                     break
43 |                 end
44 |                 %%%%%%
45 |             end
46 | 
47 |         end     
48 | end


--------------------------------------------------------------------------------
/RRT_Star/Code/EuclDist.m:
--------------------------------------------------------------------------------
1 | % This scripts returns the euclidean distance between two points
2 | function dis = EuclDist(p,q)
3 |     dis  = sqrt(sum((p - q) .^ 2));        
4 | end


--------------------------------------------------------------------------------
/RRT_Star/Code/IfOnEdge.m:
--------------------------------------------------------------------------------
 1 | % This script checks if the new node to be added is not lying on any of
 2 | % the obstacle edges
 3 | function res = IfOnEdge(p,edges)
 4 | 
 5 |         ledgeSize = size(edges);
 6 |         noEdges = ledgeSize(1);
 7 |         
 8 |         for k = 1:noEdges            
 9 |             ed = edges(k,:);            
10 |             m = (ed(4) - ed(3))/(ed(2) - ed(1));
11 |             if(ed(2)==ed(1))
12 |                 m = 1e+10;
13 |             end
14 |             c = ed(3) - m*ed(1); 
15 |             
16 |             res = p(1,2) - m*p(1,1) - c;
17 |             
18 |             if (res == 0)
19 |                 break
20 |             end
21 |             
22 |         end
23 | 
24 | end


--------------------------------------------------------------------------------
/RRT_Star/Code/MinCostNode.m:
--------------------------------------------------------------------------------
 1 | % This script returns the node to which we have to connect for minimum cost
 2 | % to the start node
 3 | 
 4 | function minNode = MinCostNode(G,q,vic,start,qnear)
 5 |        sizeG = size(G.Nodes);       
 6 |        minCost = 1e+10;         % min distance
 7 |        minNode = qnear;         % initialising
 8 |        for i=1:sizeG(1,1)    % length of number of nodes in the graph
 9 |         temp = G.Nodes{i,1};    
10 |         point = NodeToPoint(temp);
11 | 
12 |         if (EuclDist(q,point) <= vic)
13 |                path = shortestpath(G, PointToNode(start), temp);
14 |                pathSize = size(path);
15 | 
16 |                 totalDis = 0;
17 |                 for j=1:pathSize(2)-1
18 |                     t1 = path{j};
19 |                     t2 = path{j+1};
20 |     
21 |                     p1 = NodeToPoint(t1);
22 |                     p2 = NodeToPoint(t2);
23 |                     totalDis = totalDis + EuclDist(p1,p2);
24 |     
25 |                 end 
26 |             cost = totalDis + EuclDist(q,point);
27 |             
28 |             if (cost < minCost)
29 |                 minCost = cost;
30 |                 minNode = point;
31 |             end
32 |         end       
33 |         
34 |        end
35 | end


--------------------------------------------------------------------------------
/RRT_Star/Code/NearestNode.m:
--------------------------------------------------------------------------------
 1 | % Given input a graph and a random point in the grid, this script returns
 2 | % the nearest node in the graph to that point
 3 | function n = NearestNode(G,q)
 4 |        sizeG = size(G.Nodes);       
 5 |        dmin = 1e+10;         % min distance
 6 |        
 7 |        for i=1:sizeG(1,1)    % length of number of nodes in the graph
 8 |         temp = G.Nodes{i,1};    
 9 |         point = NodeToPoint(temp);
10 | %         D  = sqrt(sum((q - point) .^ 2));
11 |         D = EuclDist(q,point);
12 |         
13 |         if (D<dmin)
14 |             dmin = D;
15 |             n = point;            
16 |         end
17 |         
18 |        end
19 | end


--------------------------------------------------------------------------------
/RRT_Star/Code/NewPointonLine.m:
--------------------------------------------------------------------------------
1 | % This function returns the new point along the line joining qnearest and
2 | % qrandom, with step size away from qnear
3 | 
4 | function p = NewPointonLine(qnear,qrand,step)
5 |         v = qrand - qnear;
6 |         u = v/norm(v);
7 |         p = qnear + step*u;
8 | end


--------------------------------------------------------------------------------
/RRT_Star/Code/NodeToPoint.m:
--------------------------------------------------------------------------------
 1 | % This function returns the graph node (which is in cell format) to a
 2 | % number array to be used for distance calculations and stuff
 3 | 
 4 | function p = NodeToPoint(n)    
 5 |     temp = char(n);
 6 |     temp2 = strsplit(temp,'\');
 7 |     tempx = temp2{1,1};
 8 |     tempy = temp2{1,2};
 9 |     p = [str2num(tempx), str2num(tempy)];
10 | end


--------------------------------------------------------------------------------
/RRT_Star/Code/PointToNode.m:
--------------------------------------------------------------------------------
1 | % This function converts an (x,y) coordinate point in an array to a string which can be
2 | % then added as a node in the graph for RRT
3 | function n = PointToNode(p)    
4 |     temp1 = num2str(p(1,1));
5 |     temp2 = num2str(p(1,2));
6 |     n = strcat(temp1,'\',temp2);   % & is to differentiate between the x and y coordinate
7 | end


--------------------------------------------------------------------------------
/RRT_Star/Code/README.md:
--------------------------------------------------------------------------------
1 | # Running the code
2 | 
3 | 1. Navigate to the code folder
4 | 2. Run "RRTStar.m" file
5 | 
6 | Note:
7 | - other files are helper files for the above main scripts
8 | - To switch between the three environment configurations, comment the others in RRTStar.m (well documented and commented code)
9 | 


--------------------------------------------------------------------------------
/RRT_Star/Code/RRTStar.m:
--------------------------------------------------------------------------------
  1 | %%% Author: Aditya Jain %%%
  2 | %%% Topic: RRT* for Static Obstacles
  3 | %%% Date: 10th February, 2018 %%%
  4 | clc
  5 | close all
  6 | clear
  7 | tic
  8 | 
  9 | %% Defining environment variables
 10 | start = [4,4];     % start position
 11 | goal = [90, 85];   % goal position
 12 | n = 2;             % no. of obstacles
 13 | 
 14 | %% Defining the grid
 15 | 
 16 | % %%%% 1st Config %%%%%%
 17 | figure
 18 | rectangle('Position',[20 10 40 30], 'FaceColor',[0 .5 .5])
 19 | axis([0 100 0 100])
 20 | axis square
 21 | title('RRT Path Planning Algorithm')
 22 | 
 23 | % hold on
 24 | rectangle('Position',[50 60 20 20], 'FaceColor',[0 .5 .5])
 25 | %%%%%%%%%%%%%%%%%%%%%
 26 | 
 27 | 
 28 | % % 2nd Config
 29 | % figure
 30 | % rectangle('Position',[20 10 40 20], 'FaceColor',[0 .5 .5])
 31 | % axis([0 100 0 100])
 32 | % axis square
 33 | % 
 34 | % 
 35 | % rectangle('Position',[70 10 20 40], 'FaceColor',[0 .5 .5])
 36 | % rectangle('Position',[10 40 40 20], 'FaceColor',[0 .5 .5])
 37 | % rectangle('Position',[20 70 60 20], 'FaceColor',[0 .5 .5])
 38 | %%%%%%%%%%%%
 39 | 
 40 | 
 41 | %%% 3rd Config
 42 | % rectangle('Position',[20 10 20 20], 'FaceColor',[0 .5 .5])
 43 | % axis([0 100 0 100])
 44 | % axis square
 45 | % 
 46 | % rectangle('Position',[70 10 20 20], 'FaceColor',[0 .5 .5])
 47 | % rectangle('Position',[10 40 20 20], 'FaceColor',[0 .5 .5])
 48 | % rectangle('Position',[20 70 20 20], 'FaceColor',[0 .5 .5])
 49 | % rectangle('Position',[50 5 15 20], 'FaceColor',[0 .5 .5])
 50 | % rectangle('Position',[40 40 35 20], 'FaceColor',[0 .5 .5])
 51 | % rectangle('Position',[10 70 5 20], 'FaceColor',[0 .5 .5])
 52 | % rectangle('Position',[50 65 20 25], 'FaceColor',[0 .5 .5])
 53 | % rectangle('Position',[80 35 15 40], 'FaceColor',[0 .5 .5])
 54 | %%%%%%%%%%%%%
 55 | 
 56 | % Plotting start position
 57 | circles(start(1), start(2),2, 'facecolor','green')
 58 | 
 59 | % Plotting goal position
 60 | circles(goal(1), goal(2),2, 'facecolor','yellow')
 61 | %% initialising the hash map
 62 | %%% 1st Config
 63 | keys = {'a', 'b', 'c', 'd', 'e', 'f', 'g', 'h', 'i', 'j'};
 64 | values = {start, [20,10], [20,40], [60,40], [60,10], [50,60], [50,80], [70,80], [70,60], goal};
 65 | %%%%%%%%%%%%%%%%%
 66 | 
 67 | %%% 2nd Config
 68 | % keys = {'a', 'b', 'c', 'd', 'e', 'f', 'g', 'h', 'i', 'j', 'k', 'l', 'm', 'n', 'o', 'p', 'q', 'r'};
 69 | % values = {start, [20,10], [20,30], [60,30], [60,10], [10,40], [10,60], [50,60], [50,40], [70,10], [70,50], [90,50], [90,10], [20,70], [20,90], [80,90], [80,70], goal};
 70 | % %%%%%%%%%%%%%%%%%
 71 | 
 72 | %%% 3rd Config
 73 | % keys = {'a', 'b', 'c', 'd', 'e', 'f', 'g', 'h', 'i', 'j', 'k', 'l', 'm', 'n', 'o', 'p', 'q', 'r', 's', 't', 'u', 'v', 'w', 'x', 'y', 'z', 'aa', 'ab', 'ac', 'ad', 'ae', 'af', 'ag', 'ah', 'ai', 'aj', 'ak', 'al'};
 74 | % values = {start, [20,10], [20,30], [40,30], [40,10], [50,5], [50,25], [65,25], [65,5], [70,10], [70,30], [90,30], [90,10], [10,40], [10,60], [30,60], [30,40], [40,40], [40,60], [75,60], [75,40], [80,35], [80,75], [95,75], [95,35], [10,70], [10,90], [15,90], [15,70], [20,70], [20,90], [40,90], [40,70], [50,65], [50,90], [70,90], [70,65], goal};
 75 | %%%%%%%%%%%%%%%%%
 76 | 
 77 | Map = containers.Map(keys, values);
 78 | len = Map.values;
 79 | Map('j');
 80 | 
 81 | 
 82 | 
 83 | %% Building all the obstacle edges
 84 | length = size(keys);    % this contains the number of nodes
 85 | 
 86 | edges = [];
 87 | 
 88 | for i = 2:(length(2)-2)   
 89 |         temp = [values{1,i}(1), values{1,i+1}(1), values{1,i}(2), values{1,i+1}(2)];
 90 |         edges = vertcat(edges, temp);                
 91 | end
 92 | 
 93 | 
 94 | % Removing edges which are not obstacle edges
 95 | sizeEdges = size(edges);
 96 | i = 4;
 97 | while i < (sizeEdges(1)-1)
 98 |     [edges,ps] = removerows(edges,'ind',i);
 99 |     sizeEdges = size(edges);
100 |     i = i + 3;
101 | end
102 | 
103 | % Adding 1 edge pair in each obstacle which were not added in the earlier
104 | % for loop
105 | i = 2;
106 | while i < length(2)-2    
107 |     temp = [values{1,i}(1), values{1,i+3}(1), values{1,i}(2), values{1,i+3}(2)];
108 |     edges = vertcat(edges, temp);
109 |     i = i + 4;    
110 | end
111 | %% RRT Algorithm
112 | G = graph;                      % initialising the graph
113 | step = 2;                       % step size for the RRT algo
114 | qinit = start;                  % initialising the first node of the graph
115 | qnew = PointToNode(qinit);      % latest added node in the graph
116 | G = addnode(G, qnew);           % adding the node to the graph
117 | flag = 1;                       % flag to terminate RRT
118 | ranG = 7;                       % every ranGth point will be the goal position in the random function script
119 | i = 0;                          % iterator used to pick every ranGth point as goal position
120 | vic = 5;                        % this is the vicinity neighbourhood in units to be checked
121 | 
122 | while (flag==1)
123 |     i = i + 1;
124 |     
125 |     qrand = RandomPoint(ranG,i,goal);             % generating the random point on the grid
126 |     qnear = NearestNode(G,qrand);                 % nearest node to qrand in the form of array
127 |     qnewtemp = round(NewPointonLine(qnear,qrand,step));  % the new point along the line with a step distance
128 |     ch = IfOnEdge(qnewtemp,edges);   
129 |     
130 |     if (ch ~= 0)  % If not lying on any of the edges, then proceed further
131 |      qbestNeighb = MinCostNode(G,qnewtemp,vic,start, qnear);   % calculate the best node in the vicinity
132 |      check = CheckIntersection(qbestNeighb,qnewtemp,edges);    % 1 means no intersection
133 |     
134 |     if (check==1)
135 |         
136 |         if (findnode(G,PointToNode(qnewtemp))) == 0   % to check if node already exists in the graph
137 |             G = addnode(G, PointToNode(qnewtemp));
138 |         end
139 |         
140 |         if (findedge(G,PointToNode(qbestNeighb),PointToNode(qnewtemp)))==0  % to check if edge exists in the graph
141 |             G = addedge(G,PointToNode(qbestNeighb), PointToNode(qnewtemp));            
142 |         end
143 |         
144 |         qnew = qnewtemp;
145 |         % plot the edge
146 |         xpoints = [qbestNeighb(1,1), qnew(1,1)];
147 |         ypoints = [qbestNeighb(1,2), qnew(1,2)];        
148 |         hold on
149 |         plot(xpoints, ypoints, 'b')
150 |         %%%
151 |         
152 |         % checking distance with the goal position
153 |         if (EuclDist(qnew,goal) <= step)
154 |             G = addedge(G,PointToNode(qnew), PointToNode(goal));
155 |             flag = 0;   % terminating the expansion of tree
156 |         end
157 |         
158 |     end
159 |     
160 |     end
161 |     
162 | %     break
163 | end
164 | 
165 | %% find the shortest path from the graph generated from RRT
166 | 
167 | path = shortestpath(G, PointToNode(start), PointToNode(goal));
168 | pathSize = size(path);
169 | 
170 | totalDis = 0;
171 | 
172 | for i=1:pathSize(2)-1
173 |     t1 = path{i};
174 |     t2 = path{i+1};
175 |     
176 |     p1 = NodeToPoint(t1);
177 |     p2 = NodeToPoint(t2);
178 |     totalDis = totalDis + EuclDist(p1,p2);
179 |     
180 |     xpoints = [p1(1,1), p2(1,1)];
181 |     ypoints = [p1(1,2), p2(1,2)];
182 |     hold on
183 |     plot(xpoints, ypoints, 'k', 'LineWidth', 3) 
184 |     title('RRT* Path Planning Algorithm')
185 | end
186 | 
187 | toc
188 | totalDis
189 | %%%%%%%%%%%%%% end %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
190 | 


--------------------------------------------------------------------------------
/RRT_Star/Code/RandomPoint.m:
--------------------------------------------------------------------------------
 1 | % This function generates the random point in the grid with the nth random
 2 | % point to be always the goal position
 3 | 
 4 | function p = RandomPoint(g,i,goalPos)
 5 |         % g is the gth point generated will be the goal position
 6 |         % i is the iterator to check the above
 7 |         % goalPos has the goal position to be returned
 8 |         if (mod(i,g)==0)
 9 |             p = goalPos;
10 |         else
11 |             p = randi([1 100],1,2);
12 |         end
13 | 
14 | end


--------------------------------------------------------------------------------
/RRT_Star/Code/circles.m:
--------------------------------------------------------------------------------
  1 | function [ h ] = circles(x,y,r,varargin)
  2 | % h = circles(x,y,r,varargin) plots circles of radius r at points x and y. 
  3 | % x, y, and r can be scalars or N-D arrays.  
  4 | % 
  5 | % Chad Greene, March 2014. Updated August 2014. 
  6 | % University of Texas Institute for Geophysics. 
  7 | % 
  8 | %% Syntax 
  9 | %  circles(x,y,r)
 10 | %  circles(...,'points',numberOfPoints)
 11 | %  circles(...,'rotation',degreesRotation)
 12 | %  circles(...,'ColorProperty',ColorValue)
 13 | %  circles(...,'LineProperty',LineValue)
 14 | %  h = circles(...)
 15 | % 
 16 | %% Description
 17 | % 
 18 | % circles(x,y,r) plots circle(s) of radius or radii r centered at points given by 
 19 | % x and y.  Inputs x, y, and r may be any combination of scalar,
 20 | % vector, or 2D matrix, but dimensions of all nonscalar inputs must agree. 
 21 | % 
 22 | % circles(...,'points',numberOfPoints) allows specification of how many points to use 
 23 | % for the outline of each circle. Default value is 1000, but this may be
 24 | % increased to increase plotting resolution.  Or you may specify a small
 25 | % number (e.g. 4 to plot a square, 5 to plot a pentagon, etc.). 
 26 | % 
 27 | % circles(...,'rotation',degreesRotation) rotates the shape by a given
 28 | % degreesRotation, which can be a scalar or a matrix. This is useless for
 29 | % circles, but may be desired for polygons with a discernible number of corner points. 
 30 | % 
 31 | % circles(...,'ColorProperty',ColorValue) allows declaration of
 32 | % 'facecolor' or 'facealpha'
 33 | % as name-value pairs. Try declaring any fill property as name-value pairs. 
 34 | %
 35 | % circles(...,'LineProperty',LineValue) allows declaration of 'edgecolor', 
 36 | % 'linewidth', etc.
 37 | %
 38 | % h = circles(...) returns the handle(s) h of the plotted object(s). 
 39 | % 
 40 | % 
 41 | %% EXAMPLES: 
 42 | %
 43 | % Example 1: 
 44 | % circles(5,10,3)
 45 | % 
 46 | % % Example 2: 
 47 | % x = 2:7;
 48 | % y = [5,15,12,25,3,18]; 
 49 | % r = [3 4 5 5 7 3]; 
 50 | % figure
 51 | % circles(x,y,r)
 52 | % 
 53 | % % Example 3: 
 54 | % figure
 55 | % circles(1:10,5,2)
 56 | % 
 57 | % % Example 4: 
 58 | % figure
 59 | % circles(5,15,1:5,'facecolor','none')
 60 | % 
 61 | % % Example 5: 
 62 | % figure 
 63 | % circles(5,10,3,'facecolor','green')
 64 | % 
 65 | % % Example 6: 
 66 | % figure
 67 | % h = circles(5,10,3,'edgecolor',[.5 .2 .9])
 68 | % 
 69 | % % Example 7: 
 70 | % lat = repmat((10:-1:1)',1,10); 
 71 | % lon = repmat(1:10,10,1); 
 72 | % r = .4; 
 73 | % figure
 74 | % h1 = circles(lon,lat,r,'linewidth',4,'edgecolor','m','facecolor',[.6 .4 .8]);
 75 | % hold on;
 76 | % h2 = circles(1:.5:10,((1:.5:10).^2)/10,.12,'edgecolor','k','facecolor','none');
 77 | % axis equal 
 78 | % 
 79 | % % Example 8: Circles have corners
 80 | % This script approximates circles with 1000 points. If all those points
 81 | % are too complex for your Pentium-II, you can reduce the number of points
 82 | % used to make each circle.  If 1000 points is not high enough resolution,
 83 | % you can increase the number of points.  Or if you'd like to draw
 84 | % triangles or squares, or pentagons, you can significantly reduce the
 85 | % number of points. Let's try drawing a stop sign: 
 86 | % 
 87 | % figure
 88 | % h = circles(1,1,10,'points',8,'color','red'); 
 89 | % axis equal
 90 | % % and we see that our stop sign needs to be rotated a little bit, so we'll
 91 | % % delete the one we drew and try again: 
 92 | % delete(h)
 93 | % h = circles(1,1,10,'points',8,'color','red','rot',45/2); 
 94 | % text(1,1,'STOP','fontname','helvetica CY',...
 95 | %     'horizontalalignment','center','fontsize',140,...
 96 | %     'color','w','fontweight','bold')
 97 | % 
 98 | % figure
 99 | % circles([1 3 5],2,1,'points',4,'rot',[0 45 35])
100 | % 
101 | %
102 | % TIPS: 
103 | % 1. Include the name-value pair 'facecolor','none' to draw outlines
104 | % (non-filled) circles. 
105 | % 
106 | % 2. Follow the circles command with axis equal to fix distorted circles. 
107 | %
108 | % See also: fill, patch, and scatter. 
109 | 
110 | %% Check inputs: 
111 | 
112 | assert(isnumeric(x),'Input x must be numeric.') 
113 | assert(isnumeric(y),'Input y must be numeric.') 
114 | assert(isnumeric(r),'Input r must be numeric.') 
115 | 
116 | if ~isscalar(x) && ~isscalar(y)
117 |     assert(numel(x)==numel(y),'If neither x nor y is a scalar, their dimensions must match.')
118 | end
119 | if ~isscalar(x) && ~isscalar(r)
120 |     assert(numel(x)==numel(r),'If neither x nor r is a scalar, their dimensions must match.')
121 | end
122 | if ~isscalar(r) && ~isscalar(y)
123 |     assert(numel(r)==numel(y),'If neither y nor r is a scalar, their dimensions must match.')
124 | end
125 | 
126 | %% Parse inputs: 
127 | 
128 | % Define number of points per circle: 
129 | tmp = strcmpi(varargin,'points')|strcmpi(varargin,'NOP')|strcmpi(varargin,'corners')|...
130 |     strncmpi(varargin,'vert',4); 
131 | if any(tmp)
132 |     NOP = varargin{find(tmp)+1}; 
133 |     tmp(find(tmp)+1)=1; 
134 |     varargin = varargin(~tmp); 
135 | else
136 |     NOP = 1000; % 1000 points on periphery by default 
137 | end
138 | 
139 | % Define rotation
140 | tmp = strncmpi(varargin,'rot',3);
141 | if any(tmp)
142 |     rotation = varargin{find(tmp)+1}; 
143 |     assert(isnumeric(rotation)==1,'Rotation must be numeric.')
144 |     rotation = rotation*pi/180; % converts to radians
145 |     tmp(find(tmp)+1)=1; 
146 |     varargin = varargin(~tmp); 
147 | else
148 |     rotation = 0; % no rotation by default.
149 | end
150 | 
151 | % Be forgiving if the user enters "color" instead of "facecolor"
152 | tmp = strcmpi(varargin,'color');
153 | if any(tmp)
154 |     varargin{tmp} = 'facecolor'; 
155 | end
156 | 
157 | %% Begin operations:
158 | 
159 | % Make inputs column vectors: 
160 | x = x(:); 
161 | y = y(:);
162 | r = r(:); 
163 | rotation = rotation(:); 
164 | 
165 | % Determine how many circles to plot: 
166 | numcircles = max([length(x) length(y) length(r) length(rotation)]); 
167 | 
168 | % Create redundant arrays to make the plotting loop easy: 
169 | if length(x)<numcircles
170 |     x(1:numcircles) = x; 
171 | end
172 | 
173 | if length(y)<numcircles
174 |     y(1:numcircles) = y; 
175 | end
176 | 
177 | if length(r)<numcircles
178 |     r(1:numcircles) = r; 
179 | end
180 | 
181 | if length(rotation)<numcircles
182 |     rotation(1:numcircles) = rotation; 
183 | end
184 | 
185 | % Define an independent variable for drawing circle(s):
186 | t = 2*pi/NOP*(1:NOP); 
187 | 
188 | % Query original hold state:
189 | holdState = ishold; 
190 | hold on; 
191 | 
192 | % Preallocate object handle: 
193 | h = NaN(size(x)); 
194 | 
195 | % Plot circles singly: 
196 | for n = 1:numcircles
197 |     h(n) = fill(x(n)+r(n).*cos(t+rotation(n)), y(n)+r(n).*sin(t+rotation(n)),'',varargin{:});
198 | end
199 | 
200 | % Return to original hold state: 
201 | if ~holdState
202 |     hold off
203 | end
204 | 
205 | % Delete object handles if not requested by user: 
206 | if nargout==0 
207 |     clear h 
208 | end
209 | 
210 | end
211 | 
212 | 


--------------------------------------------------------------------------------
/RRT_Star/Plots/README.md:
--------------------------------------------------------------------------------
1 | # About
2 | This has all the plots in raw form.
3 | 


--------------------------------------------------------------------------------
/RRT_Star/Plots/RRTstar_Conf1.png:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/adityajain07/Path-Planning-Algorithms/48c3db87d330033e40407ad9021b10981584cef1/RRT_Star/Plots/RRTstar_Conf1.png


--------------------------------------------------------------------------------
/RRT_Star/Plots/RRTstar_Conf2.png:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/adityajain07/Path-Planning-Algorithms/48c3db87d330033e40407ad9021b10981584cef1/RRT_Star/Plots/RRTstar_Conf2.png


--------------------------------------------------------------------------------
/RRT_Star/Plots/RRTstar_Conf3.png:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/adityajain07/Path-Planning-Algorithms/48c3db87d330033e40407ad9021b10981584cef1/RRT_Star/Plots/RRTstar_Conf3.png


--------------------------------------------------------------------------------
/RRT_Star/README.md:
--------------------------------------------------------------------------------
 1 | # About 
 2 | This project implements the algorithm of RRT*.
 3 | 
 4 | ## Rewiring
 5 | Two types of rewiring happens in RRT*, in addition to the standard RRT: <br />
 6 | qrand: randomly chosen point in the environment  <br />
 7 | qnear: nearest node to qrand                      <br />
 8 | qnew: new node in the direction of qrand and step size 's' from qnear   <br />
 9 | 1. After qnew has been found, it is not connected to qnear unlike in RRT. All the nodes in neighbourhood (specified radius) of qnew is checked; the one which gives the shortest path from qnew to the start node is chosen. qnew is connected to this node.
10 | 2. After the above connection and again in the same neighbourhood of qnew, the path from start to all these neighbour nodes is calculated. If this path is more than the path from start to qnew and from qnew to the node, then the parent of this neighbour node is removed and qnew is made the new parent. <br />
11 | <br />
12 | The algorithm has been borrowed from this paper: https://arxiv.org/pdf/1704.04585.pdf <br />
13 | 
14 | Note: Only the first rewiring has been implemented till now.
15 | 
16 | ## Points to Note
17 | 1. The obstacles have been deterministically placed in the environment
18 | 2. Shortest path in the graph is calculated using BFS
19 | 3. The code also outputs total path length in unit distance and time taken to execute the code
20 | 
21 | 
22 | ## Path: 1st Environment
23 | Green circle: start position  <br />
24 | Yellow circle: goal position  <br />
25 | ![alt text](https://github.com/adityajain07/Path-Planning-Algorithms/blob/master/RRT_Star/Plots/RRTstar_Conf1.png)
26 | 
27 | 
28 | 
29 | 
30 | ## Path: 2nd Environment
31 | ![alt text](https://github.com/adityajain07/Path-Planning-Algorithms/blob/master/RRT_Star/Plots/RRTstar_Conf2.png)
32 | 
33 | 
34 | 
35 | 
36 | ## Path: 3rd Environment
37 | ![alt text](https://github.com/adityajain07/Path-Planning-Algorithms/blob/master/RRT_Star/Plots/RRTstar_Conf3.png)
38 | 


--------------------------------------------------------------------------------
/Report_PDF.pdf:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/adityajain07/Path-Planning-Algorithms/48c3db87d330033e40407ad9021b10981584cef1/Report_PDF.pdf


--------------------------------------------------------------------------------
/Visibility Graph/Code/EuclDist.m:
--------------------------------------------------------------------------------
1 | % This scripts returns the euclidean distance between two points
2 | function dis = EuclDist(p,q)
3 |     dis  = sqrt(sum((p - q) .^ 2));        
4 | end


--------------------------------------------------------------------------------
/Visibility Graph/Code/README.md:
--------------------------------------------------------------------------------
1 | # Running the code
2 | 
3 | 1. Navigate to the code folder
4 | 2. Run "visible_graph.m" file
5 | 
6 | Note:
7 | - other files are helper files for the above main scripts
8 | - To switch between the three environment configurations, comment the others in visible_graph.m (well documented and commented code)
9 | 


--------------------------------------------------------------------------------
/Visibility Graph/Code/circles.m:
--------------------------------------------------------------------------------
  1 | function [ h ] = circles(x,y,r,varargin)
  2 | % h = circles(x,y,r,varargin) plots circles of radius r at points x and y. 
  3 | % x, y, and r can be scalars or N-D arrays.  
  4 | % 
  5 | % Chad Greene, March 2014. Updated August 2014. 
  6 | % University of Texas Institute for Geophysics. 
  7 | % 
  8 | %% Syntax 
  9 | %  circles(x,y,r)
 10 | %  circles(...,'points',numberOfPoints)
 11 | %  circles(...,'rotation',degreesRotation)
 12 | %  circles(...,'ColorProperty',ColorValue)
 13 | %  circles(...,'LineProperty',LineValue)
 14 | %  h = circles(...)
 15 | % 
 16 | %% Description
 17 | % 
 18 | % circles(x,y,r) plots circle(s) of radius or radii r centered at points given by 
 19 | % x and y.  Inputs x, y, and r may be any combination of scalar,
 20 | % vector, or 2D matrix, but dimensions of all nonscalar inputs must agree. 
 21 | % 
 22 | % circles(...,'points',numberOfPoints) allows specification of how many points to use 
 23 | % for the outline of each circle. Default value is 1000, but this may be
 24 | % increased to increase plotting resolution.  Or you may specify a small
 25 | % number (e.g. 4 to plot a square, 5 to plot a pentagon, etc.). 
 26 | % 
 27 | % circles(...,'rotation',degreesRotation) rotates the shape by a given
 28 | % degreesRotation, which can be a scalar or a matrix. This is useless for
 29 | % circles, but may be desired for polygons with a discernible number of corner points. 
 30 | % 
 31 | % circles(...,'ColorProperty',ColorValue) allows declaration of
 32 | % 'facecolor' or 'facealpha'
 33 | % as name-value pairs. Try declaring any fill property as name-value pairs. 
 34 | %
 35 | % circles(...,'LineProperty',LineValue) allows declaration of 'edgecolor', 
 36 | % 'linewidth', etc.
 37 | %
 38 | % h = circles(...) returns the handle(s) h of the plotted object(s). 
 39 | % 
 40 | % 
 41 | %% EXAMPLES: 
 42 | %
 43 | % Example 1: 
 44 | % circles(5,10,3)
 45 | % 
 46 | % % Example 2: 
 47 | % x = 2:7;
 48 | % y = [5,15,12,25,3,18]; 
 49 | % r = [3 4 5 5 7 3]; 
 50 | % figure
 51 | % circles(x,y,r)
 52 | % 
 53 | % % Example 3: 
 54 | % figure
 55 | % circles(1:10,5,2)
 56 | % 
 57 | % % Example 4: 
 58 | % figure
 59 | % circles(5,15,1:5,'facecolor','none')
 60 | % 
 61 | % % Example 5: 
 62 | % figure 
 63 | % circles(5,10,3,'facecolor','green')
 64 | % 
 65 | % % Example 6: 
 66 | % figure
 67 | % h = circles(5,10,3,'edgecolor',[.5 .2 .9])
 68 | % 
 69 | % % Example 7: 
 70 | % lat = repmat((10:-1:1)',1,10); 
 71 | % lon = repmat(1:10,10,1); 
 72 | % r = .4; 
 73 | % figure
 74 | % h1 = circles(lon,lat,r,'linewidth',4,'edgecolor','m','facecolor',[.6 .4 .8]);
 75 | % hold on;
 76 | % h2 = circles(1:.5:10,((1:.5:10).^2)/10,.12,'edgecolor','k','facecolor','none');
 77 | % axis equal 
 78 | % 
 79 | % % Example 8: Circles have corners
 80 | % This script approximates circles with 1000 points. If all those points
 81 | % are too complex for your Pentium-II, you can reduce the number of points
 82 | % used to make each circle.  If 1000 points is not high enough resolution,
 83 | % you can increase the number of points.  Or if you'd like to draw
 84 | % triangles or squares, or pentagons, you can significantly reduce the
 85 | % number of points. Let's try drawing a stop sign: 
 86 | % 
 87 | % figure
 88 | % h = circles(1,1,10,'points',8,'color','red'); 
 89 | % axis equal
 90 | % % and we see that our stop sign needs to be rotated a little bit, so we'll
 91 | % % delete the one we drew and try again: 
 92 | % delete(h)
 93 | % h = circles(1,1,10,'points',8,'color','red','rot',45/2); 
 94 | % text(1,1,'STOP','fontname','helvetica CY',...
 95 | %     'horizontalalignment','center','fontsize',140,...
 96 | %     'color','w','fontweight','bold')
 97 | % 
 98 | % figure
 99 | % circles([1 3 5],2,1,'points',4,'rot',[0 45 35])
100 | % 
101 | %
102 | % TIPS: 
103 | % 1. Include the name-value pair 'facecolor','none' to draw outlines
104 | % (non-filled) circles. 
105 | % 
106 | % 2. Follow the circles command with axis equal to fix distorted circles. 
107 | %
108 | % See also: fill, patch, and scatter. 
109 | 
110 | %% Check inputs: 
111 | 
112 | assert(isnumeric(x),'Input x must be numeric.') 
113 | assert(isnumeric(y),'Input y must be numeric.') 
114 | assert(isnumeric(r),'Input r must be numeric.') 
115 | 
116 | if ~isscalar(x) && ~isscalar(y)
117 |     assert(numel(x)==numel(y),'If neither x nor y is a scalar, their dimensions must match.')
118 | end
119 | if ~isscalar(x) && ~isscalar(r)
120 |     assert(numel(x)==numel(r),'If neither x nor r is a scalar, their dimensions must match.')
121 | end
122 | if ~isscalar(r) && ~isscalar(y)
123 |     assert(numel(r)==numel(y),'If neither y nor r is a scalar, their dimensions must match.')
124 | end
125 | 
126 | %% Parse inputs: 
127 | 
128 | % Define number of points per circle: 
129 | tmp = strcmpi(varargin,'points')|strcmpi(varargin,'NOP')|strcmpi(varargin,'corners')|...
130 |     strncmpi(varargin,'vert',4); 
131 | if any(tmp)
132 |     NOP = varargin{find(tmp)+1}; 
133 |     tmp(find(tmp)+1)=1; 
134 |     varargin = varargin(~tmp); 
135 | else
136 |     NOP = 1000; % 1000 points on periphery by default 
137 | end
138 | 
139 | % Define rotation
140 | tmp = strncmpi(varargin,'rot',3);
141 | if any(tmp)
142 |     rotation = varargin{find(tmp)+1}; 
143 |     assert(isnumeric(rotation)==1,'Rotation must be numeric.')
144 |     rotation = rotation*pi/180; % converts to radians
145 |     tmp(find(tmp)+1)=1; 
146 |     varargin = varargin(~tmp); 
147 | else
148 |     rotation = 0; % no rotation by default.
149 | end
150 | 
151 | % Be forgiving if the user enters "color" instead of "facecolor"
152 | tmp = strcmpi(varargin,'color');
153 | if any(tmp)
154 |     varargin{tmp} = 'facecolor'; 
155 | end
156 | 
157 | %% Begin operations:
158 | 
159 | % Make inputs column vectors: 
160 | x = x(:); 
161 | y = y(:);
162 | r = r(:); 
163 | rotation = rotation(:); 
164 | 
165 | % Determine how many circles to plot: 
166 | numcircles = max([length(x) length(y) length(r) length(rotation)]); 
167 | 
168 | % Create redundant arrays to make the plotting loop easy: 
169 | if length(x)<numcircles
170 |     x(1:numcircles) = x; 
171 | end
172 | 
173 | if length(y)<numcircles
174 |     y(1:numcircles) = y; 
175 | end
176 | 
177 | if length(r)<numcircles
178 |     r(1:numcircles) = r; 
179 | end
180 | 
181 | if length(rotation)<numcircles
182 |     rotation(1:numcircles) = rotation; 
183 | end
184 | 
185 | % Define an independent variable for drawing circle(s):
186 | t = 2*pi/NOP*(1:NOP); 
187 | 
188 | % Query original hold state:
189 | holdState = ishold; 
190 | hold on; 
191 | 
192 | % Preallocate object handle: 
193 | h = NaN(size(x)); 
194 | 
195 | % Plot circles singly: 
196 | for n = 1:numcircles
197 |     h(n) = fill(x(n)+r(n).*cos(t+rotation(n)), y(n)+r(n).*sin(t+rotation(n)),'',varargin{:});
198 | end
199 | 
200 | % Return to original hold state: 
201 | if ~holdState
202 |     hold off
203 | end
204 | 
205 | % Delete object handles if not requested by user: 
206 | if nargout==0 
207 |     clear h 
208 | end
209 | 
210 | end
211 | 
212 | 


--------------------------------------------------------------------------------
/Visibility Graph/Code/visible_graph.m:
--------------------------------------------------------------------------------
  1 | %%% Author: Aditya Jain %%%
  2 | %%% Topic: Visibility Graph for Static Obstacles
  3 | %%% Date: 6th February, 2018 %%%
  4 | clc
  5 | close all
  6 | clear
  7 | tic
  8 | %% Defining environment variables
  9 | start = [4,4];     % start position
 10 | goal = [90, 85];   % goal position
 11 | n = 2;             % no. of obstacles
 12 | 
 13 | %% Defining the grid
 14 | figure
 15 | 
 16 | %%% 1st Config %%%%
 17 | % rectangle('Position',[20 10 40 30], 'FaceColor',[0 .5 .5])
 18 | % axis([0 100 0 100])
 19 | % axis square
 20 | % 
 21 | % hold on
 22 | % rectangle('Position',[50 60 20 20], 'FaceColor',[0 .5 .5])
 23 | %%%%%%%%%%%%%%%%%%%
 24 | 
 25 | 
 26 | %%% 2nd Config
 27 | % rectangle('Position',[20 10 40 20], 'FaceColor',[0 .5 .5])
 28 | % axis([0 100 0 100])
 29 | % axis square
 30 | % 
 31 | % rectangle('Position',[70 10 20 40], 'FaceColor',[0 .5 .5])
 32 | % rectangle('Position',[10 40 40 20], 'FaceColor',[0 .5 .5])
 33 | % rectangle('Position',[20 70 60 20], 'FaceColor',[0 .5 .5])
 34 | %%%%%%%%%%%%%
 35 | 
 36 | 
 37 | %%% 3rd Config
 38 | rectangle('Position',[20 10 20 20], 'FaceColor',[0 .5 .5])
 39 | axis([0 100 0 100])
 40 | axis square
 41 | 
 42 | rectangle('Position',[70 10 20 20], 'FaceColor',[0 .5 .5])
 43 | rectangle('Position',[10 40 20 20], 'FaceColor',[0 .5 .5])
 44 | rectangle('Position',[20 70 20 20], 'FaceColor',[0 .5 .5])
 45 | rectangle('Position',[50 5 15 20], 'FaceColor',[0 .5 .5])
 46 | rectangle('Position',[40 40 35 20], 'FaceColor',[0 .5 .5])
 47 | rectangle('Position',[10 70 5 20], 'FaceColor',[0 .5 .5])
 48 | rectangle('Position',[50 65 20 25], 'FaceColor',[0 .5 .5])
 49 | rectangle('Position',[80 35 15 40], 'FaceColor',[0 .5 .5])
 50 | %%%%%%%%%%%%%
 51 | 
 52 | 
 53 | 
 54 | % Plotting start position
 55 | circles(start(1), start(2),2, 'facecolor','green')
 56 | 
 57 | % Plotting goal position
 58 | circles(goal(1), goal(2),2, 'facecolor','yellow')
 59 | 
 60 | %% initialising the hash map
 61 | 
 62 | %%% 1st Config
 63 | % keys = {'a', 'b', 'c', 'd', 'e', 'f', 'g', 'h', 'i', 'j'};
 64 | % values = {start, [20,10], [20,40], [60,40], [60,10], [50,60], [50,80], [70,80], [70,60], goal};
 65 | %%%%%%%%%%%%%%%%%
 66 | 
 67 | %%% 2nd Config
 68 | % keys = {'a', 'b', 'c', 'd', 'e', 'f', 'g', 'h', 'i', 'j', 'k', 'l', 'm', 'n', 'o', 'p', 'q', 'r'};
 69 | % values = {start, [20,10], [20,30], [60,30], [60,10], [10,40], [10,60], [50,60], [50,40], [70,10], [70,50], [90,50], [90,10], [20,70], [20,90], [80,90], [80,70], goal};
 70 | %%%%%%%%%%%%%%%%%
 71 | 
 72 | %%% 3rd Config
 73 | keys = {'a', 'b', 'c', 'd', 'e', 'f', 'g', 'h', 'i', 'j', 'k', 'l', 'm', 'n', 'o', 'p', 'q', 'r', 's', 't', 'u', 'v', 'w', 'x', 'y', 'z', 'aa', 'ab', 'ac', 'ad', 'ae', 'af', 'ag', 'ah', 'ai', 'aj', 'ak', 'al'};
 74 | values = {start, [20,10], [20,30], [40,30], [40,10], [50,5], [50,25], [65,25], [65,5], [70,10], [70,30], [90,30], [90,10], [10,40], [10,60], [30,60], [30,40], [40,40], [40,60], [75,60], [75,40], [80,35], [80,75], [95,75], [95,35], [10,70], [10,90], [15,90], [15,70], [20,70], [20,90], [40,90], [40,70], [50,65], [50,90], [70,90], [70,65], goal};
 75 | %%%%%%%%%%%%%%%%%
 76 | 
 77 | Map = containers.Map(keys, values);
 78 | len = Map.values;
 79 | Map('j');
 80 | 
 81 | 
 82 | %% Building all the obstacle edges
 83 | length = size(keys);    % this contains the number of nodes
 84 | 
 85 | edges = [];
 86 | 
 87 | for i = 2:(length(2)-2)   
 88 |         temp = [values{1,i}(1), values{1,i+1}(1), values{1,i}(2), values{1,i+1}(2)];
 89 |         edges = vertcat(edges, temp);                
 90 | end
 91 | 
 92 | 
 93 | % Removing edges which are not obstacle edges
 94 | sizeEdges = size(edges);
 95 | i = 4;
 96 | while i < (sizeEdges(1)-1)
 97 |     [edges,ps] = removerows(edges,'ind',i);
 98 |     sizeEdges = size(edges);
 99 |     i = i + 3;
100 | end
101 | 
102 | % Adding 1 edge pair in each obstacle which were not added in the earlier
103 | % for loop
104 | i = 2;
105 | while i < length(2)-2    
106 |     temp = [values{1,i}(1), values{1,i+3}(1), values{1,i}(2), values{1,i+3}(2)];
107 |     edges = vertcat(edges, temp);
108 |     i = i + 4;    
109 | end
110 | 
111 | 
112 | %% Calculating the valid edges and adding them to the graph
113 | ledgeSize = size(edges);
114 | noEdges = ledgeSize(1);
115 | 
116 | G = graph();
117 | 
118 | for i = 1:length(2)    
119 |     for j = (i + 1):length(2)        
120 |         % find equation of the edge to be checked
121 |         p1 = values{1,i};
122 |         q1 = values{1,j};
123 |         m1 = (q1(2)-p1(2))/(q1(1)-p1(1));
124 |         c1 = p1(2) - m1*(p1(1));
125 |         %%%%%%%%%%%%
126 |     
127 |         flag = 1;    % flag to check if the edge has any intersection with any other edge;
128 |                      % '1' means no intersection
129 |         % need to compare with the edges   
130 |         for k = 1:noEdges
131 |             
132 |             ed = edges(k,:);            
133 |             m2 = (ed(4) - ed(3))/(ed(2) - ed(1));
134 |             if(ed(2)==ed(1))
135 |                 m2 = 1e+10;
136 |             end
137 |             c2 = ed(3) - m2*ed(1);  
138 |             
139 |             if m1==m2 %% ignoring 
140 |                 t = 1;
141 |             else
142 |                 
143 |                 %%%%%%
144 |                 temp1 = ed(3) - m1*ed(1) - c1;
145 |                 temp2 = ed(4) - m1*ed(2) - c1;
146 |                 
147 |                 temp3 = p1(2) - m2*p1(1) - c2;
148 |                 temp4 = q1(2) - m2*q1(1) - c2;
149 |                 
150 |                 if (sign(temp1) ~= sign(temp2)) &&  sign(temp1)~=0 && sign(temp2)~=0 && (sign(temp3) ~= sign(temp4)) &&  sign(temp3)~=0 && sign(temp4)~=0
151 |                     flag = 0;
152 |                     break
153 |                 end
154 |                 %%%%%%
155 |             end
156 | 
157 |         end
158 |         if flag==1
159 |             G = addedge(G,keys{i}, keys{j});
160 |         end
161 |                 
162 |     end    
163 | end
164 | 
165 | %% Removing the diagonals of the obstacle from the visible edges
166 | length = size(keys);    % this contains the number of nodes
167 | i = 2;
168 | 
169 | while i < (length(2)-2)
170 |     
171 |     G = rmedge(G,keys{i}, keys{i+2});
172 |     G = rmedge(G,keys{i+1}, keys{i+3});
173 |     i = i + 4;
174 | end
175 | 
176 | % G = rmedge(G,keys{15}, keys{32});
177 | % G = rmedge(G,keys{14}, keys{33});
178 | % G = rmedge(G,keys{15}, keys{4});
179 | % G = rmedge(G,keys{14}, keys{5});
180 | % G = rmedge(G,keys{10}, keys{25});
181 | % G = rmedge(G,keys{9}, keys{12});
182 | 
183 | %% Plotting all the visible edges
184 | 
185 | visEd = G.Edges;  % this has the visible edges
186 | sizeEd = size(G.Edges);
187 | 
188 | for i=1:sizeEd(1)
189 |    x = visEd(i,1);
190 |    xx = x{1,1};
191 |    p1 = Map(xx{1,1});
192 |    p2 = Map(xx{1,2});
193 |    xpoints = [p1(1,1), p2(1,1)];
194 |    ypoints = [p1(1,2), p2(1,2)];
195 |    hold on
196 |    plot(xpoints, ypoints, 'b')
197 | end
198 | 
199 | %% finding the shortest path in the graph and printing it 
200 | 
201 | path = shortestpath(G, keys{1}, keys{length(2)});
202 | pathSize = size(path);
203 | 
204 | totalDis = 0;    % this will contain the total distance in units for the final selected path
205 | 
206 | for i=1:pathSize(2)-1
207 |     p1 = Map(path{i});
208 |     p2 = Map(path{i+1});
209 |     
210 |     totalDis = totalDis + EuclDist(p1,p2);
211 |     
212 |     xpoints = [p1(1,1), p2(1,1)];
213 |     ypoints = [p1(1,2), p2(1,2)];
214 |     hold on
215 |     plot(xpoints, ypoints, 'k', 'LineWidth', 3)
216 |     title('Visibility Graph')
217 | end
218 | 
219 | toc
220 | totalDis
221 | %%%%%%%%%%%%%% end %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
222 | 


--------------------------------------------------------------------------------
/Visibility Graph/Plots/README.md:
--------------------------------------------------------------------------------
1 | # About
2 | This has all the plots in raw form.
3 | 


--------------------------------------------------------------------------------
/Visibility Graph/Plots/VisGraph_Conf1.png:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/adityajain07/Path-Planning-Algorithms/48c3db87d330033e40407ad9021b10981584cef1/Visibility Graph/Plots/VisGraph_Conf1.png


--------------------------------------------------------------------------------
/Visibility Graph/Plots/VisGraph_Conf2.png:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/adityajain07/Path-Planning-Algorithms/48c3db87d330033e40407ad9021b10981584cef1/Visibility Graph/Plots/VisGraph_Conf2.png


--------------------------------------------------------------------------------
/Visibility Graph/Plots/VisGraph_Conf3.png:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/adityajain07/Path-Planning-Algorithms/48c3db87d330033e40407ad9021b10981584cef1/Visibility Graph/Plots/VisGraph_Conf3.png


--------------------------------------------------------------------------------
/Visibility Graph/README.md:
--------------------------------------------------------------------------------
 1 | # About 
 2 | This project implements the algorithm of visibility graph.
 3 | 
 4 | ## Points to Note
 5 | 1. The obstacles have been deterministically placed in the environment
 6 | 2. Shortest path in the graph is calculated using BFS
 7 | 3. The code also outputs total path length in unit distance and time taken to execute the code
 8 | 
 9 | 
10 | ## Path: 1st Environment
11 | Green circle: start position
12 | Yellow circle: goal position
13 | ![alt text](https://github.com/adityajain07/Path-Planning-Algorithms/blob/master/Visibility%20Graph/Plots/VisGraph_Conf1.png)
14 | 
15 | 
16 | 
17 | 
18 | ## Path: 2nd Environment
19 | ![alt text](https://github.com/adityajain07/Path-Planning-Algorithms/blob/master/Visibility%20Graph/Plots/VisGraph_Conf2.png)
20 | 
21 | 
22 | 
23 | 
24 | ## Path: 3rd Environment
25 | ![alt text](https://github.com/adityajain07/Path-Planning-Algorithms/blob/master/Visibility%20Graph/Plots/VisGraph_Conf3.png)
26 | 


--------------------------------------------------------------------------------