├── misc ├── demo.gif └── apf_diagram.png ├── classes ├── __pycache__ │ ├── objects.cpython-37.pyc │ └── positional.cpython-37.pyc ├── positional.py └── objects.py ├── README.md └── main.py /misc/demo.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/nccvector/aritificial-potential-field/HEAD/misc/demo.gif -------------------------------------------------------------------------------- /misc/apf_diagram.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/nccvector/aritificial-potential-field/HEAD/misc/apf_diagram.png -------------------------------------------------------------------------------- /classes/__pycache__/objects.cpython-37.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/nccvector/aritificial-potential-field/HEAD/classes/__pycache__/objects.cpython-37.pyc -------------------------------------------------------------------------------- /classes/__pycache__/positional.cpython-37.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/nccvector/aritificial-potential-field/HEAD/classes/__pycache__/positional.cpython-37.pyc -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # Artificial Potential Field for path planning 2 | 3 | ### Algorithm in action: 4 | 5 | - Blue => Agent trajectory 6 | - Red => Obstacle 7 | - Green => Goal 8 | 9 |

10 | 11 | 12 | ### What is artificial potential field? 13 | 14 |

15 | 16 | At each instance the surrounding (reachable) coordinates are evaluated using a cost function. The cost function calculates the attraction/repulsion force of all the objects in the scenes (repulsion for obstacles and attraction for goal) and assigns a score value to each cell. 17 | This algorithm does not garauntee optimal shortest path as it follows local gradients. We can use random particles with pobablistic cell selection to get better results (computationally expensive but sometimes may give optimal results). -------------------------------------------------------------------------------- /classes/positional.py: -------------------------------------------------------------------------------- 1 | import math 2 | 3 | # Position class for position based calculations 4 | class Position: 5 | """ 6 | Creates a Position object 7 | 8 | Parameters 9 | ---------- 10 | x : double 11 | x coordinate 12 | y : double 13 | y coordinate 14 | 15 | """ 16 | 17 | def __init__(self, x, y): 18 | 19 | self.x = x 20 | self.y = y 21 | 22 | # General type function 23 | def calculate_distance(self, other): 24 | """ 25 | Calculates distance between current position and the position passed as parameter 26 | It can also be usedd as static function by specifying class name and giving two parameters 27 | 28 | Parameters 29 | ---------- 30 | other : Position 31 | Position to check distance against 32 | 33 | Returns 34 | ------- 35 | double 36 | Distance value 37 | 38 | """ 39 | 40 | return math.sqrt((self.x - other.x)**2 + (self.y - other.y)**2) 41 | 42 | # General type function 43 | def calculate_distance_squared(self, other): 44 | """ 45 | Calculates squared distance between current position and the position passed as parameter 46 | It can also be usedd as static function by specifying class name and giving two parameters 47 | 48 | Parameters 49 | ---------- 50 | other : Position 51 | Position to check squared distance against 52 | 53 | Returns 54 | ------- 55 | double 56 | Distance value squared 57 | 58 | """ 59 | 60 | return (self.x - other.x)**2 + (self.y - other.y)**2 61 | -------------------------------------------------------------------------------- /main.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | from cv2 import cv2 3 | import math 4 | 5 | from classes.positional import Position 6 | from classes.objects import * 7 | 8 | # Main 9 | if __name__ == '__main__': 10 | # Defining world dimensions 11 | world_size = (640, 480) 12 | # Initializing blank canvas with white color 13 | image = np.ones((world_size[1],world_size[0],3),dtype=np.uint8) * 255 14 | 15 | # Defining agent and goal 16 | agent = Agent(Position(50, 50), scan_radius=10, possible_moves=30) 17 | goal = Goal(Position(450, 450), sigma=math.sqrt(world_size[0]**2 + world_size[1]**2)) 18 | 19 | # Defining obstacles in a list 20 | sigma_obstacles = 5 21 | obstacles = [Obstacle(Position(250, 180), sigma=sigma_obstacles, draw_radius=4*sigma_obstacles), 22 | Obstacle(Position(250, 280), sigma=sigma_obstacles, draw_radius=4*sigma_obstacles), 23 | Obstacle(Position(250, 380), sigma=sigma_obstacles, draw_radius=4*sigma_obstacles), 24 | Obstacle(Position(350, 180), sigma=sigma_obstacles, draw_radius=4*sigma_obstacles), 25 | Obstacle(Position(350, 280), sigma=sigma_obstacles, draw_radius=4*sigma_obstacles), 26 | Obstacle(Position(350, 380), sigma=sigma_obstacles, draw_radius=4*sigma_obstacles)] 27 | 28 | # Drawing objects 29 | agent.draw(image) 30 | goal.draw(image) 31 | for obstacle in obstacles: 32 | obstacle.draw(image) 33 | 34 | # Displaying initial frame and wait for intial key press 35 | cv2.imshow('Output', image) 36 | cv2.waitKey(1000) 37 | 38 | while Position.calculate_distance(agent.position, goal.position) > 10: 39 | possible_moves = agent.get_possible_moves() 40 | min_value = math.inf 41 | best_move = possible_moves[0] # initializing best move with first move 42 | # Finding move with the least value 43 | for move in possible_moves: 44 | move_value = goal.get_attraction_force(move) 45 | for obstacle in obstacles: 46 | move_value += obstacle.get_repulsion_force(move) 47 | 48 | if move_value < min_value: 49 | min_value = move_value 50 | best_move = move 51 | 52 | # Setting best move as agent's next position 53 | agent.position = best_move 54 | 55 | ''' 56 | As we are not clearing up the initial frame at every iteration 57 | so we do not need to draw static objects again and again 58 | 59 | ''' 60 | agent.draw(image) 61 | 62 | # Displaying updated frame 63 | cv2.imshow('Output', image) 64 | cv2.waitKey(20) 65 | 66 | # Hold on last frame 67 | cv2.waitKey(0) 68 | 69 | -------------------------------------------------------------------------------- /classes/objects.py: -------------------------------------------------------------------------------- 1 | import math 2 | from cv2 import cv2 3 | from classes.positional import Position 4 | 5 | # Agent class for Agent attributes 6 | class Agent: 7 | """ 8 | Creates an Agent object 9 | 10 | Parameters 11 | ---------- 12 | position : Position 13 | Position of agent in world 14 | scan_radius : int, optional 15 | Step size of agent, by default 1 16 | possible_moves : int, optional 17 | Number of point generated around agent, by default 6 18 | draw_radius : int, optional 19 | Radius for visualization, by default 5 20 | draw_color : tuple, optional 21 | Color for visulization, by default (255,0,0) 22 | 23 | """ 24 | 25 | def __init__(self, position, scan_radius=1, possible_moves=6, draw_radius=5, draw_color=(255,0,0)): 26 | 27 | # Property attributes 28 | self.position = position 29 | self._scan_radius = scan_radius 30 | self._possible_moves = possible_moves 31 | 32 | # Visual attributes 33 | self._draw_radius = draw_radius 34 | self._draw_color = draw_color 35 | 36 | def draw(self, image): 37 | cv2.circle(image, (int(self.position.x), int(self.position.y)), 38 | self._draw_radius, self._draw_color, -1) # Fill 39 | 40 | def get_possible_moves(self): 41 | """ 42 | Makes a list of points around agent 43 | 44 | Returns 45 | ------- 46 | list 47 | List of points around agent 48 | 49 | """ 50 | 51 | angle_increment = (2*math.pi)/self._possible_moves # 2pi/n 52 | angle = -angle_increment # Going one step negative to start from zero 53 | possible_moves_list = [] 54 | for _ in range(self._possible_moves): 55 | # Starting from angle 0 56 | angle += angle_increment 57 | possible_moves_list.append(Position(self._scan_radius * math.cos(angle) + self.position.x, 58 | self._scan_radius * math.sin(angle) + self.position.y)) 59 | 60 | return possible_moves_list 61 | 62 | # Obstacle class for repulsion based objects 63 | class Obstacle: 64 | """ 65 | Creates an Obstacle object 66 | 67 | Parameters 68 | ---------- 69 | position : Position 70 | Position of Obstacle in the world 71 | mu : int, optional 72 | Peak of distribution, by default 1 73 | sigma : int, optional 74 | Spread of distribution, by default 1 75 | draw_radius : int, optional 76 | Radius for visualization, by default 5 77 | draw_color : tuple, optional 78 | Color for visualization, by default (0,0,255) 79 | 80 | """ 81 | 82 | def __init__(self, position, mu=1, sigma=1, draw_radius=5, draw_color=(0,0,255)): 83 | 84 | # Property attributes 85 | self.position = position 86 | self._mu = mu 87 | self._sigma = sigma 88 | 89 | # Visual attributes 90 | self._draw_radius = draw_radius 91 | self._draw_color = draw_color 92 | 93 | def draw(self, image): 94 | cv2.circle(image, (int(self.position.x), int(self.position.y)), 95 | self._draw_radius, self._draw_color, -1) # Fill 96 | 97 | # Attribute type function 98 | def get_repulsion_force(self, position): 99 | """ 100 | Repulsion force calculation function 101 | 102 | Parameters 103 | ---------- 104 | position : Position 105 | Position of cell to check force at 106 | 107 | Returns 108 | ------- 109 | double 110 | The value of repulsion at cell 111 | 112 | """ 113 | 114 | # Implementing repulsion equation 115 | dist_value = (1/(self._sigma*math.sqrt(2*math.pi))) * math.exp(-(Position.calculate_distance_squared(self.position,position)/(2*self._sigma*self._sigma))) 116 | return dist_value 117 | 118 | # Goal class for Goal object 119 | class Goal: 120 | """ 121 | Creates a Goal object 122 | 123 | Parameters 124 | ---------- 125 | position : Position 126 | Position of Goal in the world 127 | mu : int, optional 128 | Peak of distribution, by default 1 129 | sigma : int, optional 130 | Spread of distribution, by default 1 131 | draw_radius : int, optional 132 | Radius for visualization, by default 5 133 | draw_color : tuple, optional 134 | Color for visualization, by default (0,255,0) 135 | 136 | """ 137 | 138 | def __init__(self, position, mu=1, sigma=1, draw_radius=5, draw_color=(0,255,0)): 139 | 140 | # Property attributes 141 | self.position = position 142 | self._mu = mu 143 | self._sigma = sigma 144 | 145 | # Visual attributes 146 | self._draw_radius = draw_radius 147 | self._draw_color = draw_color 148 | 149 | def draw(self, image): 150 | cv2.circle(image, (int(self.position.x), int(self.position.y)), 151 | self._draw_radius, self._draw_color, -1) # Fill 152 | 153 | # Attribute type function 154 | def get_attraction_force(self, position): 155 | """ 156 | Attraction force calculation function 157 | 158 | Parameters 159 | ---------- 160 | position : Position 161 | Position of cell to check force at 162 | 163 | Returns 164 | ------- 165 | double 166 | The value of attraction at cell 167 | 168 | """ 169 | 170 | # Implementing attraction equation 171 | dist_value = -(1/(self._sigma*math.sqrt(2*math.pi))) * math.exp(-(Position.calculate_distance_squared(self.position,position)/(2*self._sigma*self._sigma))) 172 | return dist_value 173 | --------------------------------------------------------------------------------