├── .gitignore └── src └── path-planning ├── __init__.py ├── artificial-potential-field ├── Object.py ├── README.md ├── Robot.py ├── __init__.py ├── demo_1.gif ├── demo_2.gif ├── docs │ └── images_for_gif │ │ ├── 1.0.png │ │ ├── 10.0.png │ │ ├── 11.0.png │ │ ├── 12.0.png │ │ ├── 13.0.png │ │ ├── 14.0.png │ │ ├── 15.0.png │ │ ├── 16.0.png │ │ ├── 17.0.png │ │ ├── 18.0.png │ │ ├── 19.0.png │ │ ├── 2.0.png │ │ ├── 20.0.png │ │ ├── 21.0.png │ │ ├── 22.0.png │ │ ├── 23.0.png │ │ ├── 3.0.png │ │ ├── 4.0.png │ │ ├── 5.0.png │ │ ├── 6.0.png │ │ ├── 7.0.png │ │ ├── 8.0.png │ │ └── 9.0.png ├── main.py └── utils │ ├── __init__.py │ └── positional.py └── genetic-algorithm-path-planning ├── .gitignore ├── README.md ├── __init__.py ├── config ├── Config.py └── __init__.py ├── docs ├── gifs │ ├── GA_path_planning.gif │ └── GA_path_planning_2.gif └── images │ ├── 1.png │ ├── 2.png │ ├── 3.png │ ├── 4.png │ ├── 5.png │ ├── demo_gif_images │ ├── 1.png │ ├── 10.png │ ├── 11.png │ ├── 12.png │ ├── 13.png │ ├── 14.png │ ├── 15.png │ ├── 16.png │ ├── 17.png │ ├── 18.png │ ├── 19.png │ ├── 2.png │ ├── 20.png │ ├── 21.png │ ├── 22.png │ ├── 23.png │ ├── 24.png │ ├── 25.png │ ├── 3.png │ ├── 4.png │ ├── 5.png │ ├── 6.png │ ├── 7.png │ ├── 8.png │ └── 9.png │ └── flow_chart_GA.png ├── main.py └── tools ├── __init__.py ├── dna.py ├── draw_plot.py ├── fitness.py ├── population.py └── ranking.py /.gitignore: -------------------------------------------------------------------------------- 1 | .vscode/ 2 | **.pyc 3 | -------------------------------------------------------------------------------- /src/path-planning/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Yaaximus/robotics-python/7602e4c8cffe90aaf538b0bc87ada22a863da9a5/src/path-planning/__init__.py -------------------------------------------------------------------------------- /src/path-planning/artificial-potential-field/Object.py: -------------------------------------------------------------------------------- 1 | """ 2 | Object class 3 | 4 | Author: Yasim Ahmad(yaaximus) 5 | 6 | Email: yasim.ahmed63@yahoo.com 7 | """ 8 | import math as ma 9 | from utils.positional import Position 10 | 11 | 12 | class Object(object): 13 | """ 14 | This class encorporates the capability of defining Goal/Obstacle object. 15 | """ 16 | 17 | def __init__(self, Position, sigma=4.0): 18 | """ 19 | This is the constructor of Object class. 20 | 21 | Parameters 22 | ---------- 23 | position : [position] 24 | [x and y coordinates of object] 25 | sigma : float, optional 26 | [Width of Attractant/Repellant], by default 4.0 27 | """ 28 | 29 | self.position = Position 30 | self._sigma = sigma 31 | self._alpha = 1.0 / (self._sigma*ma.sqrt(2.0*ma.pi)) 32 | 33 | def get_alpha(self): 34 | """ 35 | This function is responsible for returning Depth of attractant/repellant 36 | 37 | Returns 38 | ------- 39 | [float] 40 | [Depth of attractant] 41 | """ 42 | 43 | return self._alpha 44 | 45 | def get_sigma(self): 46 | """ 47 | This function is responsible for returning Width of attractan 48 | 49 | Returns 50 | ------- 51 | [float] 52 | [Width of attractant] 53 | """ 54 | 55 | return self._sigma 56 | -------------------------------------------------------------------------------- /src/path-planning/artificial-potential-field/README.md: -------------------------------------------------------------------------------- 1 | # Python implementation of Artificial Potential Field 2 | 3 | ![demo_2](https://user-images.githubusercontent.com/37571161/57884778-ea902280-7842-11e9-809b-c175beb539f3.gif) 4 | 5 | - Check demo_1.gif to see the output with 4 obstacles b/w robot and goal 6 | - Check demo_2.gif to see the output with 6 obstacles b/w robot and goal 7 | 8 | ## Running instruction: 9 | - Run the main.py file from directory. 10 | - User can defined objects like Robot, Goal, Obstacles in this file. 11 | 12 | ## Requirements 13 | - python 14 | - numpy 15 | 16 | ## Features 17 | - Supports multiple obstacle objects 18 | - Single robot and goal object 19 | - image saving capability added 20 | - positional class encorporated to remove duplicate code in all objects 21 | - Public and private functions defined in classes 22 | 23 | ## Fixed issues: 24 | - bug fixed in take_next_move function of Robot class 25 | - vague name replaced with meaningful names 26 | 27 | ## Example for usage: 28 | #### You can define objects, robot, goal and obstacle's in main function like: 29 | - obstacle1 = Object(position(x=9.0, y=5.0), sigma=1.0) 30 | - obstacle2 = Object(position(x=9.0, y=8.0), sigma=1.0) 31 | - obstacle3 = Object(position(x=14.0, y=15.0), sigma=1.0) 32 | - obstacle4 = Object(position(x=14.0, y=18.0), sigma=1.0) 33 | 34 | - goal = Object(position(x=18.0, y=12.0), sigma=2.0) 35 | 36 | - robot = Robot(position(x=5.0, y=5.0), sensor_range=2.0, npts=60) 37 | 38 | ## List of Obstacles: 39 | - obstacles = [obstacle1, obstacle2, obstacle3, obstacle4] 40 | -------------------------------------------------------------------------------- /src/path-planning/artificial-potential-field/Robot.py: -------------------------------------------------------------------------------- 1 | """ 2 | Robot Class 3 | 4 | Author: Yasim Ahmad(yaaximus) 5 | 6 | Email: yasim.ahmed63@yahoo.com 7 | """ 8 | import math as ma 9 | import numpy as np 10 | from utils.positional import Position 11 | 12 | 13 | class Robot(object): 14 | """ 15 | This class encorporates the capability of defining Robot object. 16 | """ 17 | 18 | def __init__(self, Position, sensor_range=2.0, num_of_artif_pts=60): 19 | """ 20 | This is the constructor of Robot class 21 | 22 | Parameters 23 | ---------- 24 | Position : [Position] 25 | [Position of object], by default is 1.0 in both x and y axis 26 | sensor_range : float, optional 27 | [Distance from robot centre where artificial points will be defined], by default 2.0 28 | num_of_artif_pts : int, optional 29 | [Number of artificial points], by default 60 30 | """ 31 | 32 | self.position = Position 33 | self.__sensor_range = sensor_range 34 | self.__step_size = 0.4*self.__sensor_range 35 | self.__num_of_artif_pts = num_of_artif_pts 36 | self.__step_degree = 360.0/self.__num_of_artif_pts 37 | self.__artif_pts_x = np.zeros((1, self.__num_of_artif_pts)) 38 | self.__artif_pts_y = np.zeros((1, self.__num_of_artif_pts)) 39 | self.__theta = np.zeros((1, self.__num_of_artif_pts)) 40 | self.__update_theta() 41 | self.__artif_pts_cost_obst = np.zeros((1, self.__num_of_artif_pts)) 42 | self.__artif_pts_cost_goal = np.zeros((1, self.__num_of_artif_pts)) 43 | self.__artif_pts_cost = np.zeros((1, self.__num_of_artif_pts)) 44 | self.__dist_to_goal_artif_pts = np.zeros((1, self.__num_of_artif_pts)) 45 | self.__err_cost_func = np.zeros((1, self.__num_of_artif_pts)) 46 | self.__err_dist_to_goal = np.zeros((1, self.__num_of_artif_pts)) 47 | self.__fitness = np.zeros((1, self.__num_of_artif_pts)) 48 | 49 | def __update_theta(self): 50 | """ 51 | This function is used for computing angle theta's which are responsible for 52 | finding x and y position of artif points x = xprev + cos(theta), 53 | y = yprev + sin(theta). 54 | """ 55 | 56 | self.__theta[0][0] = self.__step_degree 57 | 58 | for num in range(1, self.__num_of_artif_pts): 59 | self.__theta[0][num] = self.__theta[0][num-1] + self.__step_degree 60 | 61 | def __cost_function_for_obstacles(self, obstacles, pt_x=None, pt_y=None): 62 | """ 63 | This function is responsible for calculating potential of current 64 | point w.r.t all obstacle. If pt_x and pt_y are not passed as a argument 65 | then x and y coordinate of robot will be used. 66 | 67 | Parameters 68 | ---------- 69 | obstacles : [list] 70 | [list of obstalce objects] 71 | pt_x : [float], optional 72 | [Position in x-axis], by default None 73 | pt_y : [float], optional 74 | [Position in y-axis], by default None 75 | 76 | Returns 77 | ------- 78 | [float] 79 | [Potential of current point w.r.t to obstacles] 80 | """ 81 | 82 | cost_obst = 0.0 83 | 84 | if pt_x is None: 85 | for obstacle in obstacles: 86 | cost_obst = cost_obst + obstacle.get_alpha()*ma.exp(-0.5 * 87 | ma.pow((self.position.calculate_distance(other=obstacle) / obstacle.get_sigma()), 2.0)) 88 | else: 89 | for obstacle in obstacles: 90 | cost_obst = cost_obst + obstacle.get_alpha()*ma.exp(-0.5 * 91 | ma.pow((self.position.calculate_distance(other=obstacle, x=pt_x, y=pt_y) / obstacle.get_sigma()), 2.0)) 92 | 93 | return cost_obst 94 | 95 | def __cost_function_for_goal(self, goal, pt_x=None, pt_y=None): 96 | """ 97 | This function is responsible for calculating potential of current 98 | point w.r.t goal. If pt_x and pt_y are not passed as a argument 99 | then x and y coordinate of robot will be used. 100 | 101 | Parameters 102 | ---------- 103 | goal : [Goal]] 104 | [Object of class Goal] 105 | pt_x : [float], optional 106 | [Position in x-axis], by default None 107 | pt_y : [float], optional 108 | [Position in y-axis], by default None 109 | 110 | Returns 111 | ------- 112 | [float] 113 | [Potential of current point w.r.t to goal] 114 | """ 115 | 116 | if pt_x is None: 117 | return -goal.get_alpha()*ma.exp(-0.5*ma.pow((self.position.calculate_distance(other=goal) / goal.get_sigma()), 2.0)) 118 | else: 119 | return -goal.get_alpha()*ma.exp(-0.5*ma.pow((self.position.calculate_distance(other=goal, x=pt_x, y=pt_y) / goal.get_sigma()), 2.0)) 120 | 121 | def cost_function(self, goal, obstacles, pt_x=None, pt_y=None): 122 | """ 123 | This function is responsible for calculating total potential of a point 124 | w.r.t goal and obstacles.If pt_x and pt_y are not passed as a argument 125 | then x and y coordinate of robot will be used. 126 | 127 | Parameters 128 | ---------- 129 | goal : [Goal] 130 | [Object of class Goal] 131 | obstacles : [list] 132 | [List of obstalce objects] 133 | pt_x : [float], optional 134 | [Position in x-axis], by default None 135 | pt_y : [float], optional 136 | [Position in y-axis], by default None 137 | 138 | Returns 139 | ------- 140 | [float] 141 | [total potential of a point w.r.t goal and obstacles] 142 | """ 143 | 144 | if pt_x is None: 145 | return self.__cost_function_for_goal(goal) + self.__cost_function_for_obstacles(obstacles) 146 | 147 | else: 148 | return self.__cost_function_for_goal(goal, pt_x=pt_x, pt_y=pt_y) + self.__cost_function_for_obstacles(obstacles, pt_x=pt_x, pt_y=pt_x) 149 | 150 | def __update_artif_pts_coords(self): 151 | """ 152 | This function is responsible for updating artif points of robot. 153 | """ 154 | 155 | for num in range(1, self.__num_of_artif_pts+1): 156 | self.__artif_pts_x[0][num-1] = self.position.x + \ 157 | (self.__step_size*ma.cos(ma.pi*self.__theta[0][num-1]/180.0)) 158 | self.__artif_pts_y[0][num-1] = self.position.y + \ 159 | (self.__step_size*ma.sin(ma.pi*self.__theta[0][num-1]/180.0)) 160 | 161 | def __update_artif_pts(self, goal, obstacles): 162 | """ 163 | This function is responsible for updating potential, distance to goal 164 | , error calculation, and fitness of artif points. 165 | 166 | Parameters 167 | ---------- 168 | goal : [Goal] 169 | [Object of class Goal] 170 | obstacles : [list] 171 | [List of obstalce objects] 172 | """ 173 | 174 | for num in range(0, self.__num_of_artif_pts): 175 | self.__artif_pts_cost_obst[0][num] = self.__cost_function_for_obstacles( 176 | obstacles=obstacles, pt_x=self.__artif_pts_x[0][num], pt_y=self.__artif_pts_y[0][num]) 177 | self.__artif_pts_cost_goal[0][num] = self.__cost_function_for_goal( 178 | goal=goal, pt_x=self.__artif_pts_x[0][num], pt_y=self.__artif_pts_y[0][num]) 179 | self.__artif_pts_cost[0][num] = self.__artif_pts_cost_obst[0][num] + \ 180 | self.__artif_pts_cost_goal[0][num] 181 | self.__dist_to_goal_artif_pts[0][num] = self.position.calculate_distance( 182 | other=goal, x=self.__artif_pts_x[0][num], y=self.__artif_pts_y[0][num]) 183 | 184 | self.__calculate_error(goal=goal, obstacles=obstacles) 185 | self.__calculate_fitness() 186 | 187 | def __calculate_error(self, goal, obstacles): 188 | """ 189 | This function is responsible for calculating error in potential and 190 | distance to goal of artif points w.r.t goal and obstacles. 191 | 192 | Parameters 193 | ---------- 194 | goal : [Goal] 195 | [Object of class Goal] 196 | obstacles : [list] 197 | [List of obstalce objects] 198 | """ 199 | 200 | JT = self.__cost_function_for_obstacles( 201 | obstacles) + self.__cost_function_for_goal(goal) 202 | DTG = self.position.calculate_distance(other=goal) 203 | 204 | for num in range(0, self.__num_of_artif_pts): 205 | self.__err_cost_func[0][num] = self.__artif_pts_cost[0][num] - JT 206 | self.__err_dist_to_goal[0][num] = self.__dist_to_goal_artif_pts[0][num] - DTG 207 | 208 | def __calculate_fitness(self): 209 | """ 210 | This function is responsible for calculating fitness based on error 211 | of artif points w.r.t goal and obstacles. 212 | """ 213 | 214 | for num in range(0, self.__num_of_artif_pts): 215 | self.__fitness[0][num] = - self.__err_dist_to_goal[0][num] 216 | 217 | def decide_next_move(self, goal, obstacles): 218 | """ 219 | This function is responsible for deciding next move of robot based on 220 | potential, and distance to goal of all artif potential points w.r.t 221 | goal and obstacles. 222 | 223 | Parameters 224 | ---------- 225 | goal : [type] 226 | [description] 227 | obstacles : [type] 228 | [description] 229 | """ 230 | 231 | self.__update_artif_pts_coords() 232 | 233 | self.__update_artif_pts(goal=goal, obstacles=obstacles) 234 | 235 | def take_next_move(self): 236 | """ 237 | This function is responsible for checking criteria for moving robot 238 | to next position based on a point for which the distance error is 239 | negative and the potential error is smallest and negative. 240 | """ 241 | 242 | check = 0 243 | for num in range(0, self.__num_of_artif_pts): 244 | k = np.argmax(self.__fitness[0]) 245 | if (self.__err_cost_func[0][k] < 0): 246 | check = check + 1 247 | self.position.x = self.__artif_pts_x[0][k] 248 | self.position.y = self.__artif_pts_y[0][k] 249 | else: 250 | self.__fitness[0][k] = 0.0 251 | 252 | if check == 0: 253 | print("No Solution Exists") 254 | else: 255 | print("Solution Exists") -------------------------------------------------------------------------------- /src/path-planning/artificial-potential-field/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Yaaximus/robotics-python/7602e4c8cffe90aaf538b0bc87ada22a863da9a5/src/path-planning/artificial-potential-field/__init__.py -------------------------------------------------------------------------------- /src/path-planning/artificial-potential-field/demo_1.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Yaaximus/robotics-python/7602e4c8cffe90aaf538b0bc87ada22a863da9a5/src/path-planning/artificial-potential-field/demo_1.gif -------------------------------------------------------------------------------- /src/path-planning/artificial-potential-field/demo_2.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Yaaximus/robotics-python/7602e4c8cffe90aaf538b0bc87ada22a863da9a5/src/path-planning/artificial-potential-field/demo_2.gif -------------------------------------------------------------------------------- /src/path-planning/artificial-potential-field/docs/images_for_gif/1.0.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Yaaximus/robotics-python/7602e4c8cffe90aaf538b0bc87ada22a863da9a5/src/path-planning/artificial-potential-field/docs/images_for_gif/1.0.png -------------------------------------------------------------------------------- /src/path-planning/artificial-potential-field/docs/images_for_gif/10.0.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Yaaximus/robotics-python/7602e4c8cffe90aaf538b0bc87ada22a863da9a5/src/path-planning/artificial-potential-field/docs/images_for_gif/10.0.png -------------------------------------------------------------------------------- /src/path-planning/artificial-potential-field/docs/images_for_gif/11.0.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Yaaximus/robotics-python/7602e4c8cffe90aaf538b0bc87ada22a863da9a5/src/path-planning/artificial-potential-field/docs/images_for_gif/11.0.png -------------------------------------------------------------------------------- /src/path-planning/artificial-potential-field/docs/images_for_gif/12.0.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Yaaximus/robotics-python/7602e4c8cffe90aaf538b0bc87ada22a863da9a5/src/path-planning/artificial-potential-field/docs/images_for_gif/12.0.png -------------------------------------------------------------------------------- /src/path-planning/artificial-potential-field/docs/images_for_gif/13.0.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Yaaximus/robotics-python/7602e4c8cffe90aaf538b0bc87ada22a863da9a5/src/path-planning/artificial-potential-field/docs/images_for_gif/13.0.png -------------------------------------------------------------------------------- /src/path-planning/artificial-potential-field/docs/images_for_gif/14.0.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Yaaximus/robotics-python/7602e4c8cffe90aaf538b0bc87ada22a863da9a5/src/path-planning/artificial-potential-field/docs/images_for_gif/14.0.png -------------------------------------------------------------------------------- /src/path-planning/artificial-potential-field/docs/images_for_gif/15.0.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Yaaximus/robotics-python/7602e4c8cffe90aaf538b0bc87ada22a863da9a5/src/path-planning/artificial-potential-field/docs/images_for_gif/15.0.png -------------------------------------------------------------------------------- /src/path-planning/artificial-potential-field/docs/images_for_gif/16.0.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Yaaximus/robotics-python/7602e4c8cffe90aaf538b0bc87ada22a863da9a5/src/path-planning/artificial-potential-field/docs/images_for_gif/16.0.png -------------------------------------------------------------------------------- /src/path-planning/artificial-potential-field/docs/images_for_gif/17.0.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Yaaximus/robotics-python/7602e4c8cffe90aaf538b0bc87ada22a863da9a5/src/path-planning/artificial-potential-field/docs/images_for_gif/17.0.png -------------------------------------------------------------------------------- /src/path-planning/artificial-potential-field/docs/images_for_gif/18.0.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Yaaximus/robotics-python/7602e4c8cffe90aaf538b0bc87ada22a863da9a5/src/path-planning/artificial-potential-field/docs/images_for_gif/18.0.png -------------------------------------------------------------------------------- /src/path-planning/artificial-potential-field/docs/images_for_gif/19.0.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Yaaximus/robotics-python/7602e4c8cffe90aaf538b0bc87ada22a863da9a5/src/path-planning/artificial-potential-field/docs/images_for_gif/19.0.png -------------------------------------------------------------------------------- /src/path-planning/artificial-potential-field/docs/images_for_gif/2.0.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Yaaximus/robotics-python/7602e4c8cffe90aaf538b0bc87ada22a863da9a5/src/path-planning/artificial-potential-field/docs/images_for_gif/2.0.png -------------------------------------------------------------------------------- /src/path-planning/artificial-potential-field/docs/images_for_gif/20.0.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Yaaximus/robotics-python/7602e4c8cffe90aaf538b0bc87ada22a863da9a5/src/path-planning/artificial-potential-field/docs/images_for_gif/20.0.png -------------------------------------------------------------------------------- /src/path-planning/artificial-potential-field/docs/images_for_gif/21.0.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Yaaximus/robotics-python/7602e4c8cffe90aaf538b0bc87ada22a863da9a5/src/path-planning/artificial-potential-field/docs/images_for_gif/21.0.png -------------------------------------------------------------------------------- /src/path-planning/artificial-potential-field/docs/images_for_gif/22.0.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Yaaximus/robotics-python/7602e4c8cffe90aaf538b0bc87ada22a863da9a5/src/path-planning/artificial-potential-field/docs/images_for_gif/22.0.png -------------------------------------------------------------------------------- /src/path-planning/artificial-potential-field/docs/images_for_gif/23.0.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Yaaximus/robotics-python/7602e4c8cffe90aaf538b0bc87ada22a863da9a5/src/path-planning/artificial-potential-field/docs/images_for_gif/23.0.png -------------------------------------------------------------------------------- /src/path-planning/artificial-potential-field/docs/images_for_gif/3.0.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Yaaximus/robotics-python/7602e4c8cffe90aaf538b0bc87ada22a863da9a5/src/path-planning/artificial-potential-field/docs/images_for_gif/3.0.png -------------------------------------------------------------------------------- /src/path-planning/artificial-potential-field/docs/images_for_gif/4.0.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Yaaximus/robotics-python/7602e4c8cffe90aaf538b0bc87ada22a863da9a5/src/path-planning/artificial-potential-field/docs/images_for_gif/4.0.png -------------------------------------------------------------------------------- /src/path-planning/artificial-potential-field/docs/images_for_gif/5.0.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Yaaximus/robotics-python/7602e4c8cffe90aaf538b0bc87ada22a863da9a5/src/path-planning/artificial-potential-field/docs/images_for_gif/5.0.png -------------------------------------------------------------------------------- /src/path-planning/artificial-potential-field/docs/images_for_gif/6.0.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Yaaximus/robotics-python/7602e4c8cffe90aaf538b0bc87ada22a863da9a5/src/path-planning/artificial-potential-field/docs/images_for_gif/6.0.png -------------------------------------------------------------------------------- /src/path-planning/artificial-potential-field/docs/images_for_gif/7.0.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Yaaximus/robotics-python/7602e4c8cffe90aaf538b0bc87ada22a863da9a5/src/path-planning/artificial-potential-field/docs/images_for_gif/7.0.png -------------------------------------------------------------------------------- /src/path-planning/artificial-potential-field/docs/images_for_gif/8.0.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Yaaximus/robotics-python/7602e4c8cffe90aaf538b0bc87ada22a863da9a5/src/path-planning/artificial-potential-field/docs/images_for_gif/8.0.png -------------------------------------------------------------------------------- /src/path-planning/artificial-potential-field/docs/images_for_gif/9.0.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Yaaximus/robotics-python/7602e4c8cffe90aaf538b0bc87ada22a863da9a5/src/path-planning/artificial-potential-field/docs/images_for_gif/9.0.png -------------------------------------------------------------------------------- /src/path-planning/artificial-potential-field/main.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | """ 4 | Main script 5 | 6 | Contains: main function 7 | 8 | Author: Yasim Ahmad(yaaximus) 9 | 10 | Email: yasim.ahmed63@yahoo.com 11 | """ 12 | 13 | from Robot import Robot 14 | from Object import Object 15 | import matplotlib.pyplot as plt 16 | from utils.positional import Position 17 | 18 | 19 | def main(): 20 | """ 21 | This is the function for instantiating object of Robot, Goal, and Obstacles. 22 | This programs runs until distance of robot is less then 0.9 to goal and 23 | during this process it keeps on deciding next move and then exectuing it. 24 | """ 25 | 26 | iteration_no = 1.0 27 | x_limit = 20.0 28 | y_limit = 20.0 29 | 30 | obstacle1 = Object(Position(x=9.0, y=5.0), sigma=1.0) 31 | obstacle2 = Object(Position(x=9.0, y=8.0), sigma=1.0) 32 | obstacle3 = Object(Position(x=14.0, y=15.0), sigma=1.0) 33 | obstacle4 = Object(Position(x=14.0, y=18.0), sigma=1.0) 34 | obstacles = [obstacle1, obstacle2, obstacle3, obstacle4] 35 | goal = Object(Position(x=18.0, y=12.0), sigma=2.0) 36 | robot = Robot(Position(x=5.0, y=5.0), 37 | sensor_range=2.0, num_of_artif_pts=60) 38 | 39 | plt.figure() 40 | plt.axis([0.0, x_limit, 0.0, y_limit]) 41 | 42 | while robot.position.calculate_distance(other=goal) > 0.9: 43 | 44 | plt.plot(robot.position.x, robot.position.y, 45 | "bo", markersize=x_limit/1.5) 46 | plt.plot(goal.position.x, goal.position.y, "go", 47 | markersize=x_limit*goal.get_sigma()) 48 | plt.plot(obstacle1.position.x, obstacle1.position.y, 49 | "ro", markersize=x_limit*obstacle1.get_sigma()) 50 | plt.plot(obstacle2.position.x, obstacle2.position.y, 51 | "ro", markersize=x_limit*obstacle2.get_sigma()) 52 | plt.plot(obstacle3.position.x, obstacle3.position.y, 53 | "ro", markersize=x_limit*obstacle3.get_sigma()) 54 | plt.plot(obstacle4.position.x, obstacle4.position.y, 55 | "ro", markersize=x_limit*obstacle4.get_sigma()) 56 | 57 | plt.legend(('Robot', 'Goal', 'Obstacle'), loc='lower right', 58 | fontsize='small', numpoints=1, markerscale=0.5, labelspacing=1) 59 | 60 | robot.decide_next_move(goal=goal, obstacles=obstacles) 61 | robot.take_next_move() 62 | plt.draw() 63 | plt.savefig("./docs/images_for_gif/"+str(iteration_no)+".png") 64 | iteration_no += 1.0 65 | plt.pause(0.02) 66 | 67 | 68 | if __name__ == "__main__": 69 | 70 | main() 71 | -------------------------------------------------------------------------------- /src/path-planning/artificial-potential-field/utils/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Yaaximus/robotics-python/7602e4c8cffe90aaf538b0bc87ada22a863da9a5/src/path-planning/artificial-potential-field/utils/__init__.py -------------------------------------------------------------------------------- /src/path-planning/artificial-potential-field/utils/positional.py: -------------------------------------------------------------------------------- 1 | """ 2 | Position Class 3 | 4 | Author: Yasim Ahmad(yaaximus) 5 | 6 | Email: yasim.ahmed63@yahoo.com 7 | """ 8 | import math as ma 9 | 10 | 11 | class Position: 12 | """ 13 | This class encapsulates the capability of defining position of object in 14 | coordinate system and calculating distance b/w two objects 15 | """ 16 | 17 | def __init__(self, x=1.0, y=1.0): 18 | """ 19 | This is the constructor of Position class 20 | 21 | Parameters 22 | ---------- 23 | x : float, optional 24 | [Position in x-axis of Object], by default 1.0 25 | y : float, optional 26 | [Position in y-axis of Object], by default 1.0 27 | """ 28 | 29 | self.x = x 30 | self.y = y 31 | 32 | def calculate_distance(self, other, x=None, y=None): 33 | """ 34 | This function is responsible for calculating distance b/w two 35 | points. If x and y are not passed as a argument 36 | then x and y coordinate of Position class object will be used. 37 | 38 | Parameters 39 | ---------- 40 | other : [Object] 41 | [object of class Object] 42 | x : [float], optional 43 | [Position in x-axis], by default None 44 | y : [float], optional 45 | [Position in y-axis], by default None 46 | 47 | Returns 48 | ------- 49 | [float] 50 | [distance b/w two points] 51 | """ 52 | 53 | if x is None: 54 | return ma.sqrt(ma.pow((self.x-other.position.x), 2)+ma.pow((self.y-other.position.y), 2)) 55 | 56 | else: 57 | return ma.sqrt(ma.pow((x-other.position.x), 2)+ma.pow((y-other.position.y), 2)) 58 | -------------------------------------------------------------------------------- /src/path-planning/genetic-algorithm-path-planning/.gitignore: -------------------------------------------------------------------------------- 1 | .vscode/ 2 | **.pyc 3 | -------------------------------------------------------------------------------- /src/path-planning/genetic-algorithm-path-planning/README.md: -------------------------------------------------------------------------------- 1 | # Python implementation of Genetic Algorithm in Path Planning 2 | 3 | ![GA_path_planning](https://user-images.githubusercontent.com/37571161/58662732-51820100-8344-11e9-97fb-a66e6e1cc877.gif) 4 | 5 | ## Running instruction: 6 | - Run the main.py file from directory. 7 | - User can defined path points, links b/w path points, population size, mutation rate 8 | in the Config.py file 9 | 10 | ## Requirements 11 | - python 12 | - numpy 13 | 14 | ## Features 15 | - user can define fixed links b/w nodes 16 | - user can initialize population of chromosomes which are initialized randomly 17 | but program insures that two consective nodes should be linked to each other. 18 | - user can calculate chromosome fitness based on total distance of chromosomes 19 | and connectivity of nodes. 20 | - user can find best fitness indices 21 | - user can rank the initial population of chromosomes using roulets wheels 22 | selection method 23 | - user can do crossover on the population 24 | - user can do mutation on the population 25 | 26 | ## Fixed issues: 27 | - bug fixed in create population function 28 | - bug related to chromosome population fitness best indices removed 29 | - Bug in generate mating pool function of ranking file fixed 30 | - Bug removed in function check fitness based on connection in fitness file 31 | 32 | ## Example for usage: 33 | #### You can initialize population in main function like: 34 | - chr_population = population() 35 | #### You can calculate fitness & find best fitness indices in main function like: 36 | - chr_pop_fitness, chr_best_fitness_index = fitness(chr_pop=chr_population) 37 | #### You can obtain ranked population in main function like: 38 | - chr_ranked_population = ranking(chr_pop_fitness=chr_pop_fitness, pop=chr_population) 39 | #### You can do crossover & mutation in main function like: 40 | - chr_crossover_mutated_population = dna(chr_pop_fitness=chr_pop_fitness, 41 | ranked_population=chr_ranked_population, chr_best_fitness_index=chr_best_fitness_index, 42 | last_pop=chr_population) 43 | 44 | ## Objective 45 | - Use Genetic Algorithm for finding a best path for mobile robot in a 2D environment. 46 | - To move from starting point to the endpoint while avoiding collisions with 47 | obstacles and minimizing total distance travelled. 48 | 49 | ## Flow Chart of Genetic Algorithm 50 | 51 | ![flow_chart_GA](https://user-images.githubusercontent.com/37571161/58673241-f829ca00-8363-11e9-8643-9f508ce0f94c.png) 52 | -------------------------------------------------------------------------------- /src/path-planning/genetic-algorithm-path-planning/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Yaaximus/robotics-python/7602e4c8cffe90aaf538b0bc87ada22a863da9a5/src/path-planning/genetic-algorithm-path-planning/__init__.py -------------------------------------------------------------------------------- /src/path-planning/genetic-algorithm-path-planning/config/Config.py: -------------------------------------------------------------------------------- 1 | import math as ma 2 | import numpy as np 3 | 4 | path_points = [[1, 7], [1, 11], [3, 14], [3, 1], [5, 8], [6, 11], [6, 4], [ 5 | 8, 4], [10, 1], [10, 7], [10, 11], [11, 14], [13, 12], [12, 2], [14, 3], [14, 8]] 6 | npts = len(path_points) 7 | pop_max = 1000 8 | mutation_rate = 0.01 9 | start_index = int(0) 10 | end_index = npts - 1 11 | generations = 1 12 | prev_best_fitness = 0 13 | nobs = 7 14 | nbits = ma.log10(npts) / ma.log10(2) 15 | chr_len = int(((nobs+2)*nbits)/nbits) 16 | stop_criteria = 0 17 | stop_generation = False 18 | img_iter_no = 1 19 | plt_tolerance = -1 20 | plt_ax_x_min = -1.0 21 | plt_ax_x_max = 16.0 22 | plt_ax_y_min = -1 23 | plt_ax_y_max = 16.0 24 | 25 | 26 | def define_links(): 27 | """ 28 | This function defines the links b/w path points 29 | 30 | Returns 31 | ------- 32 | [numpy.ndarray] 33 | [Every path point has a number of allowed connection with other path 34 | points. Those allowed connections are defined below. During calculation 35 | of fitness of population if two consecutive path points are connected 36 | then the fitness of that chromosome increases] 37 | """ 38 | 39 | link = -1 * np.ones((16, 5)) 40 | 41 | link[0][0] = 0 42 | link[0][1] = 1 43 | link[0][2] = 3 44 | link[0][3] = 4 45 | link[1][0] = 1 46 | link[1][1] = 2 47 | link[1][2] = 0 48 | link[2][0] = 2 49 | link[2][1] = 5 50 | link[2][2] = 11 51 | link[2][3] = 1 52 | link[3][0] = 3 53 | link[3][1] = 6 54 | link[3][2] = 8 55 | link[3][3] = 0 56 | link[4][0] = 4 57 | link[4][1] = 5 58 | link[4][2] = 6 59 | link[4][3] = 0 60 | link[5][0] = 5 61 | link[5][1] = 4 62 | link[5][2] = 10 63 | link[5][3] = 2 64 | link[6][0] = 6 65 | link[6][1] = 7 66 | link[6][2] = 3 67 | link[6][3] = 4 68 | link[7][0] = 7 69 | link[7][1] = 9 70 | link[7][2] = 13 71 | link[7][3] = 6 72 | link[8][0] = 8 73 | link[8][1] = 13 74 | link[8][2] = 3 75 | link[9][0] = 9 76 | link[9][1] = 10 77 | link[9][2] = 15 78 | link[9][3] = 7 79 | link[10][0] = 10 80 | link[10][1] = 9 81 | link[10][2] = 11 82 | link[10][3] = 5 83 | link[11][0] = 11 84 | link[11][1] = 12 85 | link[11][2] = 2 86 | link[11][3] = 10 87 | link[12][0] = 12 88 | link[12][1] = 15 89 | link[12][2] = 11 90 | link[13][0] = 13 91 | link[13][1] = 14 92 | link[13][2] = 15 93 | link[13][3] = 7 94 | link[13][4] = 8 95 | link[14][0] = 14 96 | link[14][1] = 15 97 | link[14][2] = 13 98 | link[15][0] = 15 99 | link[15][1] = 9 100 | link[15][2] = 12 101 | link[15][3] = 13 102 | link[15][4] = 14 103 | 104 | return link 105 | -------------------------------------------------------------------------------- /src/path-planning/genetic-algorithm-path-planning/config/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Yaaximus/robotics-python/7602e4c8cffe90aaf538b0bc87ada22a863da9a5/src/path-planning/genetic-algorithm-path-planning/config/__init__.py -------------------------------------------------------------------------------- /src/path-planning/genetic-algorithm-path-planning/docs/gifs/GA_path_planning.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Yaaximus/robotics-python/7602e4c8cffe90aaf538b0bc87ada22a863da9a5/src/path-planning/genetic-algorithm-path-planning/docs/gifs/GA_path_planning.gif -------------------------------------------------------------------------------- /src/path-planning/genetic-algorithm-path-planning/docs/gifs/GA_path_planning_2.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Yaaximus/robotics-python/7602e4c8cffe90aaf538b0bc87ada22a863da9a5/src/path-planning/genetic-algorithm-path-planning/docs/gifs/GA_path_planning_2.gif -------------------------------------------------------------------------------- /src/path-planning/genetic-algorithm-path-planning/docs/images/1.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Yaaximus/robotics-python/7602e4c8cffe90aaf538b0bc87ada22a863da9a5/src/path-planning/genetic-algorithm-path-planning/docs/images/1.png -------------------------------------------------------------------------------- /src/path-planning/genetic-algorithm-path-planning/docs/images/2.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Yaaximus/robotics-python/7602e4c8cffe90aaf538b0bc87ada22a863da9a5/src/path-planning/genetic-algorithm-path-planning/docs/images/2.png -------------------------------------------------------------------------------- /src/path-planning/genetic-algorithm-path-planning/docs/images/3.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Yaaximus/robotics-python/7602e4c8cffe90aaf538b0bc87ada22a863da9a5/src/path-planning/genetic-algorithm-path-planning/docs/images/3.png -------------------------------------------------------------------------------- /src/path-planning/genetic-algorithm-path-planning/docs/images/4.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Yaaximus/robotics-python/7602e4c8cffe90aaf538b0bc87ada22a863da9a5/src/path-planning/genetic-algorithm-path-planning/docs/images/4.png -------------------------------------------------------------------------------- /src/path-planning/genetic-algorithm-path-planning/docs/images/5.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Yaaximus/robotics-python/7602e4c8cffe90aaf538b0bc87ada22a863da9a5/src/path-planning/genetic-algorithm-path-planning/docs/images/5.png -------------------------------------------------------------------------------- /src/path-planning/genetic-algorithm-path-planning/docs/images/demo_gif_images/1.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Yaaximus/robotics-python/7602e4c8cffe90aaf538b0bc87ada22a863da9a5/src/path-planning/genetic-algorithm-path-planning/docs/images/demo_gif_images/1.png -------------------------------------------------------------------------------- /src/path-planning/genetic-algorithm-path-planning/docs/images/demo_gif_images/10.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Yaaximus/robotics-python/7602e4c8cffe90aaf538b0bc87ada22a863da9a5/src/path-planning/genetic-algorithm-path-planning/docs/images/demo_gif_images/10.png -------------------------------------------------------------------------------- /src/path-planning/genetic-algorithm-path-planning/docs/images/demo_gif_images/11.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Yaaximus/robotics-python/7602e4c8cffe90aaf538b0bc87ada22a863da9a5/src/path-planning/genetic-algorithm-path-planning/docs/images/demo_gif_images/11.png -------------------------------------------------------------------------------- /src/path-planning/genetic-algorithm-path-planning/docs/images/demo_gif_images/12.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Yaaximus/robotics-python/7602e4c8cffe90aaf538b0bc87ada22a863da9a5/src/path-planning/genetic-algorithm-path-planning/docs/images/demo_gif_images/12.png -------------------------------------------------------------------------------- /src/path-planning/genetic-algorithm-path-planning/docs/images/demo_gif_images/13.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Yaaximus/robotics-python/7602e4c8cffe90aaf538b0bc87ada22a863da9a5/src/path-planning/genetic-algorithm-path-planning/docs/images/demo_gif_images/13.png -------------------------------------------------------------------------------- /src/path-planning/genetic-algorithm-path-planning/docs/images/demo_gif_images/14.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Yaaximus/robotics-python/7602e4c8cffe90aaf538b0bc87ada22a863da9a5/src/path-planning/genetic-algorithm-path-planning/docs/images/demo_gif_images/14.png -------------------------------------------------------------------------------- /src/path-planning/genetic-algorithm-path-planning/docs/images/demo_gif_images/15.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Yaaximus/robotics-python/7602e4c8cffe90aaf538b0bc87ada22a863da9a5/src/path-planning/genetic-algorithm-path-planning/docs/images/demo_gif_images/15.png -------------------------------------------------------------------------------- /src/path-planning/genetic-algorithm-path-planning/docs/images/demo_gif_images/16.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Yaaximus/robotics-python/7602e4c8cffe90aaf538b0bc87ada22a863da9a5/src/path-planning/genetic-algorithm-path-planning/docs/images/demo_gif_images/16.png -------------------------------------------------------------------------------- /src/path-planning/genetic-algorithm-path-planning/docs/images/demo_gif_images/17.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Yaaximus/robotics-python/7602e4c8cffe90aaf538b0bc87ada22a863da9a5/src/path-planning/genetic-algorithm-path-planning/docs/images/demo_gif_images/17.png -------------------------------------------------------------------------------- /src/path-planning/genetic-algorithm-path-planning/docs/images/demo_gif_images/18.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Yaaximus/robotics-python/7602e4c8cffe90aaf538b0bc87ada22a863da9a5/src/path-planning/genetic-algorithm-path-planning/docs/images/demo_gif_images/18.png -------------------------------------------------------------------------------- /src/path-planning/genetic-algorithm-path-planning/docs/images/demo_gif_images/19.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Yaaximus/robotics-python/7602e4c8cffe90aaf538b0bc87ada22a863da9a5/src/path-planning/genetic-algorithm-path-planning/docs/images/demo_gif_images/19.png -------------------------------------------------------------------------------- /src/path-planning/genetic-algorithm-path-planning/docs/images/demo_gif_images/2.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Yaaximus/robotics-python/7602e4c8cffe90aaf538b0bc87ada22a863da9a5/src/path-planning/genetic-algorithm-path-planning/docs/images/demo_gif_images/2.png -------------------------------------------------------------------------------- /src/path-planning/genetic-algorithm-path-planning/docs/images/demo_gif_images/20.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Yaaximus/robotics-python/7602e4c8cffe90aaf538b0bc87ada22a863da9a5/src/path-planning/genetic-algorithm-path-planning/docs/images/demo_gif_images/20.png -------------------------------------------------------------------------------- /src/path-planning/genetic-algorithm-path-planning/docs/images/demo_gif_images/21.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Yaaximus/robotics-python/7602e4c8cffe90aaf538b0bc87ada22a863da9a5/src/path-planning/genetic-algorithm-path-planning/docs/images/demo_gif_images/21.png -------------------------------------------------------------------------------- /src/path-planning/genetic-algorithm-path-planning/docs/images/demo_gif_images/22.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Yaaximus/robotics-python/7602e4c8cffe90aaf538b0bc87ada22a863da9a5/src/path-planning/genetic-algorithm-path-planning/docs/images/demo_gif_images/22.png -------------------------------------------------------------------------------- /src/path-planning/genetic-algorithm-path-planning/docs/images/demo_gif_images/23.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Yaaximus/robotics-python/7602e4c8cffe90aaf538b0bc87ada22a863da9a5/src/path-planning/genetic-algorithm-path-planning/docs/images/demo_gif_images/23.png -------------------------------------------------------------------------------- /src/path-planning/genetic-algorithm-path-planning/docs/images/demo_gif_images/24.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Yaaximus/robotics-python/7602e4c8cffe90aaf538b0bc87ada22a863da9a5/src/path-planning/genetic-algorithm-path-planning/docs/images/demo_gif_images/24.png -------------------------------------------------------------------------------- /src/path-planning/genetic-algorithm-path-planning/docs/images/demo_gif_images/25.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Yaaximus/robotics-python/7602e4c8cffe90aaf538b0bc87ada22a863da9a5/src/path-planning/genetic-algorithm-path-planning/docs/images/demo_gif_images/25.png -------------------------------------------------------------------------------- /src/path-planning/genetic-algorithm-path-planning/docs/images/demo_gif_images/3.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Yaaximus/robotics-python/7602e4c8cffe90aaf538b0bc87ada22a863da9a5/src/path-planning/genetic-algorithm-path-planning/docs/images/demo_gif_images/3.png -------------------------------------------------------------------------------- /src/path-planning/genetic-algorithm-path-planning/docs/images/demo_gif_images/4.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Yaaximus/robotics-python/7602e4c8cffe90aaf538b0bc87ada22a863da9a5/src/path-planning/genetic-algorithm-path-planning/docs/images/demo_gif_images/4.png -------------------------------------------------------------------------------- /src/path-planning/genetic-algorithm-path-planning/docs/images/demo_gif_images/5.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Yaaximus/robotics-python/7602e4c8cffe90aaf538b0bc87ada22a863da9a5/src/path-planning/genetic-algorithm-path-planning/docs/images/demo_gif_images/5.png -------------------------------------------------------------------------------- /src/path-planning/genetic-algorithm-path-planning/docs/images/demo_gif_images/6.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Yaaximus/robotics-python/7602e4c8cffe90aaf538b0bc87ada22a863da9a5/src/path-planning/genetic-algorithm-path-planning/docs/images/demo_gif_images/6.png -------------------------------------------------------------------------------- /src/path-planning/genetic-algorithm-path-planning/docs/images/demo_gif_images/7.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Yaaximus/robotics-python/7602e4c8cffe90aaf538b0bc87ada22a863da9a5/src/path-planning/genetic-algorithm-path-planning/docs/images/demo_gif_images/7.png -------------------------------------------------------------------------------- /src/path-planning/genetic-algorithm-path-planning/docs/images/demo_gif_images/8.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Yaaximus/robotics-python/7602e4c8cffe90aaf538b0bc87ada22a863da9a5/src/path-planning/genetic-algorithm-path-planning/docs/images/demo_gif_images/8.png -------------------------------------------------------------------------------- /src/path-planning/genetic-algorithm-path-planning/docs/images/demo_gif_images/9.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Yaaximus/robotics-python/7602e4c8cffe90aaf538b0bc87ada22a863da9a5/src/path-planning/genetic-algorithm-path-planning/docs/images/demo_gif_images/9.png -------------------------------------------------------------------------------- /src/path-planning/genetic-algorithm-path-planning/docs/images/flow_chart_GA.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Yaaximus/robotics-python/7602e4c8cffe90aaf538b0bc87ada22a863da9a5/src/path-planning/genetic-algorithm-path-planning/docs/images/flow_chart_GA.png -------------------------------------------------------------------------------- /src/path-planning/genetic-algorithm-path-planning/main.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | """ 4 | Main script 5 | 6 | Contains: main function 7 | 8 | Author: Yasim Ahmad(yaaximus) 9 | 10 | Email: yasim.ahmed63@yahoo.com 11 | """ 12 | from tools.population import population 13 | from tools.fitness import fitness 14 | from tools.ranking import ranking 15 | from tools.dna import dna 16 | from tools.draw_plot import show_plot 17 | 18 | from config import Config 19 | 20 | def main(): 21 | """ 22 | This function encapsulates the cappbilty of initializing chromosome population 23 | and then continue calculating fitness, generate ranking, perform crossover and 24 | mutation & repeating above steps until a defined stopping crietia is not met. 25 | """ 26 | 27 | chr_population = population() 28 | 29 | chr_pop_fitness, chr_best_fitness_index = fitness(chr_pop=chr_population) 30 | 31 | chr_ranked_population = ranking(chr_pop_fitness=chr_pop_fitness, pop=chr_population) 32 | 33 | chr_crossover_mutated_population = dna(chr_pop_fitness=chr_pop_fitness, 34 | ranked_population=chr_ranked_population, chr_best_fitness_index= 35 | chr_best_fitness_index, last_pop=chr_population) 36 | 37 | while not Config.stop_generation: 38 | 39 | prev_best_fit = chr_pop_fitness[chr_best_fitness_index[0], 0] 40 | 41 | chr_pop_fitness, chr_best_fitness_index = fitness( 42 | chr_pop=chr_crossover_mutated_population) 43 | 44 | chr_ranked_population = ranking(chr_pop_fitness=chr_pop_fitness, 45 | pop=chr_crossover_mutated_population) 46 | 47 | chr_crossover_mutated_population = dna(chr_pop_fitness=chr_pop_fitness, 48 | ranked_population=chr_ranked_population, chr_best_fitness_index= 49 | chr_best_fitness_index, last_pop=chr_crossover_mutated_population) 50 | 51 | if prev_best_fit == chr_pop_fitness[chr_best_fitness_index[0], 0]: 52 | Config.stop_criteria += 1 53 | else: 54 | Config.stop_criteria = 0 55 | 56 | if Config.stop_criteria >= 5: 57 | Config.stop_generation = True 58 | 59 | print("Best chromosome is:", chr_crossover_mutated_population[chr_best_fitness_index[0]]) 60 | 61 | show_plot(best_chromosome=chr_crossover_mutated_population[0]) 62 | Config.generations += 1 63 | 64 | if __name__ == '__main__': 65 | 66 | main() 67 | -------------------------------------------------------------------------------- /src/path-planning/genetic-algorithm-path-planning/tools/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Yaaximus/robotics-python/7602e4c8cffe90aaf538b0bc87ada22a863da9a5/src/path-planning/genetic-algorithm-path-planning/tools/__init__.py -------------------------------------------------------------------------------- /src/path-planning/genetic-algorithm-path-planning/tools/dna.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | """ 4 | Script containing functionality related to DNA in Genetics like crossover & mutation. 5 | 6 | Contains: 'do crossover' & 'do mutation' function 7 | 8 | Author: Yasim Ahmad(yaaximus) 9 | 10 | Email: yasim.ahmed63@yahoo.com 11 | """ 12 | 13 | from config import Config 14 | 15 | import numpy as np 16 | import random 17 | 18 | 19 | def dna(chr_pop_fitness, ranked_population, chr_best_fitness_index, last_pop): 20 | """ 21 | This function encapsulates functionality related to dna like crossover 22 | and mutation. 23 | 24 | Parameters 25 | ---------- 26 | chr_pop_fitness : [numpy.ndarray] 27 | [Contains fitness values of chromosome population] 28 | ranked_population : [numpy.ndarray] 29 | [Contains numpy array of ranked chromosome population] 30 | chr_best_fitness_index : [list] 31 | [Contains list of best fitness indices in chromosome population] 32 | last_pop : [numpy.ndarray] 33 | [Contains numpy array of last population] 34 | 35 | Returns 36 | ------- 37 | [numpy.ndarray] 38 | [numpy array of chromosome with have gone through random crossover and mutation] 39 | """ 40 | 41 | chromo_crossover_pop = _do_crossover( 42 | ranked_pop=ranked_population, chr_best_fit_indx=chr_best_fitness_index, 43 | pop=last_pop) 44 | 45 | chromo_crossover_mutated_pop = _do_mutation(pop=chromo_crossover_pop) 46 | 47 | return chromo_crossover_mutated_pop 48 | 49 | 50 | def _do_mutation(pop): 51 | """ 52 | This function is responsible for handling mutation in population of chromosomes. 53 | 54 | Parameters 55 | ---------- 56 | pop : [numpy.ndarray] 57 | [numpy array of chromosome population which will undergo mutation] 58 | 59 | Returns 60 | ------- 61 | [numpy.ndarray] 62 | [numpy array of chromosome population undergone mutation] 63 | """ 64 | 65 | mutated_pop = np.array(pop, copy=True) 66 | 67 | itr = 3 68 | while itr < Config.pop_max: 69 | for k in range(Config.chr_len): 70 | c = random.random() 71 | if c < Config.mutation_rate and k is not 0: 72 | mutated_pop[itr, k] = random.randint(1, Config.npts-2) 73 | else: 74 | pass 75 | itr += 1 76 | return mutated_pop 77 | 78 | 79 | def _do_crossover(ranked_pop, chr_best_fit_indx, pop): 80 | """ 81 | This function is responsible for handling crossover in population of chromosomes. 82 | 83 | Parameters 84 | ---------- 85 | ranked_pop : [numpy.ndarray] 86 | [numpy array of chromosome population which will undergo crossover] 87 | chr_best_fit_indx : [list] 88 | [Contains list of best fitness indices in chromosome population] 89 | pop : [numpy.ndarray] 90 | [numpy array of chromosome population to get best fitness chromosomes 91 | from last population] 92 | 93 | Returns 94 | ------- 95 | [numpy.ndarray] 96 | [numpy array of chromosome population undergone crossover] 97 | """ 98 | 99 | crossover_pop = np.zeros((Config.pop_max, Config.chr_len)) 100 | 101 | crossover_pop[0, :] = pop[chr_best_fit_indx[0], :] 102 | crossover_pop[1, :] = pop[chr_best_fit_indx[1], :] 103 | crossover_pop[2, :] = pop[chr_best_fit_indx[2], :] 104 | 105 | itr = 3 106 | 107 | while itr < Config.pop_max / 5: 108 | 109 | a = random.randint(0, Config.chr_len - 1) 110 | b = random.randint(0, Config.chr_len - 1) 111 | 112 | partner_a = ranked_pop[a, :] 113 | partner_b = ranked_pop[b, :] 114 | joining_pt = random.randint(0, Config.chr_len - 1) 115 | 116 | crossover_pop[itr, :joining_pt] = partner_a[:joining_pt] 117 | crossover_pop[itr+1, :joining_pt] = partner_b[:joining_pt] 118 | 119 | crossover_pop[itr, joining_pt:] = partner_b[joining_pt:] 120 | crossover_pop[itr+1, joining_pt:] = partner_a[joining_pt:] 121 | 122 | itr += 2 123 | 124 | while itr < Config.pop_max: 125 | 126 | crossover_pop[itr] = ranked_pop[itr] 127 | itr += 1 128 | 129 | return crossover_pop 130 | -------------------------------------------------------------------------------- /src/path-planning/genetic-algorithm-path-planning/tools/draw_plot.py: -------------------------------------------------------------------------------- 1 | from config import Config 2 | import matplotlib.pyplot as plt 3 | 4 | 5 | def show_plot(best_chromosome): 6 | """ 7 | This function is responsible for displaying plot. 8 | 9 | Parameters 10 | ---------- 11 | best_chromosome : [numpy.ndarray] 12 | [numpy array of best chromosome in population of chromosomes] 13 | """ 14 | 15 | plt.figure(num=1) 16 | plt.clf() 17 | plt.axis([Config.plt_ax_x_min, Config.plt_ax_x_max, Config.plt_ax_y_min, 18 | Config.plt_ax_y_max]) 19 | 20 | _draw_path_points() 21 | _draw_obstacles() 22 | 23 | best_path_x = [] 24 | best_path_y = [] 25 | 26 | plt.annotate('Start Point', xy=(Config.path_points[int(best_chromosome[0])][0] 27 | + Config.plt_tolerance, Config.path_points[int(best_chromosome[0])][1])) 28 | plt.annotate('Goal Point', xy=(Config.path_points[int(best_chromosome[-1])][0] 29 | + Config.plt_tolerance, Config.path_points[int(best_chromosome[-1])][1])) 30 | 31 | plt.text(x=Config.plt_ax_x_min, y=Config.plt_ax_y_max + Config.plt_tolerance, 32 | s='Generation:(%s)'%(Config.generations)) 33 | 34 | for element in best_chromosome: 35 | 36 | best_path_x.append(Config.path_points[int(element)][0]) 37 | best_path_y.append(Config.path_points[int(element)][1]) 38 | 39 | plt.plot(best_path_x, best_path_y, "g-") 40 | plt.draw() 41 | plt.savefig("./docs/images/"+str(Config.img_iter_no)+".png") 42 | Config.img_iter_no += 1 43 | plt.pause(0.01) 44 | 45 | 46 | def _draw_path_points(): 47 | """ 48 | This function is responsible for displaying path points on plot. 49 | """ 50 | 51 | node_x = [] 52 | node_y = [] 53 | 54 | for element in Config.path_points: 55 | node_x.append(element[0]) 56 | node_y.append(element[1]) 57 | 58 | plt.plot(node_x, node_y, "ko") 59 | 60 | 61 | def _draw_obstacles(): 62 | """ 63 | This function is responsible for displaying obstacles on plot. 64 | """ 65 | 66 | obs_1_x = [2.5, 3.5, 3.5, 2.5, 2.5] 67 | obs_1_y = [9, 9, 12, 12, 9] 68 | plt.fill(obs_1_x, obs_1_y, "r") 69 | 70 | plt.legend(('Path points', 'Obstacles'), loc='upper right', fontsize='small', 71 | numpoints=1, markerscale=0.5, labelspacing=1) 72 | 73 | obs_2_x = [3, 4, 4, 3, 3] 74 | obs_2_y = [6.5, 6.5, 4, 4, 6.5] 75 | plt.fill(obs_2_x, obs_2_y, "r") 76 | 77 | obs_3_x = [7, 9, 9, 7, 7] 78 | obs_3_y = [12, 12, 13, 13, 12] 79 | plt.fill(obs_3_x, obs_3_y, "r") 80 | 81 | obs_4_x = [6.5, 8, 8, 6.5, 6.5] 82 | obs_4_y = [6, 6, 9.5, 9.5, 6] 83 | plt.fill(obs_4_x, obs_4_y, "r") 84 | 85 | obs_5_x = [5.7, 8.7, 8.7, 5.7, 5.7] 86 | obs_5_y = [2, 2, 3, 3, 2] 87 | plt.fill(obs_5_x, obs_5_y, "r") 88 | 89 | obs_6_x = [11, 12, 12, 11, 11] 90 | obs_6_y = [8.5, 8.5, 12, 12, 8.5] 91 | plt.fill(obs_6_x, obs_6_y, "r") 92 | 93 | obs_7_x = [10, 11.5, 11.5, 10, 10] 94 | obs_7_y = [3.5, 3.5, 5.5, 5.5, 3.5] 95 | plt.fill(obs_7_x, obs_7_y, "r") 96 | -------------------------------------------------------------------------------- /src/path-planning/genetic-algorithm-path-planning/tools/fitness.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | """ 4 | Script provides functionality related to fitness of chromosomes based 5 | on total distance and connection b.w two consecutive nodes of a chromosome 6 | 7 | Author: Yasim Ahmad(yaaximus) 8 | 9 | Email: yasim.ahmed63@yahoo.com 10 | """ 11 | 12 | from config import Config 13 | from tools.population import calculate_distance 14 | 15 | import numpy as np 16 | 17 | 18 | def fitness(chr_pop): 19 | """ 20 | This function is responsible for calculating fitness of chromosomes. 21 | 22 | Parameters 23 | ---------- 24 | chr_pop : [numpy.ndarray] 25 | [Population of chromosomes whose fitness is to be calculated] 26 | 27 | Returns 28 | ------- 29 | [numpy.ndarray] 30 | [(1)Population of chromosomes whose fitness is calculated, 31 | (2)List of indices of best fitness chromosomes] 32 | """ 33 | 34 | chromo_pts_consec_dist = chr_pts_consecutive_dist(pop=chr_pop) 35 | 36 | chromo_fit_based_dist = chr_fit_based_dist(chr_pts_consec_dist=chromo_pts_consec_dist) 37 | 38 | chromo_conn = chr_conn(chr_pop=chr_pop) 39 | 40 | chromo_fit_based_conn = chr_fit_based_conn(chr_conn=chromo_conn) 41 | 42 | chromo_fit = chr_fit(chr_fit_based_dist=chromo_fit_based_dist, 43 | chr_fit_based_conn=chromo_fit_based_conn) 44 | 45 | chromo_best_fit_index = chr_best_fit_ind(chr_fit=chromo_fit) 46 | 47 | return chromo_fit, chromo_best_fit_index 48 | 49 | 50 | def chr_best_fit_ind(chr_fit): 51 | """ 52 | This function is responsible for finding best fitness chromosome indices. 53 | 54 | Parameters 55 | ---------- 56 | chr_fit : [numpy.ndarray] 57 | [numpy array of chromosome population fitness] 58 | 59 | Returns 60 | ------- 61 | [list] 62 | [list of best fitness chromosome indices] 63 | """ 64 | 65 | temp_chr_fit = np.array(chr_fit, copy=True) 66 | 67 | chr_best_fit_index = [] 68 | 69 | while len(chr_best_fit_index) < 3: 70 | 71 | y = np.where(temp_chr_fit == np.amax(temp_chr_fit))[0] 72 | 73 | for i in range(len(y)): 74 | chr_best_fit_index.append(int(y[i])) 75 | 76 | for i in chr_best_fit_index: 77 | temp_chr_fit[i][0] = 0 78 | 79 | return chr_best_fit_index 80 | 81 | 82 | def chr_fit(chr_fit_based_dist, chr_fit_based_conn): 83 | """ 84 | This function is responsible for calculating fitness of chromosome population 85 | based on total distance of individual chromosome, and links b/w path points 86 | of individual chromosome. 87 | 88 | Parameters 89 | ---------- 90 | chr_fit_based_dist : [numpy.ndarray] 91 | [numpy array of chromosome fitness based on total distance] 92 | chr_fit_based_conn : [numpy.ndarray] 93 | [numpy array of chromosome fitness based on links b/w path 94 | points of individual chromosome] 95 | 96 | Returns 97 | ------- 98 | [numpy.ndarray] 99 | [final fitness of chromosome population] 100 | """ 101 | 102 | chr_fit = np.zeros((Config.pop_max, 1)) 103 | 104 | for i in range(Config.pop_max): 105 | 106 | chr_fit[i][0] = chr_fit_based_dist[i][0] + chr_fit_based_conn[i][0] 107 | 108 | return chr_fit 109 | 110 | 111 | def chr_fit_based_conn(chr_conn): 112 | """ 113 | This function is responsible for calculating fitness of chromosome population 114 | based on number of connections b/w path points of an individual chromosome 115 | 116 | Parameters 117 | ---------- 118 | chr_conn : [numpy.ndarray] 119 | [numpy array of number of connection b/w path points of an individual chromosome] 120 | 121 | Returns 122 | ------- 123 | [numpy.ndarray] 124 | [numpy array of chromosome population fitness based on connections] 125 | """ 126 | 127 | chr_conn_fit = np.zeros((Config.pop_max, 1)) 128 | 129 | for i in range(Config.pop_max): 130 | 131 | chr_conn_fit[i][0] = chr_conn[i][0] / ( Config.chr_len - 1 ) 132 | 133 | return chr_conn_fit 134 | 135 | 136 | def chr_conn(chr_pop): 137 | """ 138 | This function is responsible for finding number of connections b/w path points of 139 | a individual chromosome. 140 | 141 | Parameters 142 | ---------- 143 | chr_pop : [numpy.ndarray] 144 | [Population of chromosomes whose number of connections are to be calculated] 145 | 146 | Returns 147 | ------- 148 | [numpy.ndarray] 149 | [numpy array of number of connection b/w path points of an individual chromosome] 150 | """ 151 | 152 | link = Config.define_links() 153 | chr_conn = np.zeros((Config.pop_max, 1)) 154 | 155 | for i in range(Config.pop_max): 156 | for j in range(Config.chr_len-1): 157 | a = int(chr_pop[i][j]) 158 | b = int(chr_pop[i][j+1]) 159 | for k in range(np.shape(link)[1]): 160 | if link[a, k] == b: 161 | chr_conn[i][0] += 1 162 | 163 | return chr_conn 164 | 165 | 166 | def chr_fit_based_dist(chr_pts_consec_dist): 167 | """ 168 | This function is responsible for calculating chromosome fitness based on total 169 | distance of individual chromosome. 170 | 171 | Parameters 172 | ---------- 173 | chr_pts_consec_dist : [numpy.ndarray] 174 | [numpy array of individual chromosome total distance] 175 | 176 | Returns 177 | ------- 178 | [numpy.ndarray] 179 | [numpy array of individual chromosome fitness based on total distance] 180 | """ 181 | 182 | chr_pop_fit_based_dist = np.zeros((Config.pop_max, 1)) 183 | 184 | for i in range(Config.pop_max): 185 | 186 | chr_pop_fit_based_dist[i][0] = 10.0 * \ 187 | (1.0 / np.sum(chr_pts_consec_dist[i], keepdims=True)) 188 | 189 | return chr_pop_fit_based_dist 190 | 191 | 192 | def chr_pts_consecutive_dist(pop): 193 | """ 194 | This function is responsible for calculating total distance of individual 195 | chromosome in population of chromosomes. 196 | 197 | Parameters 198 | ---------- 199 | pop : [numpy.ndarray] 200 | [Population of chromosomes whose total distance is to be calculated] 201 | 202 | Returns 203 | ------- 204 | [umpy.ndarray] 205 | [numpy array of individual chromosome total distance] 206 | """ 207 | 208 | chr_pop_dist = np.zeros((Config.pop_max, Config.chr_len-1)) 209 | 210 | for i in range(Config.pop_max): 211 | 212 | for j in range(Config.chr_len-1): 213 | 214 | chr_pop_dist[i][j] = calculate_distance( 215 | pt_1=Config.path_points[int(pop[i][j+1])], 216 | pt_2=Config.path_points[int(pop[i][j])]) 217 | 218 | return chr_pop_dist 219 | -------------------------------------------------------------------------------- /src/path-planning/genetic-algorithm-path-planning/tools/population.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | """ 4 | Script provide functionality related to initialization of population based 5 | on nodes(path points) and links b/w nodes(path points) 6 | 7 | Author: Yasim Ahmad(yaaximus) 8 | 9 | Email: yasim.ahmed63@yahoo.com 10 | """ 11 | 12 | from config import Config 13 | 14 | import numpy as np 15 | import math as ma 16 | import random 17 | 18 | 19 | def population(): 20 | """ 21 | This function encapsulates the capability to initialize population of chromosomes. 22 | 23 | Returns 24 | ------- 25 | [numpy.ndarray] 26 | [Population of chromosomes] 27 | """ 28 | 29 | np.set_printoptions(threshold=np.nan) 30 | link = Config.define_links() 31 | link_fit = _link_distance(link) 32 | link_prob = _link_prob(link_fit) 33 | link_cum_prob = np.cumsum(link_prob, axis=1) 34 | initial_pop = _create_pop(link_cum_prob=link_cum_prob) 35 | 36 | return initial_pop 37 | 38 | 39 | def _link_distance(link): 40 | """ 41 | This function is responsible for calculating distance b/w links 42 | 43 | Parameters 44 | ---------- 45 | link : [numpy.ndarray] 46 | [links b/w path points of chromosomes] 47 | 48 | Returns 49 | ------- 50 | [numpy.ndarray] 51 | [numpy array of distance b/w links] 52 | """ 53 | 54 | link_dist = np.zeros((np.shape(link)[0], np.shape(link)[1]-1)) 55 | 56 | for i in range(np.shape(link)[0]): 57 | 58 | for j in range((np.shape(link)[1])-1): 59 | 60 | if link[i][j] > -0.1 and link[i][j+1] > -0.1: 61 | 62 | link_dist[i][j] = calculate_distance( 63 | pt_1=Config.path_points[int(link[i][j])], pt_2=Config.path_points[int(link[i][j+1])]) 64 | 65 | return link_dist 66 | 67 | 68 | def _link_prob(link_fit): 69 | """ 70 | This function calculates the probability of links. 71 | 72 | Parameters 73 | ---------- 74 | link_fit : [numpy.ndarray] 75 | [numpy array of links connections fitness based on distance] 76 | 77 | Returns 78 | ------- 79 | [numpy.ndarray] 80 | [numpy array of links probability based on links fitness] 81 | """ 82 | 83 | link_prob = np.zeros((np.shape(link_fit)[0], np.shape(link_fit)[1])) 84 | 85 | for i in range(np.shape(link_fit)[0]): 86 | 87 | for j in range(np.shape(link_fit)[1]): 88 | 89 | link_prob[i][j] = link_fit[i][j]/np.sum(link_fit[i], keepdims=True) 90 | 91 | return link_prob 92 | 93 | 94 | def _create_pop(link_cum_prob): 95 | """ 96 | This function is responsible for creating chromosome population based on connection 97 | b/w links. 98 | 99 | Parameters 100 | ---------- 101 | link_cum_prob : [numpy.ndarray] 102 | [numpy array of links cumulative probability based on links fitness] 103 | 104 | Returns 105 | ------- 106 | [numpy.ndarray] 107 | [numpy array of chromosome population based on connection b/w links] 108 | """ 109 | 110 | pop = np.zeros((Config.pop_max, Config.chr_len)) 111 | pop[:, 0] = Config.start_index 112 | pop[:, Config.chr_len - 1] = Config.end_index 113 | 114 | link = Config.define_links() 115 | 116 | for k in range(Config.pop_max): 117 | i = Config.start_index 118 | j = Config.start_index + 1 119 | while j < Config.chr_len: 120 | i = int(i) 121 | if j > 0 and j < (Config.chr_len - 1): 122 | random_val = random.random() 123 | if random_val < link_cum_prob[i][0]: 124 | pop[k][j] = link[i][1] 125 | i = link[i][1] 126 | if _both_equ(i, Config.end_index): 127 | while j < (Config.chr_len - 1): 128 | pop[k][j+1] = Config.end_index 129 | j += 1 130 | elif random_val < link_cum_prob[i][1]: 131 | pop[k][j] = link[i][2] 132 | i = link[i][2] 133 | if _both_equ(i, Config.end_index): 134 | while j < (Config.chr_len - 1): 135 | pop[k][j+1] = Config.end_index 136 | j += 1 137 | elif random_val < link_cum_prob[i][2]: 138 | pop[k][j] = link[i][3] 139 | i = link[i][3] 140 | if _both_equ(i, Config.end_index): 141 | while j < (Config.chr_len - 1): 142 | pop[k][j+1] = Config.end_index 143 | j += 1 144 | elif random_val < link_cum_prob[i][3]: 145 | pop[k][j] = link[i][4] 146 | i = link[i][4] 147 | if _both_equ(i, Config.end_index): 148 | while j < (Config.chr_len - 1): 149 | pop[k][j+1] = Config.end_index 150 | j += 1 151 | j += 1 152 | 153 | return pop 154 | 155 | 156 | def _both_equ(element_1, element_2): 157 | """ 158 | This function is responsible for finding if both elements are equal or not. 159 | 160 | Parameters 161 | ---------- 162 | element_1 : [Int] 163 | [First element for comparison] 164 | element_2 : [Int] 165 | [Second element for comparison] 166 | 167 | Returns 168 | ------- 169 | [Bool] 170 | [True or False based on wether both elements were equal or not] 171 | """ 172 | 173 | return True if int(element_1) == int(element_2) else False 174 | 175 | 176 | def calculate_distance(pt_1, pt_2): 177 | """ 178 | This function encapsulates the capability of calculating distance b/w two points. 179 | 180 | Parameters 181 | ---------- 182 | pt_1 : [Float] 183 | [point 1 for calculating distance] 184 | pt_2 : [Float] 185 | [point 2 for calculating distance] 186 | 187 | Returns 188 | ------- 189 | [float] 190 | [Distance b/w two points] 191 | """ 192 | 193 | return ma.sqrt(ma.pow((pt_1[0]-pt_2[0]), 2)+ma.pow((pt_1[1]-pt_2[1]), 2)) 194 | -------------------------------------------------------------------------------- /src/path-planning/genetic-algorithm-path-planning/tools/ranking.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | """ 4 | Script provide functionality related to ranking of the population. Ranking is 5 | obtained using roulets wheel selection method. 6 | 7 | Author: Yasim Ahmad(yaaximus) 8 | 9 | Email: yasim.ahmed63@yahoo.com 10 | """ 11 | 12 | from config import Config 13 | 14 | import numpy as np 15 | 16 | 17 | def ranking(chr_pop_fitness, pop): 18 | """ 19 | This function encapsulates the capability of generate ranking of chromosome 20 | population based on roulet wheel selection method. 21 | 22 | Parameters 23 | ---------- 24 | chr_pop_fitness : [numpy.ndarray] 25 | [fitness of chromosome population] 26 | pop : [numpy.ndarray] 27 | [Population of chromosomes which will undergo ranking process] 28 | 29 | Returns 30 | ------- 31 | [numpy.ndarray] 32 | [Population of ranked chromosomes] 33 | """ 34 | 35 | chromo_prob = cal_prob(chr_pop_fitness=chr_pop_fitness) 36 | 37 | chromo_cum_prob = np.cumsum(chromo_prob, axis=0) 38 | 39 | chromo_rank = _ranking_based_on_roulet_wheel_selection(chr_cum_prob=chromo_cum_prob) 40 | 41 | chromo_pop_ranked = _generate_mating_pool(chr_rank=chromo_rank, pop=pop) 42 | 43 | return chromo_pop_ranked 44 | 45 | 46 | def _generate_mating_pool(chr_rank, pop): 47 | """ 48 | This function is responsible for generating mating pool which will undergo 49 | crossover and mutation in the next stage. 50 | 51 | Parameters 52 | ---------- 53 | chr_rank : [numpy.ndarray] 54 | [ranks of chromosomes based on roulets wheeel selection method] 55 | pop : [numpy.ndarray] 56 | [Population of chromosomes from which mating pool will be generated] 57 | 58 | Returns 59 | ------- 60 | [numpy.ndarray] 61 | [numpy arrayy of mating pool] 62 | """ 63 | 64 | ranked_pop = np.zeros((1, np.shape(pop)[1])) 65 | 66 | for i in range(Config.pop_max): 67 | for j in range(int(chr_rank[i, 0])): 68 | if np.shape(ranked_pop)[0] == 1: 69 | ranked_pop = pop[i, :] 70 | else: 71 | ranked_pop = np.vstack((ranked_pop, pop[i, :])) 72 | return ranked_pop 73 | 74 | 75 | def _ranking_based_on_roulet_wheel_selection(chr_cum_prob): 76 | """ 77 | This function encapsulates the capability of doing ranking of chromosomes 78 | population based on roulets wheel selection method 79 | 80 | Parameters 81 | ---------- 82 | chr_cum_prob : [numpy.ndarray] 83 | [Chromosome cumulative probabilty based on chromosome fitness] 84 | 85 | Returns 86 | ------- 87 | [numpy.ndarray] 88 | [ranks of chromosomes based on roulets wheeel selection method] 89 | """ 90 | 91 | rand_array = np.random.rand(Config.pop_max) 92 | no_of_times_chr_got_choosen = np.zeros((Config.pop_max, 1)) 93 | chr_rank = np.zeros((Config.pop_max, 1)) 94 | 95 | for i in range(Config.pop_max): 96 | k = 0 97 | while chr_cum_prob[k, 0] < rand_array[i]: 98 | k += 1 99 | no_of_times_chr_got_choosen[i, 0] = k 100 | 101 | for i in range(Config.pop_max): 102 | for j in range(Config.pop_max): 103 | if no_of_times_chr_got_choosen[j, 0] == i: 104 | chr_rank[i, 0] += 1 105 | 106 | return chr_rank 107 | 108 | 109 | def cal_prob(chr_pop_fitness): 110 | """ 111 | 112 | 113 | Parameters 114 | ---------- 115 | chr_pop_fitness : [numpy.ndarray] 116 | [fitness of chromosome population] 117 | 118 | Returns 119 | ------- 120 | [numpy.ndarray] 121 | [Chromosome probabilty based on chromosome fitness] 122 | """ 123 | 124 | chr_prob = np.zeros((Config.pop_max, 1)) 125 | 126 | for i in range(Config.pop_max): 127 | 128 | chr_prob[i, 0] = chr_pop_fitness[i, 0] / \ 129 | np.sum(chr_pop_fitness, keepdims=True) 130 | 131 | return chr_prob 132 | --------------------------------------------------------------------------------