├── LICENSE ├── 动态势场 ├── dynamic.gif ├── main.py ├── objects.py └── positional.py ├── 多机器人协作 ├── __pycache__ │ ├── objects.cpython-36.pyc │ └── positional.cpython-36.pyc ├── main.py ├── multi.gif ├── objects.py └── positional.py └── 静态势场 ├── break_local_min1.gif ├── local_min.gif ├── local_min1.gif ├── main.py ├── objects.py └── positional.py /LICENSE: -------------------------------------------------------------------------------- 1 | MIT License 2 | 3 | Copyright (c) 2019 jinzhuer 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy 6 | of this software and associated documentation files (the "Software"), to deal 7 | in the Software without restriction, including without limitation the rights 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | copies of the Software, and to permit persons to whom the Software is 10 | furnished to do so, subject to the following conditions: 11 | 12 | The above copyright notice and this permission notice shall be included in all 13 | copies or substantial portions of the Software. 14 | 15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | SOFTWARE. 22 | -------------------------------------------------------------------------------- /动态势场/dynamic.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/dlml/artificial_potential_field_method/ea96c667d2cc68c4e71477015c64c19d21bc4544/动态势场/dynamic.gif -------------------------------------------------------------------------------- /动态势场/main.py: -------------------------------------------------------------------------------- 1 | """ 2 | Main script 3 | 4 | Contains 5 | ------- 6 | Functions 7 | draw and __main__ 8 | 9 | Author 10 | ------- 11 | Faizan Ali(nccvector) 12 | 13 | """ 14 | 15 | import numpy as np 16 | import cv2 17 | import math 18 | import random 19 | 20 | from positional import * 21 | from objects import * 22 | 23 | def testCrash(position1,position2): 24 | return 0 25 | 26 | 27 | # Main 28 | if __name__ == '__main__': 29 | # Defining world dimensions 30 | #world_size = (640, 480) 31 | world_size = (800,800) 32 | # Initializing blank canvas with white color 33 | image = np.ones((world_size[1],world_size[0],3),dtype=np.uint8) * 255 34 | #background color 35 | image[:,:] = [245,245,245] 36 | 37 | #line color 38 | x_line = list(range(0,800,20)) 39 | y_line = list(range(0,800,20)) 40 | 41 | image[x_line,:] = [128,128,128] 42 | image[:,y_line] = [128,128,128] 43 | 44 | # Defining agent and goal 45 | agent = Agent(Position(40, 40), scan_radius=20, possible_moves=4) 46 | goal = Goal(Position(640, 640), sigma=math.sqrt(world_size[0]**2 + world_size[1]**2)) 47 | 48 | # Defining obstacles in a list 49 | sigma_obstacles = 5 50 | ''' 51 | obstacles = [Obstacle(Position(240, 180), sigma=sigma_obstacles), 52 | Obstacle(Position(240, 380), sigma=sigma_obstacles), 53 | Obstacle(Position(240, 580), sigma=sigma_obstacles), 54 | Obstacle(Position(440, 180), sigma=sigma_obstacles), 55 | Obstacle(Position(440, 380), sigma=sigma_obstacles), 56 | Obstacle(Position(440, 580), sigma=sigma_obstacles)] 57 | #Obstacle(Position(580, 580), sigma=sigma_obstacles),] 58 | #draw_radius=4*sigma_obstacles), 59 | ''' 60 | obstacles = [Obstacle(Position(500, 180), sigma=sigma_obstacles,direction=1), 61 | Obstacle(Position(300, 280), sigma=sigma_obstacles,direction=2), 62 | Obstacle(Position(500, 280), sigma=sigma_obstacles,direction=2), 63 | Obstacle(Position(500, 380), sigma=sigma_obstacles,direction=1), 64 | Obstacle(Position(0, 380), sigma=sigma_obstacles,direction=1), 65 | Obstacle(Position(200, 480), sigma=sigma_obstacles,direction=1), 66 | Obstacle(Position(100, 580), sigma=sigma_obstacles,direction=1), 67 | ] 68 | 69 | 70 | 71 | # Drawing objects 72 | agent.draw(image) 73 | goal.draw(image) 74 | for obstacle in obstacles: 75 | obstacle.draw(image) 76 | 77 | # Displaying initial frame and wait for intial key press 78 | cv2.imshow('Output', image) 79 | cv2.waitKey(1000) 80 | 81 | cost_weight = 20 82 | 83 | cost = 0 84 | visited_list = [] 85 | count = 0 86 | direction = 1 87 | 88 | while Position.calculate_distance(agent.position, goal.position) > 10: 89 | 90 | possible_moves = agent.get_possible_moves() 91 | min_value = math.inf 92 | best_move = possible_moves[0] # initializing best move with first move 93 | # Finding move with the least value 94 | for move in possible_moves: 95 | move_value = goal.get_attraction_force(move) 96 | for obstacle in obstacles: 97 | 98 | move_value += obstacle.get_repulsion_force_pre(move) 99 | 100 | 101 | if PtoXY(move) in visited_list: 102 | move_value += 1 103 | print("test") 104 | 105 | if move_value < min_value: 106 | min_value = move_value 107 | best_move = move 108 | 109 | visited_list.append(PtoXY(move)) 110 | #move.visit += 1 111 | 112 | #print("value_",move_value) 113 | print("best_move",best_move.x,best_move.y) 114 | #print("new move") 115 | 116 | # Setting best move as agent's next position 117 | agent.position = best_move 118 | 119 | ''' 120 | random_move 121 | if count%5 == 0: 122 | for everObstacle in obstacles: 123 | #everObstacle.randMove(image) 124 | #1-right,2-down,3-left,4-up 125 | everObstacle.move_speed(image,4,1) 126 | count += 1 127 | ''' 128 | #move speed 129 | 130 | for everObstacle in obstacles: 131 | #everObstacle.randMove(image) 132 | #1-right,2-down,3-left,4-up 133 | 134 | everObstacle.move_speed(image,everObstacle.direction,1) 135 | if (everObstacle.position.x >= 750 or everObstacle.position.y >= 750 136 | or everObstacle.position.x <0 or everObstacle.position.y <0): 137 | if (2+everObstacle.direction>4):everObstacle.direction-=2 138 | else:everObstacle.direction+=2 139 | 140 | ''' 141 | As we are not clearing up the initial frame at every iteration 142 | so we do not need to draw static objects again and again 143 | 144 | ''' 145 | agent.draw(image) 146 | flag_crash = False 147 | for everObstacle in obstacles: 148 | #everObstacle.randMove(image) 149 | if agent.position.calculate_distance(everObstacle.position) <= 10: 150 | flag_crash = True 151 | 152 | if(flag_crash): 153 | break 154 | 155 | # Displaying updated frame 156 | cv2.imshow('Output', image) 157 | k = cv2.waitKey(100) 158 | if k == 27: 159 | break 160 | 161 | # Hold on last frame 162 | cv2.waitKey(0) 163 | 164 | -------------------------------------------------------------------------------- /动态势场/objects.py: -------------------------------------------------------------------------------- 1 | """ 2 | Objects script 3 | 4 | Contains 5 | ------- 6 | Classes 7 | Agent, Obstacle and Goal 8 | 9 | Author 10 | ------- 11 | Faizan Ali(nccvector) 12 | 13 | """ 14 | 15 | import math 16 | import cv2 17 | from positional import Position 18 | import random 19 | 20 | # Agent class for Agent attributes 21 | class Agent: 22 | """ 23 | Creates an Agent object 24 | 25 | Parameters 26 | ---------- 27 | position : Position 28 | Position of agent in world 29 | scan_radius : int, optional 30 | Step size of agent, by default 1 31 | possible_moves : int, optional 32 | Number of point generated around agent, by default 6 33 | draw_radius : int, optional 34 | Radius for visualization, by default 5 35 | draw_color : tuple, optional 36 | Color for visulization, by default (255,0,0) 37 | 38 | """ 39 | 40 | def __init__(self, position, scan_radius=1, possible_moves=20, draw_radius=20, draw_color=(46,139,87)): 41 | 42 | # Property attributes 43 | self.position = position 44 | self._scan_radius = scan_radius 45 | self._possible_moves = possible_moves 46 | 47 | # Visual attributes 48 | self._draw_radius = draw_radius 49 | self._draw_color = draw_color 50 | 51 | def draw(self, image): 52 | #cv2.circle(image, (int(self.position.x), int(self.position.y)), self._draw_radius, self._draw_color, -1) # Fill 53 | x1 = int(self.position.x) 54 | y1 = int(self.position.y) 55 | cv2.rectangle(image,(x1,y1),(x1+self._draw_radius,y1+self._draw_radius),self._draw_color,-1) 56 | 57 | def get_possible_moves(self): 58 | """ 59 | Makes a list of points around agent 60 | 61 | Returns 62 | ------- 63 | list 64 | List of points around agent 65 | 66 | """ 67 | 68 | angle_increment = (2*math.pi)/self._possible_moves # 2pi/n 69 | angle = -angle_increment # Going one step negative to start from zero 70 | possible_moves_list = [] 71 | for _ in range(self._possible_moves): 72 | # Starting from angle 0 73 | angle += angle_increment 74 | possible_moves_list.append(Position(self._scan_radius * math.cos(angle) + self.position.x,self._scan_radius * math.sin(angle) + self.position.y)) 75 | #possible_moves_list.append(Position(self.position.x)) 76 | return possible_moves_list 77 | 78 | # Obstacle class for repulsion based objects 79 | class Obstacle: 80 | """ 81 | Creates an Obstacle object 82 | 83 | Parameters 84 | ---------- 85 | position : Position 86 | Position of Obstacle in the world 87 | mu : int, optional 88 | Peak of distribution, by default 1 89 | sigma : int, optional 90 | Spread of distribution, by default 1 91 | draw_radius : int, optional 92 | Radius for visualization, by default 5 93 | draw_color : tuple, optional 94 | Color for visualization, by default (0,0,255) 95 | 96 | """ 97 | 98 | def __init__(self, position, mu=1, sigma=1, draw_radius=40, draw_color=(255,99,71),direction=1): 99 | 100 | # Property attributes 101 | self.position = position 102 | self.originPosition = position 103 | self.lastposition = position 104 | self._mu = mu 105 | self.direction=direction 106 | self._sigma = sigma 107 | 108 | # Visual attributes 109 | self._draw_radius = draw_radius 110 | self._draw_color = draw_color 111 | 112 | def draw(self, image): 113 | #cv2.circle(image, (int(self.position.x), int(self.position.y)), self._draw_radius, self._draw_color, -1) # Fill 114 | x1 = int(self.position.x) 115 | y1 = int(self.position.y) 116 | 117 | cv2.rectangle(image,(x1,y1),(x1+self._draw_radius,y1+self._draw_radius),self._draw_color,-1) 118 | #line_type,shift) 119 | 120 | 121 | 122 | # Attribute type function 123 | def get_repulsion_force(self, position): 124 | """ 125 | Repulsion force calculation function 126 | 127 | Parameters 128 | ---------- 129 | position : Position 130 | Position of cell to check force at 131 | 132 | Returns 133 | ------- 134 | double 135 | The value of repulsion at cell 136 | 137 | """ 138 | 139 | # Implementing repulsion equation 140 | position_list_cal = [self.position,Position(self.position.x+20,self.position.y),Position(self.position.x,self.position.y+20),Position(self.position.x+20,self.position.y+20)] 141 | dist_value = 0 142 | for Ever_position in position_list_cal: 143 | dist_value += (1/(self._sigma*math.sqrt(2*math.pi))) * math.exp(-(Position.calculate_distance_squared(Ever_position,position)/(2*self._sigma*self._sigma))) 144 | 145 | return dist_value 146 | 147 | def get_repulsion_force_pre(self,position): 148 | print('position',self.position.x) 149 | print('lastposition ',self.lastposition.x) 150 | 151 | predict_position=self.position+self.position-self.lastposition 152 | print('predictposition ',predict_position.x) 153 | print('------------------------------------') 154 | 155 | position_list_cal = [predict_position,Position(predict_position.x+20,predict_position.y),Position(predict_position.x,predict_position.y+20),Position(predict_position.x+20,predict_position.y+20)] 156 | dist_value = 0 157 | for Ever_position in position_list_cal: 158 | dist_value += (1/(self._sigma*math.sqrt(2*math.pi))) * math.exp(-(Position.calculate_distance_squared(Ever_position,position)/(2*self._sigma*self._sigma))) 159 | 160 | return dist_value 161 | 162 | 163 | def move(self,image,toPosition): 164 | x1 = int(toPosition.x) 165 | y1 = int(toPosition.y) 166 | 167 | cv2.rectangle(image,(x1,y1),(x1+self._draw_radius,y1+self._draw_radius),self._draw_color,-1) 168 | cv2.rectangle(image,(self.position.x,self.position.y), 169 | (self.position.x+self._draw_radius,self.position.y+self._draw_radius), 170 | (255,255,255),-1) 171 | x_line = list(range(0,800,20)) 172 | y_line = list(range(0,800,20)) 173 | 174 | image[x_line,:] = [128,128,128] 175 | image[:,y_line] = [128,128,128] 176 | 177 | self.lastposition = self.position 178 | 179 | self.position = toPosition 180 | print("333333333333333333333333333333") 181 | print("lastposition",self.lastposition.x) 182 | print("position",self.position.x) 183 | print("333333333333333333333333333333") 184 | 185 | def randMove(self,image): 186 | p = random.randint(0,3) 187 | angle_increment = (2*math.pi)/4 # 2pi/n 188 | angle = -angle_increment # Going one step negative to start from zero 189 | possible_moves_list = [] 190 | for _ in range(4): 191 | # Starting from angle 0 192 | angle += angle_increment 193 | possible_moves_list.append(Position(40 * math.cos(angle) + self.position.x,40* math.sin(angle) + self.position.y)) 194 | #possible_moves_list.append(Position(self.position.x)) 195 | 196 | toPosition = possible_moves_list[p] 197 | while abs(toPosition.x- self.originPosition.x) > 80 or abs(toPosition.y- self.originPosition.y) > 80: 198 | p = random.randint(0,3) 199 | toPosition = possible_moves_list[p] 200 | print(toPosition.x,toPosition.y) 201 | self.move(image,toPosition) 202 | #self.position = toPosition 203 | 204 | def move_speed(self,image,direction,speed): 205 | angle_increment = (2*math.pi)/4 # 2pi/n 206 | angle = -angle_increment # Going one step negative to start from zero 207 | angle = angle + direction*angle_increment 208 | print(angle) 209 | 210 | toPosition = Position(40 *speed* math.cos(angle) + self.position.x,40*speed* math.sin(angle) + self.position.y) 211 | 212 | 213 | self.move(image,toPosition) 214 | 215 | 216 | 217 | 218 | 219 | 220 | 221 | 222 | 223 | 224 | # Goal class for Goal object 225 | class Goal: 226 | """ 227 | Creates a Goal object 228 | 229 | Parameters 230 | ---------- 231 | position : Position 232 | Position of Goal in the world 233 | mu : int, optional 234 | Peak of distribution, by default 1 235 | sigma : int, optional 236 | Spread of distribution, by default 1 237 | draw_radius : int, optional 238 | Radius for visualization, by default 5 239 | draw_color : tuple, optionalVisit https://qiao.github.io/PathFinding.js/visual/ 240 | Color for visualization, by default (0,255,0) 241 | 242 | """ 243 | 244 | def __init__(self, position, mu=1, sigma=1, draw_radius=20, draw_color=(128,0,0)): 245 | 246 | # Property attributes 247 | self.position = position 248 | self._mu = mu 249 | self._sigma = sigma 250 | 251 | # Visual attributes 252 | self._draw_radius = draw_radius 253 | self._draw_color = draw_color 254 | 255 | def draw(self, image): 256 | #cv2.circle(image, (int(self.position.x), int(self.position.y)), self._draw_radius, self._draw_color, -1) # Fill 257 | x1 = int(self.position.x) 258 | y1 = int(self.position.y) 259 | 260 | cv2.rectangle(image,(x1,y1),(x1+self._draw_radius,y1+self._draw_radius),self._draw_color,-1) 261 | 262 | # Attribute type function 263 | def get_attraction_force(self, position): 264 | """ 265 | Attraction force calculation function 266 | 267 | Parameters 268 | ---------- 269 | position : Position 270 | Position of cell to check force at 271 | 272 | Returns 273 | ------- 274 | double 275 | The value of attraction at cell 276 | 277 | """ 278 | 279 | # Implementing attraction equation 280 | dist_value = -(1/(self._sigma*math.sqrt(2*math.pi))) * math.exp(-(Position.calculate_distance_squared(self.position,position)/(2*self._sigma*self._sigma))) 281 | return dist_value 282 | -------------------------------------------------------------------------------- /动态势场/positional.py: -------------------------------------------------------------------------------- 1 | """ 2 | Positional script 3 | 4 | Contains 5 | ------- 6 | Class 7 | Position 8 | 9 | Author 10 | ------- 11 | Faizan Ali(nccvector) 12 | 13 | """ 14 | 15 | import math 16 | 17 | 18 | # Position class for position based calculations 19 | class Position: 20 | """ 21 | Creates a Position object 22 | 23 | Parameters 24 | ---------- 25 | x : double 26 | x coordinate 27 | y : double 28 | y coordinate 29 | 30 | """ 31 | 32 | def __init__(self, x, y): 33 | 34 | self.x = int(x) 35 | self.y = int(y) 36 | 37 | def __add__(self,other): 38 | 39 | result_x=self.x+other.x 40 | result_y=self.y+other.y 41 | 42 | return Position(result_x,result_y) 43 | def __sub__(self,other): 44 | 45 | result_x=self.x-other.x 46 | result_y=self.y-other.y 47 | 48 | return Position(result_x,result_y) 49 | # General type function 50 | def calculate_distance(self, other): 51 | """ 52 | Calculates distance between current position and the position passed as parameter 53 | It can also be usedd as static function by specifying class name and giving two parameters 54 | 55 | Parameters 56 | ---------- 57 | other : Position 58 | Position to check distance against 59 | 60 | Returns 61 | ------- 62 | double 63 | Distance value 64 | 65 | """ 66 | 67 | return math.sqrt((self.x - other.x)**2 + (self.y - other.y)**2) 68 | 69 | # General type function 70 | def calculate_distance_squared(self, other): 71 | """ 72 | Calculates squared distance between current position and the position passed as parameter 73 | It can also be usedd as static function by specifying class name and giving two parameters 74 | 75 | Parameters 76 | ---------- 77 | other : Position 78 | Position to check squared distance against 79 | 80 | Returns 81 | ------- 82 | double 83 | Distance value squared 84 | 85 | """ 86 | 87 | return (self.x - other.x)**2 + (self.y - other.y)**2 88 | 89 | def PtoXY(P): 90 | xy = (P.x,P.y) 91 | return xy 92 | -------------------------------------------------------------------------------- /多机器人协作/__pycache__/objects.cpython-36.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/dlml/artificial_potential_field_method/ea96c667d2cc68c4e71477015c64c19d21bc4544/多机器人协作/__pycache__/objects.cpython-36.pyc -------------------------------------------------------------------------------- /多机器人协作/__pycache__/positional.cpython-36.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/dlml/artificial_potential_field_method/ea96c667d2cc68c4e71477015c64c19d21bc4544/多机器人协作/__pycache__/positional.cpython-36.pyc -------------------------------------------------------------------------------- /多机器人协作/main.py: -------------------------------------------------------------------------------- 1 | """ 2 | Main script 3 | 4 | Contains 5 | ------- 6 | Functions 7 | draw and __main__ 8 | 9 | Author 10 | ------- 11 | Faizan Ali(nccvector) 12 | 13 | """ 14 | 15 | import numpy as np 16 | import cv2 17 | import math 18 | import random 19 | 20 | from positional import * 21 | from objects import * 22 | 23 | def testCrash(position1,position2): 24 | return 0 25 | 26 | def get_chang(Obstacles,goal,image): 27 | 28 | x_line = list(range(0,800,20)) 29 | y_line = list(range(0,800,20)) 30 | value_mat = np.zeros([len(x_line),len(y_line)]) 31 | #print(value_mat.shape) 32 | for x in x_line: 33 | for y in y_line: 34 | #print(x,y) 35 | position = Position(x,y) 36 | value = goal.get_attraction_force(position) 37 | for obstacle in obstacles: 38 | value += obstacle.get_repulsion_force(position) 39 | value_mat[x_line.index(x)][y_line.index(y)] = value 40 | if(x == 240 and y ==140 or 41 | x == 180 and y == 380 or 42 | x == 340 and y== 440 or 43 | x ==380 and y==380 or 44 | x == 600 and y ==440): 45 | print(value) 46 | #print(value) 47 | #print("yes") 48 | value_min = value_mat.min() 49 | value_max = value_mat.max() 50 | value_mean = np.mean(value_mat) 51 | print("max",value_max) 52 | print("min",value_min) 53 | dif = value_max- value_min 54 | print(dif) 55 | x,y =value_mat.shape 56 | #print(value_min + dif/5*1) 57 | 58 | 59 | for i in range(x): 60 | for j in range(y): 61 | if value_mat[i,j] <= value_min: 62 | value_mat[i,j] = 0 63 | #print("0") 64 | elif value_mat[i,j] <= value_min + dif/(1.5e4): 65 | value_mat[i,j] = 1 66 | #print("1") 67 | elif value_mat[i,j] <= value_min + dif/(1.5e4)*3.5: 68 | value_mat[i,j] = 2 69 | elif value_mat[i,j] <= value_min + dif/(1.5e4)*6: 70 | value_mat[i,j] = 3 71 | elif value_mat[i,j] <= value_min + dif/(1.5e4)*8.5: 72 | value_mat[i,j] = 4 73 | elif value_mat[i,j] <= value_min + dif/(1.5e4)*11: 74 | value_mat[i,j] = 5 75 | elif value_mat[i,j] <= value_min + dif/(1.5e4)*13.5: 76 | value_mat[i,j] = 6 77 | elif value_mat[i,j] <= value_min + dif/(1.5e4)*20: 78 | value_mat[i,j] = 7 79 | elif value_mat[i,j] <= 0.00: 80 | #0.06616 81 | value_mat[i,j] = 8 82 | else: 83 | value_mat[i,j] =9 84 | print(value_mat) 85 | print("end") 86 | 87 | 88 | return value_mat 89 | 90 | def draw_chang(image,value_mat): 91 | x,y =value_mat.shape 92 | for i in range(x): 93 | for j in range(y): 94 | if value_mat[i,j] <= 0: 95 | #color = (255,245,238) 96 | color = (0,255,255) 97 | elif value_mat[i,j] <=1: 98 | #color = (255,160,122) 99 | color = (0,69,255) 100 | elif value_mat[i,j] <=2: 101 | #color = (255,127,80) 102 | color = (80,127,255) 103 | elif value_mat[i,j] <=3: 104 | #color = (255,69,0) 105 | color = (0,215,255) 106 | elif value_mat[i,j] <=4: 107 | color = (0,255,255) 108 | elif value_mat[i,j] <=5: 109 | color = (47,107,85) 110 | elif value_mat[i,j] <=6: 111 | color = (87,139,46) 112 | elif value_mat[i,j] <=7: 113 | color = (170,178,32) 114 | elif value_mat[i,j] <=7: 115 | color = (128,0,128) 116 | elif value_mat[i,j] <=8: 117 | color = (79,79,46) 118 | else :color = (128,0,0) 119 | cv2.rectangle(image,(i*20,j*20),(i*20+20,j*20+20),color,-1) 120 | #print("test") 121 | #print(i,j,color) 122 | 123 | 124 | 125 | 126 | 127 | 128 | 129 | # Main 130 | if __name__ == '__main__': 131 | # Defining world dimensions 132 | #world_size = (640, 480) 133 | world_size = (800,800) 134 | # Initializing blank canvas with white color 135 | image = np.ones((world_size[1],world_size[0],3),dtype=np.uint8) * 255 136 | #background color 137 | image[:,:] = [245,245,245] 138 | 139 | #line color 140 | x_line = list(range(0,800,20)) 141 | y_line = list(range(0,800,20)) 142 | 143 | image[x_line,:] = [128,128,128] 144 | image[:,y_line] = [128,128,128] 145 | 146 | # Defining agent and goal 147 | agent = Agent(Position(80, 80), scan_radius=20, possible_moves=4,sigma=math.sqrt(world_size[0]**2 + world_size[1]**2)) 148 | byagent1 = ByAgent(Position(480, 200), scan_radius=20, possible_moves=4) 149 | #480 240 150 | #180 480 151 | byagent2 = ByAgent(Position(180, 480), scan_radius=20, possible_moves=4) 152 | byagents = [byagent1,byagent2] 153 | #540 640 154 | goal = Goal(Position(640, 740), sigma=math.sqrt(world_size[0]**2 + world_size[1]**2)) 155 | 156 | # Defining obstacles in a list 157 | # 158 | sigma_obstacles = 6 159 | ''' 160 | obstacles = [Obstacle(Position(240, 180), sigma=sigma_obstacles), 161 | Obstacle(Position(240, 380), sigma=sigma_obstacles), 162 | Obstacle(Position(240, 580), sigma=sigma_obstacles), 163 | Obstacle(Position(440, 180), sigma=sigma_obstacles), 164 | Obstacle(Position(440, 380), sigma=sigma_obstacles), 165 | Obstacle(Position(440, 580), sigma=sigma_obstacles)] 166 | #Obstacle(Position(580, 580), sigma=sigma_obstacles),] 167 | #draw_radius=4*sigma_obstacles), 168 | ''' 169 | 170 | 171 | obstacles = [Obstacle(Position(240, 140), sigma=sigma_obstacles), 172 | Obstacle(Position(180, 380), sigma=sigma_obstacles), 173 | Obstacle(Position(340, 440), sigma=sigma_obstacles), 174 | Obstacle(Position(380, 380), sigma=sigma_obstacles), 175 | Obstacle(Position(600, 440), sigma=sigma_obstacles) 176 | 177 | ] 178 | 179 | 180 | 181 | 182 | 183 | # Drawing objects 184 | agent.draw(image) 185 | byagent1.draw(image) 186 | byagent2.draw(image) 187 | 188 | goal.draw(image) 189 | for obstacle in obstacles: 190 | obstacle.draw(image) 191 | 192 | # Displaying initial frame and wait for intial key press 193 | cv2.imshow('Output', image) 194 | cv2.waitKey(1000) 195 | #1000 196 | 197 | cost_weight = 20 198 | 199 | cost = 0 200 | visited_list = [] 201 | count = 0 202 | direction = 1 203 | 204 | 205 | while Position.calculate_distance(agent.position, goal.position) > 10: 206 | 207 | 208 | #######agent########### 209 | 210 | possible_moves = agent.get_possible_moves() 211 | min_value = math.inf 212 | best_move = possible_moves[0] # initializing best move with first move 213 | # Finding move with the least value 214 | for move in possible_moves: 215 | move_value = goal.get_attraction_force(move) 216 | for obstacle in obstacles: 217 | move_value += obstacle.get_repulsion_force(move) 218 | 219 | if PtoXY(move) in visited_list: 220 | move_value + 0.3*abs(move_value) 221 | #move_value += 1 222 | print("test") 223 | 224 | if move_value < min_value: 225 | min_value = move_value 226 | best_move = move 227 | 228 | visited_list.append(PtoXY(move)) 229 | #move.visit += 1 230 | 231 | print("value_",move_value) 232 | print("best_move",best_move.x,best_move.y) 233 | #print("new move") 234 | 235 | # Setting best move as agent's next position 236 | agent.position = best_move 237 | 238 | #######Byagent########### 239 | for byagent in byagents: 240 | possible_moves_by = [] 241 | 242 | possible_moves_by = byagent.get_possible_moves() 243 | min_value_by = math.inf 244 | best_move_by = possible_moves_by[0] # initializing best move with first move 245 | # Finding move with the least value 246 | for move in possible_moves_by: 247 | move_value = agent.get_force(move) 248 | for obstacle in obstacles: 249 | move_value += obstacle.get_repulsion_force(move) 250 | 251 | if PtoXY(move) in visited_list: 252 | move_value = move_value + 0.0*abs(move_value) 253 | #0.0698 254 | print("test") 255 | 256 | if move_value < min_value: 257 | min_value = move_value 258 | best_move_by = move 259 | 260 | visited_list.append(PtoXY(move)) 261 | 262 | byagent.position = best_move_by 263 | 264 | 265 | 266 | 267 | 268 | #######obstacle########### 269 | 270 | 271 | #random_move 272 | ''' 273 | if count%5 == 0: 274 | for everObstacle in obstacles: 275 | everObstacle.randMove(image) 276 | #1-right,2-down,3-left,4-up 277 | #everObstacle.move_speed(image,4,1) 278 | count += 1 279 | ''' 280 | 281 | #move speed 282 | ''' 283 | 284 | for everObstacle in obstacles: 285 | #everObstacle.randMove(image) 286 | #1-right,2-down,3-left,4-up 287 | everObstacle.move_speed(image,direction,1) 288 | if (everObstacle.position.x >= 800 or everObstacle.position.y >= 800 289 | or everObstacle.position.x <0 or everObstacle.position.y <0): 290 | direction = 4-direction 291 | ''' 292 | 293 | ''' 294 | As we are not clearing up the initial frame at every iteration 295 | so we do not need to draw static objects again and again 296 | 297 | ''' 298 | 299 | 300 | agent.draw(image) 301 | byagents[0].draw(image) 302 | byagents[1].draw(image) 303 | flag_crash = False 304 | for everObstacle in obstacles: 305 | #everObstacle.randMove(image) 306 | if agent.position.calculate_distance(everObstacle.position) < 0: 307 | flag_crash = True 308 | 309 | if(flag_crash): 310 | break 311 | 312 | 313 | #value_mat = get_chang(obstacles,goal,image) 314 | #draw_chang(image,value_mat) 315 | 316 | # Displaying updated frame 317 | cv2.imshow('Output', image) 318 | k = cv2.waitKey(100) 319 | if k == 27: 320 | break 321 | #cv2.destroyAllWindows() 322 | 323 | 324 | 325 | 326 | 327 | 328 | # Hold on last frame 329 | cv2.waitKey(0) 330 | 331 | -------------------------------------------------------------------------------- /多机器人协作/multi.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/dlml/artificial_potential_field_method/ea96c667d2cc68c4e71477015c64c19d21bc4544/多机器人协作/multi.gif -------------------------------------------------------------------------------- /多机器人协作/objects.py: -------------------------------------------------------------------------------- 1 | """ 2 | Objects script 3 | 4 | Contains 5 | ------- 6 | Classes 7 | Agent, Obstacle and Goal 8 | 9 | Author 10 | ------- 11 | Faizan Ali(nccvector) 12 | 13 | """ 14 | 15 | import math 16 | import cv2 17 | from positional import Position 18 | import random 19 | 20 | # Agent class for Agent attributes 21 | class Agent: 22 | """ 23 | Creates an Agent object 24 | 25 | Parameters 26 | ---------- 27 | position : Position 28 | Position of agent in world 29 | scan_radius : int, optional 30 | Step size of agent, by default 1 31 | possible_moves : int, optional 32 | Number of point generated around agent, by default 6 33 | draw_radius : int, optional 34 | Radius for visualization, by default 5 35 | draw_color : tuple, optional 36 | Color for visulization, by default (255,0,0) 37 | 38 | """ 39 | 40 | def __init__(self, position, scan_radius=1, possible_moves=20, draw_radius=20, draw_color=(46,139,87),sigma = 1): 41 | 42 | # Property attributes 43 | self.position = position 44 | self._scan_radius = scan_radius 45 | self._possible_moves = possible_moves 46 | self._sigma = sigma 47 | 48 | # Visual attributes 49 | self._draw_radius = draw_radius 50 | self._draw_color = draw_color 51 | 52 | def draw(self, image): 53 | #cv2.circle(image, (int(self.position.x), int(self.position.y)), self._draw_radius, self._draw_color, -1) # Fill 54 | x1 = int(self.position.x) 55 | y1 = int(self.position.y) 56 | cv2.rectangle(image,(x1,y1),(x1+self._draw_radius,y1+self._draw_radius),self._draw_color,-1) 57 | 58 | def get_possible_moves(self): 59 | """ 60 | Makes a list of points around agent 61 | 62 | Returns 63 | ------- 64 | list 65 | List of points around agent 66 | 67 | """ 68 | 69 | angle_increment = (2*math.pi)/self._possible_moves # 2pi/n 70 | angle = -angle_increment # Going one step negative to start from zero 71 | possible_moves_list = [] 72 | for _ in range(self._possible_moves): 73 | # Starting from angle 0 74 | angle += angle_increment 75 | possible_moves_list.append(Position(self._scan_radius * math.cos(angle) + self.position.x,self._scan_radius * math.sin(angle) + self.position.y)) 76 | #possible_moves_list.append(Position(self.position.x)) 77 | return possible_moves_list 78 | 79 | def get_force(self, position): 80 | 81 | 82 | dist_value = -1.1*(1/(self._sigma*math.sqrt(2*math.pi))) * math.exp(-(Position.calculate_distance_squared(position,self.position)/(2*self._sigma*self._sigma))) 83 | ''' 84 | if self.position.calculate_distance(position) > 300: 85 | dist_value = 1.02*dist_value 86 | 87 | elif self.position.calculate_distance(position) >= 200: 88 | dist_value = 1.009*dist_value 89 | elif self.position.calculate_distance(position) < 20: 90 | #dist_value = 0.7*dist_value 91 | dist_value = -1*dist_value 92 | 93 | else : 94 | dist_value = 1.08*dist_value 95 | ''' 96 | if self.position.calculate_distance(position) < 80: 97 | dist_value = -1*dist_value 98 | 99 | print(self.position.calculate_distance(position)) 100 | #print(dist_value) 101 | 102 | 103 | return dist_value 104 | 105 | 106 | # Obstacle class for repulsion based objects 107 | 108 | 109 | class ByAgent: 110 | """ 111 | Creates an ByAgent object 112 | 113 | Parameters 114 | ---------- 115 | position : Position 116 | Position of agent in world 117 | scan_radius : int, optional 118 | Step size of agent, by default 1 119 | possible_moves : int, optional 120 | Number of point generated around agent, by default 6 121 | draw_radius : int, optional 122 | Radius for visualization, by default 5 123 | draw_color : tuple, optional 124 | Color for visulization, by default (255,0,0) 125 | 126 | """ 127 | 128 | def __init__(self, position, scan_radius=1, possible_moves=20, draw_radius=20, draw_color=(255,215,0)): 129 | 130 | # Property attributes 131 | self.position = position 132 | self._scan_radius = scan_radius 133 | self._possible_moves = possible_moves 134 | 135 | # Visual attributes 136 | self._draw_radius = draw_radius 137 | self._draw_color = draw_color 138 | 139 | def draw(self, image): 140 | #cv2.circle(image, (int(self.position.x), int(self.position.y)), self._draw_radius, self._draw_color, -1) # Fill 141 | x1 = int(self.position.x) 142 | y1 = int(self.position.y) 143 | cv2.rectangle(image,(x1,y1),(x1+self._draw_radius,y1+self._draw_radius),self._draw_color,-1) 144 | 145 | def get_possible_moves(self): 146 | """ 147 | Makes a list of points around agent 148 | 149 | Returns 150 | ------- 151 | list 152 | List of points around agent 153 | 154 | """ 155 | 156 | angle_increment = (2*math.pi)/self._possible_moves # 2pi/n 157 | angle = -angle_increment # Going one step negative to start from zero 158 | possible_moves_list = [] 159 | for _ in range(self._possible_moves): 160 | # Starting from angle 0 161 | angle += angle_increment 162 | possible_moves_list.append(Position(self._scan_radius * math.cos(angle) + self.position.x,self._scan_radius * math.sin(angle) + self.position.y)) 163 | #possible_moves_list.append(Position(self.position.x)) 164 | return possible_moves_list 165 | 166 | # Obstacle class for repulsion based objects 167 | class Obstacle: 168 | """ 169 | Creates an Obstacle object 170 | 171 | Parameters 172 | ---------- 173 | position : Position 174 | Position of Obstacle in the world 175 | mu : int, optional 176 | Peak of distribution, by default 1 177 | sigma : int, optional 178 | Spread of distribution, by default 1 179 | draw_radius : int, optional 180 | Radius for visualization, by default 5 181 | draw_color : tuple, optional 182 | Color for visualization, by default (0,0,255) 183 | 184 | """ 185 | 186 | def __init__(self, position, mu=1, sigma=1, draw_radius=40, draw_color=(255,99,71)): 187 | 188 | # Property attributes 189 | self.position = position 190 | self.originPosition = position 191 | self.lastposition = position 192 | self._mu = mu 193 | self._sigma = sigma 194 | 195 | # Visual attributes 196 | self._draw_radius = draw_radius 197 | self._draw_color = draw_color 198 | 199 | def draw(self, image): 200 | #cv2.circle(image, (int(self.position.x), int(self.position.y)), self._draw_radius, self._draw_color, -1) # Fill 201 | x1 = int(self.position.x) 202 | y1 = int(self.position.y) 203 | 204 | cv2.rectangle(image,(x1,y1),(x1+self._draw_radius,y1+self._draw_radius),self._draw_color,-1) 205 | #line_type,shift) 206 | 207 | # Attribute type function 208 | def get_repulsion_force(self, position): 209 | """ 210 | Repulsion force calculation function 211 | 212 | Parameters 213 | ---------- 214 | position : Position 215 | Position of cell to check force at 216 | 217 | Returns 218 | ------- 219 | double 220 | The value of repulsion at cell 221 | 222 | """ 223 | 224 | # Implementing repulsion equation 225 | #dist_value = 0 226 | ''' 227 | position_list_cal = [self.position,Position(self.position.x+20,self.position.y),Position(self.position.x,self.position.y+20),Position(self.position.x+20,self.position.y+20)] 228 | dist_value = 0 229 | for Ever_position in position_list_cal: 230 | dist_value += (1/(self._sigma*math.sqrt(2*math.pi))) * math.exp(-(Position.calculate_distance_squared(Ever_position,position)/(2*self._sigma*self._sigma))) 231 | ''' 232 | #center = Position(self.position.x+20,self.position.y+20) 233 | dist_value = (1/(self._sigma*math.sqrt(2*math.pi))) * math.exp(-(Position.calculate_distance_squared(self.position,position)/(2*self._sigma*self._sigma))) 234 | 235 | return dist_value 236 | def move(self,image,toPosition): 237 | x1 = int(toPosition.x) 238 | y1 = int(toPosition.y) 239 | 240 | cv2.rectangle(image,(x1,y1),(x1+self._draw_radius,y1+self._draw_radius),self._draw_color,-1) 241 | cv2.rectangle(image,(self.position.x,self.position.y), 242 | (self.position.x+self._draw_radius,self.position.y+self._draw_radius), 243 | (255,255,255),-1) 244 | x_line = list(range(0,800,20)) 245 | y_line = list(range(0,800,20)) 246 | 247 | image[x_line,:] = [128,128,128] 248 | image[:,y_line] = [128,128,128] 249 | 250 | self.lastposition = self.position 251 | 252 | self.position = toPosition 253 | 254 | 255 | def randMove(self,image): 256 | p = random.randint(0,3) 257 | angle_increment = (2*math.pi)/4 # 2pi/n 258 | angle = -angle_increment # Going one step negative to start from zero 259 | possible_moves_list = [] 260 | for _ in range(4): 261 | # Starting from angle 0 262 | angle += angle_increment 263 | possible_moves_list.append(Position(40 * math.cos(angle) + self.position.x,40* math.sin(angle) + self.position.y)) 264 | #possible_moves_list.append(Position(self.position.x)) 265 | 266 | toPosition = possible_moves_list[p] 267 | while abs(toPosition.x- self.originPosition.x) > 80 or abs(toPosition.y- self.originPosition.y) > 80: 268 | p = random.randint(0,3) 269 | toPosition = possible_moves_list[p] 270 | print(toPosition.x,toPosition.y) 271 | self.move(image,toPosition) 272 | #self.position = toPosition 273 | 274 | def move_speed(self,image,direction,speed): 275 | angle_increment = (2*math.pi)/4 # 2pi/n 276 | angle = -angle_increment # Going one step negative to start from zero 277 | angle = angle + direction*angle_increment 278 | print(angle) 279 | 280 | toPosition = Position(40 *speed* math.cos(angle) + self.position.x,40*speed* math.sin(angle) + self.position.y) 281 | 282 | 283 | self.move(image,toPosition) 284 | 285 | 286 | 287 | 288 | # Goal class for Goal object 289 | class Goal: 290 | """ 291 | Creates a Goal object 292 | 293 | Parameters 294 | ---------- 295 | position : Position 296 | Position of Goal in the world 297 | mu : int, optional 298 | Peak of distribution, by default 1 299 | sigma : int, optional 300 | Spread of distribution, by default 1 301 | draw_radius : int, optional 302 | Radius for visualization, by default 5 303 | draw_color : tuple, optionalVisit https://qiao.github.io/PathFinding.js/visual/ 304 | Color for visualization, by default (0,255,0) 305 | 306 | """ 307 | 308 | def __init__(self, position, mu=1, sigma=1, draw_radius=20, draw_color=(128,0,0)): 309 | 310 | # Property attributes 311 | self.position = position 312 | self._mu = mu 313 | self._sigma = sigma 314 | 315 | # Visual attributes 316 | self._draw_radius = draw_radius 317 | self._draw_color = draw_color 318 | 319 | def draw(self, image): 320 | #cv2.circle(image, (int(self.position.x), int(self.position.y)), self._draw_radius, self._draw_color, -1) # Fill 321 | x1 = int(self.position.x) 322 | y1 = int(self.position.y) 323 | 324 | cv2.rectangle(image,(x1,y1),(x1+self._draw_radius,y1+self._draw_radius),self._draw_color,-1) 325 | 326 | # Attribute type function 327 | def get_attraction_force(self, position): 328 | """ 329 | Attraction force calculation function 330 | 331 | Parameters 332 | ---------- 333 | position : Position 334 | Position of cell to check force at 335 | 336 | Returns 337 | ------- 338 | double 339 | The value of attraction at cell 340 | 341 | """ 342 | 343 | # Implementing attraction equation 344 | dist_value = -(1/(self._sigma*math.sqrt(2*math.pi))) * math.exp(-(Position.calculate_distance_squared(self.position,position)/(2*self._sigma*self._sigma))) 345 | return dist_value 346 | -------------------------------------------------------------------------------- /多机器人协作/positional.py: -------------------------------------------------------------------------------- 1 | """ 2 | Positional script 3 | 4 | Contains 5 | ------- 6 | Class 7 | Position 8 | 9 | Author 10 | ------- 11 | Faizan Ali(nccvector) 12 | 13 | """ 14 | 15 | import math 16 | 17 | 18 | # Position class for position based calculations 19 | class Position: 20 | """ 21 | Creates a Position object 22 | 23 | Parameters 24 | ---------- 25 | x : double 26 | x coordinate 27 | y : double 28 | y coordinate 29 | 30 | """ 31 | 32 | def __init__(self, x, y): 33 | 34 | self.x = int(x) 35 | self.y = int(y) 36 | 37 | 38 | # General type function 39 | def calculate_distance(self, other): 40 | """ 41 | Calculates distance between current position and the position passed as parameter 42 | It can also be usedd as static function by specifying class name and giving two parameters 43 | 44 | Parameters 45 | ---------- 46 | other : Position 47 | Position to check distance against 48 | 49 | Returns 50 | ------- 51 | double 52 | Distance value 53 | 54 | """ 55 | 56 | return math.sqrt((self.x - other.x)**2 + (self.y - other.y)**2) 57 | 58 | # General type function 59 | def calculate_distance_squared(self, other): 60 | """ 61 | Calculates squared distance between current position and the position passed as parameter 62 | It can also be usedd as static function by specifying class name and giving two parameters 63 | 64 | Parameters 65 | ---------- 66 | other : Position 67 | Position to check squared distance against 68 | 69 | Returns 70 | ------- 71 | double 72 | Distance value squared 73 | 74 | """ 75 | 76 | return (self.x - other.x)**2 + (self.y - other.y)**2 77 | 78 | def PtoXY(P): 79 | xy = (P.x,P.y) 80 | return xy 81 | -------------------------------------------------------------------------------- /静态势场/break_local_min1.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/dlml/artificial_potential_field_method/ea96c667d2cc68c4e71477015c64c19d21bc4544/静态势场/break_local_min1.gif -------------------------------------------------------------------------------- /静态势场/local_min.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/dlml/artificial_potential_field_method/ea96c667d2cc68c4e71477015c64c19d21bc4544/静态势场/local_min.gif -------------------------------------------------------------------------------- /静态势场/local_min1.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/dlml/artificial_potential_field_method/ea96c667d2cc68c4e71477015c64c19d21bc4544/静态势场/local_min1.gif -------------------------------------------------------------------------------- /静态势场/main.py: -------------------------------------------------------------------------------- 1 | """ 2 | Main script 3 | 4 | Contains 5 | ------- 6 | Functions 7 | draw and __main__ 8 | 9 | Author 10 | ------- 11 | Faizan Ali(nccvector) 12 | 13 | """ 14 | 15 | import numpy as np 16 | import cv2 17 | import math 18 | import random 19 | 20 | from positional import * 21 | from objects import * 22 | 23 | # Main 24 | if __name__ == '__main__': 25 | # Defining world dimensions 26 | #world_size = (640, 480) 27 | world_size = (800,800) 28 | # Initializing blank canvas with white color 29 | image = np.ones((world_size[1],world_size[0],3),dtype=np.uint8) * 255 30 | #background color 31 | image[:,:] = [245,245,245] 32 | 33 | #line color 34 | x_line = list(range(0,800,20)) 35 | y_line = list(range(0,800,20)) 36 | 37 | image[x_line,:] = [128,128,128] 38 | image[:,y_line] = [128,128,128] 39 | 40 | # Defining agent and goal 41 | agent = Agent(Position(40, 40), scan_radius=20, possible_moves=4) 42 | goal = Goal(Position(640, 640), sigma=math.sqrt(world_size[0]**2 + world_size[1]**2)) 43 | 44 | # Defining obstacles in a list 45 | sigma_obstacles = 5 46 | obstacles = [Obstacle(Position(240, 180), sigma=sigma_obstacles), 47 | Obstacle(Position(240, 280), sigma=sigma_obstacles), 48 | Obstacle(Position(240, 380), sigma=sigma_obstacles), 49 | Obstacle(Position(340, 180), sigma=sigma_obstacles), 50 | Obstacle(Position(340, 280), sigma=sigma_obstacles), 51 | Obstacle(Position(340, 380), sigma=sigma_obstacles), 52 | Obstacle(Position(580, 580), sigma=sigma_obstacles),] 53 | #draw_radius=4*sigma_obstacles), 54 | 55 | 56 | # Drawing objects 57 | agent.draw(image) 58 | goal.draw(image) 59 | for obstacle in obstacles: 60 | obstacle.draw(image) 61 | 62 | # Displaying initial frame and wait for intial key press 63 | cv2.imshow('Output', image) 64 | cv2.waitKey(1000) 65 | 66 | cost_weight = 20 67 | 68 | cost = 0 69 | visited_list = [] 70 | count = 0 71 | 72 | while Position.calculate_distance(agent.position, goal.position) > 10: 73 | 74 | possible_moves = agent.get_possible_moves() 75 | min_value = math.inf 76 | best_move = possible_moves[0] # initializing best move with first move 77 | # Finding move with the least value 78 | for move in possible_moves: 79 | move_value = goal.get_attraction_force(move) 80 | for obstacle in obstacles: 81 | move_value += obstacle.get_repulsion_force(move) 82 | 83 | if PtoXY(move) in visited_list: 84 | move_value += 1 85 | print("test") 86 | 87 | if move_value < min_value: 88 | min_value = move_value 89 | best_move = move 90 | 91 | visited_list.append(PtoXY(move)) 92 | #move.visit += 1 93 | 94 | print("value_",move_value) 95 | print("best_move",best_move.x,best_move.y) 96 | print("new move") 97 | 98 | # p = random.randint(-2,2) 99 | # if count%5 == 0: 100 | # newPosition = Position(obstacles[6].position.x + p*40,obstacles[6].position.y+ p*20) 101 | # obstacles[6].move(image,newPosition) 102 | # obstacles[6].position = newPosition 103 | # count += 1 104 | 105 | # Setting best move as agent's next position 106 | agent.position = best_move 107 | 108 | #last_positon = 109 | 110 | ''' 111 | As we are not clearing up the initial frame at every iteration 112 | so we do not need to draw static objects again and again 113 | 114 | ''' 115 | agent.draw(image) 116 | 117 | # Displaying updated frame 118 | cv2.imshow('Output', image) 119 | k = cv2.waitKey(20) 120 | if k == 27: 121 | break 122 | 123 | # Hold on last frame 124 | cv2.waitKey(0) 125 | 126 | -------------------------------------------------------------------------------- /静态势场/objects.py: -------------------------------------------------------------------------------- 1 | """ 2 | Objects script 3 | 4 | Contains 5 | ------- 6 | Classes 7 | Agent, Obstacle and Goal 8 | 9 | Author 10 | ------- 11 | Faizan Ali(nccvector) 12 | 13 | """ 14 | 15 | import math 16 | import cv2 17 | from positional import Position 18 | 19 | # Agent class for Agent attributes 20 | class Agent: 21 | """ 22 | Creates an Agent object 23 | 24 | Parameters 25 | ---------- 26 | position : Position 27 | Position of agent in world 28 | scan_radius : int, optional 29 | Step size of agent, by default 1 30 | possible_moves : int, optional 31 | Number of point generated around agent, by default 6 32 | draw_radius : int, optional 33 | Radius for visualization, by default 5 34 | draw_color : tuple, optional 35 | Color for visulization, by default (255,0,0) 36 | 37 | """ 38 | 39 | def __init__(self, position, scan_radius=1, possible_moves=20, draw_radius=20, draw_color=(46,139,87)): 40 | 41 | # Property attributes 42 | self.position = position 43 | self._scan_radius = scan_radius 44 | self._possible_moves = possible_moves 45 | 46 | # Visual attributes 47 | self._draw_radius = draw_radius 48 | self._draw_color = draw_color 49 | 50 | def draw(self, image): 51 | #cv2.circle(image, (int(self.position.x), int(self.position.y)), self._draw_radius, self._draw_color, -1) # Fill 52 | x1 = int(self.position.x) 53 | y1 = int(self.position.y) 54 | cv2.rectangle(image,(x1,y1),(x1+self._draw_radius,y1+self._draw_radius),self._draw_color,-1) 55 | 56 | def get_possible_moves(self): 57 | """ 58 | Makes a list of points around agent 59 | 60 | Returns 61 | ------- 62 | list 63 | List of points around agent 64 | 65 | """ 66 | 67 | angle_increment = (2*math.pi)/self._possible_moves # 2pi/n 68 | angle = -angle_increment # Going one step negative to start from zero 69 | possible_moves_list = [] 70 | for _ in range(self._possible_moves): 71 | # Starting from angle 0 72 | angle += angle_increment 73 | possible_moves_list.append(Position(self._scan_radius * math.cos(angle) + self.position.x,self._scan_radius * math.sin(angle) + self.position.y)) 74 | #possible_moves_list.append(Position(self.position.x)) 75 | return possible_moves_list 76 | 77 | # Obstacle class for repulsion based objects 78 | class Obstacle: 79 | """ 80 | Creates an Obstacle object 81 | 82 | Parameters 83 | ---------- 84 | position : Position 85 | Position of Obstacle in the world 86 | mu : int, optional 87 | Peak of distribution, by default 1 88 | sigma : int, optional 89 | Spread of distribution, by default 1 90 | draw_radius : int, optional 91 | Radius for visualization, by default 5 92 | draw_color : tuple, optional 93 | Color for visualization, by default (0,0,255) 94 | 95 | """ 96 | 97 | def __init__(self, position, mu=1, sigma=1, draw_radius=40, draw_color=(255,99,71)): 98 | 99 | # Property attributes 100 | self.position = position 101 | self._mu = mu 102 | self._sigma = sigma 103 | 104 | # Visual attributes 105 | self._draw_radius = draw_radius 106 | self._draw_color = draw_color 107 | 108 | def draw(self, image): 109 | #cv2.circle(image, (int(self.position.x), int(self.position.y)), self._draw_radius, self._draw_color, -1) # Fill 110 | x1 = int(self.position.x) 111 | y1 = int(self.position.y) 112 | 113 | cv2.rectangle(image,(x1,y1),(x1+self._draw_radius,y1+self._draw_radius),self._draw_color,-1) 114 | #line_type,shift) 115 | 116 | # Attribute type function 117 | def get_repulsion_force(self, position): 118 | """ 119 | Repulsion force calculation function 120 | 121 | Parameters 122 | ---------- 123 | position : Position 124 | Position of cell to check force at 125 | 126 | Returns 127 | ------- 128 | double 129 | The value of repulsion at cell 130 | 131 | """ 132 | 133 | # Implementing repulsion equation 134 | dist_value = (1/(self._sigma*math.sqrt(2*math.pi))) * math.exp(-(Position.calculate_distance_squared(self.position,position)/(2*self._sigma*self._sigma))) 135 | return dist_value 136 | def move(self,image,toPosition): 137 | x1 = int(toPosition.x) 138 | y1 = int(toPosition.y) 139 | 140 | cv2.rectangle(image,(x1,y1),(x1+self._draw_radius,y1+self._draw_radius),self._draw_color,-1) 141 | cv2.rectangle(image,(self.position.x,self.position.y), 142 | (self.position.x+self._draw_radius,self.position.y+self._draw_radius), 143 | (255,255,255),-1) 144 | x_line = list(range(0,800,20)) 145 | y_line = list(range(0,800,20)) 146 | 147 | image[x_line,:] = [128,128,128] 148 | image[:,y_line] = [128,128,128] 149 | 150 | #self.position = toPosition 151 | 152 | 153 | 154 | 155 | 156 | 157 | # Goal class for Goal object 158 | class Goal: 159 | """ 160 | Creates a Goal object 161 | 162 | Parameters 163 | ---------- 164 | position : Position 165 | Position of Goal in the world 166 | mu : int, optional 167 | Peak of distribution, by default 1 168 | sigma : int, optional 169 | Spread of distribution, by default 1 170 | draw_radius : int, optional 171 | Radius for visualization, by default 5 172 | draw_color : tuple, optionalVisit https://qiao.github.io/PathFinding.js/visual/ 173 | Color for visualization, by default (0,255,0) 174 | 175 | """ 176 | 177 | def __init__(self, position, mu=1, sigma=1, draw_radius=20, draw_color=(128,0,0)): 178 | 179 | # Property attributes 180 | self.position = position 181 | self._mu = mu 182 | self._sigma = sigma 183 | 184 | # Visual attributes 185 | self._draw_radius = draw_radius 186 | self._draw_color = draw_color 187 | 188 | def draw(self, image): 189 | #cv2.circle(image, (int(self.position.x), int(self.position.y)), self._draw_radius, self._draw_color, -1) # Fill 190 | x1 = int(self.position.x) 191 | y1 = int(self.position.y) 192 | 193 | cv2.rectangle(image,(x1,y1),(x1+self._draw_radius,y1+self._draw_radius),self._draw_color,-1) 194 | 195 | # Attribute type function 196 | def get_attraction_force(self, position): 197 | """ 198 | Attraction force calculation function 199 | 200 | Parameters 201 | ---------- 202 | position : Position 203 | Position of cell to check force at 204 | 205 | Returns 206 | ------- 207 | double 208 | The value of attraction at cell 209 | 210 | """ 211 | 212 | # Implementing attraction equation 213 | dist_value = -(1/(self._sigma*math.sqrt(2*math.pi))) * math.exp(-(Position.calculate_distance_squared(self.position,position)/(2*self._sigma*self._sigma))) 214 | return dist_value 215 | -------------------------------------------------------------------------------- /静态势场/positional.py: -------------------------------------------------------------------------------- 1 | """ 2 | Positional script 3 | 4 | Contains 5 | ------- 6 | Class 7 | Position 8 | 9 | Author 10 | ------- 11 | Faizan Ali(nccvector) 12 | 13 | """ 14 | 15 | import math 16 | 17 | 18 | # Position class for position based calculations 19 | class Position: 20 | """ 21 | Creates a Position object 22 | 23 | Parameters 24 | ---------- 25 | x : double 26 | x coordinate 27 | y : double 28 | y coordinate 29 | 30 | """ 31 | 32 | def __init__(self, x, y): 33 | 34 | self.x = x 35 | self.y = y 36 | 37 | 38 | # General type function 39 | def calculate_distance(self, other): 40 | """ 41 | Calculates distance between current position and the position passed as parameter 42 | It can also be usedd as static function by specifying class name and giving two parameters 43 | 44 | Parameters 45 | ---------- 46 | other : Position 47 | Position to check distance against 48 | 49 | Returns 50 | ------- 51 | double 52 | Distance value 53 | 54 | """ 55 | 56 | return math.sqrt((self.x - other.x)**2 + (self.y - other.y)**2) 57 | 58 | # General type function 59 | def calculate_distance_squared(self, other): 60 | """ 61 | Calculates squared distance between current position and the position passed as parameter 62 | It can also be usedd as static function by specifying class name and giving two parameters 63 | 64 | Parameters 65 | ---------- 66 | other : Position 67 | Position to check squared distance against 68 | 69 | Returns 70 | ------- 71 | double 72 | Distance value squared 73 | 74 | """ 75 | 76 | return (self.x - other.x)**2 + (self.y - other.y)**2 77 | 78 | def PtoXY(P): 79 | xy = (P.x,P.y) 80 | return xy 81 | --------------------------------------------------------------------------------