├── Dynamic Programming └── dynamicprog.py ├── Graph SLAM ├── onlineslam.py └── slam.py ├── Kalman Filter └── kalmanfilter.py ├── Particle Filter └── particlefilter.py ├── Put All Together └── alltogether.py ├── README.md ├── main.py └── models ├── field.egg └── robot.egg.pz /Dynamic Programming/dynamicprog.py: -------------------------------------------------------------------------------- 1 | #Finding the best path using Dynamic Programming with Stochastic Motion 2 | 3 | grid = [[0, 0, 0, 0], 4 | [0, 0, 0, 0], 5 | [0, 0, 0, 0], 6 | [0, 1, 1, 0]] 7 | 8 | goal = [0, len(grid[0])-1] # Goal is in top right corner 9 | 10 | delta = [[-1, 0 ], # go up 11 | [ 0, -1], # go left 12 | [ 1, 0 ], # go down 13 | [ 0, 1 ]] # go right 14 | 15 | delta_name = ['^', '<', 'v', '>'] # Use these when creating your policy grid. 16 | 17 | success_prob = 0.5 18 | failure_prob = (1.0 - success_prob)/2.0 # Probability(stepping left) = prob(stepping right) = failure_prob 19 | collision_cost = 100 20 | cost_step = 1 21 | 22 | def stochastic_value(): 23 | value = [[1000 for row in range(len(grid[0]))] for col in range(len(grid))] 24 | policy = [[' ' for row in range(len(grid[0]))] for col in range(len(grid))] 25 | change = True 26 | while change: 27 | change = False 28 | 29 | for x in range(len(grid)): 30 | for y in range(len(grid[0])): 31 | if goal[0] == x and goal[1] == y: 32 | if value[x][y] > 0: 33 | value[x][y] = 0 34 | policy[x][y] = '*' 35 | change = True 36 | 37 | elif grid[x][y] == 0: 38 | for a in range(len(delta)): 39 | 40 | v2 = cost_step 41 | 42 | for i in range(-1, 2): 43 | a2 = (a + i) % len(delta) 44 | x2 = x + delta[a2][0] 45 | y2 = y + delta[a2][1] 46 | 47 | if i == 0: 48 | p2 = success_prob 49 | else: 50 | p2 = (1.0 - success_prob) / 2.0 51 | 52 | if x2 >= 0 and x2 < len(grid) and y2 >= 0 and y2 < len(grid[0]) and grid[x2][y2] == 0: 53 | v2 += p2 * value[x2][y2] 54 | else: 55 | v2 += p2 * collision_cost 56 | 57 | if v2 < value[x][y]: 58 | change = True 59 | value[x][y] = v2 60 | policy[x][y] = delta_name[a] 61 | for i in range(len(value)): 62 | print map(lambda x:float("%.3f"%x),value[i]) 63 | for i in range(len(policy)): 64 | print policy[i] 65 | return value, policy 66 | 67 | print stochastic_value()[0] -------------------------------------------------------------------------------- /Graph SLAM/onlineslam.py: -------------------------------------------------------------------------------- 1 | # A more manageable version of graph SLAM in 2 dimensions. 2 | # 3 | # Define a function, online_slam, that takes 5 inputs: 4 | # data, N, num_landmarks, motion_noise, and 5 | # measurement_noise--just as was done in the last 6 | # programming assignment of unit 6. This function 7 | # must return TWO matrices, mu and the final Omega. 8 | # 9 | # Just as with the quiz, your matrices should have x 10 | # and y interlaced, so if there were two poses and 2 11 | # landmarks, mu would look like: 12 | # 13 | # mu = matrix([[Px0], 14 | # [Py0], 15 | # [Px1], 16 | # [Py1], 17 | # [Lx0], 18 | # [Ly0], 19 | # [Lx1], 20 | # [Ly1]]) 21 | # 22 | 23 | # ----------- 24 | # Testing 25 | # 26 | # You have two methods for testing your code. 27 | # 28 | # 1) You can make your own data with the make_data 29 | # function. Then you can run it through the 30 | # provided slam routine and check to see that your 31 | # online_slam function gives the same estimated 32 | # final robot pose and landmark positions. 33 | # 2) You can use the solution_check function at the 34 | # bottom of this document to check your code 35 | # for the two provided test cases. The grading 36 | # will be almost identical to this function, so 37 | # if you pass both test cases, you should be 38 | # marked correct on the homework. 39 | 40 | from math import * 41 | import random 42 | 43 | 44 | # ------------------------------------------------ 45 | # 46 | # this is the matrix class 47 | # we use it because it makes it easier to collect constraints in GraphSLAM 48 | # and to calculate solutions (albeit inefficiently) 49 | # 50 | 51 | class matrix: 52 | 53 | # implements basic operations of a matrix class 54 | 55 | # ------------ 56 | # 57 | # initialization - can be called with an initial matrix 58 | # 59 | 60 | def __init__(self, value = [[]]): 61 | self.value = value 62 | self.dimx = len(value) 63 | self.dimy = len(value[0]) 64 | if value == [[]]: 65 | self.dimx = 0 66 | 67 | # ----------- 68 | # 69 | # defines matrix equality - returns true if corresponding elements 70 | # in two matrices are within epsilon of each other. 71 | # 72 | 73 | def __eq__(self, other): 74 | epsilon = 0.01 75 | if self.dimx != other.dimx or self.dimy != other.dimy: 76 | return False 77 | for i in range(self.dimx): 78 | for j in range(self.dimy): 79 | if abs(self.value[i][j] - other.value[i][j]) > epsilon: 80 | return False 81 | return True 82 | 83 | def __ne__(self, other): 84 | return not (self == other) 85 | 86 | # ------------ 87 | # 88 | # makes matrix of a certain size and sets each element to zero 89 | # 90 | 91 | def zero(self, dimx, dimy): 92 | if dimy == 0: 93 | dimy = dimx 94 | # check if valid dimensions 95 | if dimx < 1 or dimy < 1: 96 | raise ValueError, "Invalid size of matrix" 97 | else: 98 | self.dimx = dimx 99 | self.dimy = dimy 100 | self.value = [[0.0 for row in range(dimy)] for col in range(dimx)] 101 | 102 | # ------------ 103 | # 104 | # makes matrix of a certain (square) size and turns matrix into identity matrix 105 | # 106 | 107 | def identity(self, dim): 108 | # check if valid dimension 109 | if dim < 1: 110 | raise ValueError, "Invalid size of matrix" 111 | else: 112 | self.dimx = dim 113 | self.dimy = dim 114 | self.value = [[0.0 for row in range(dim)] for col in range(dim)] 115 | for i in range(dim): 116 | self.value[i][i] = 1.0 117 | 118 | # ------------ 119 | # 120 | # prints out values of matrix 121 | # 122 | 123 | def show(self, txt = ''): 124 | for i in range(len(self.value)): 125 | print txt + '['+ ', '.join('%.3f'%x for x in self.value[i]) + ']' 126 | print ' ' 127 | 128 | # ------------ 129 | # 130 | # defines elmement-wise matrix addition. Both matrices must be of equal dimensions 131 | # 132 | 133 | def __add__(self, other): 134 | # check if correct dimensions 135 | if self.dimx != other.dimx or self.dimx != other.dimx: 136 | raise ValueError, "Matrices must be of equal dimension to add" 137 | else: 138 | # add if correct dimensions 139 | res = matrix() 140 | res.zero(self.dimx, self.dimy) 141 | for i in range(self.dimx): 142 | for j in range(self.dimy): 143 | res.value[i][j] = self.value[i][j] + other.value[i][j] 144 | return res 145 | 146 | # ------------ 147 | # 148 | # defines elmement-wise matrix subtraction. Both matrices must be of equal dimensions 149 | # 150 | 151 | def __sub__(self, other): 152 | # check if correct dimensions 153 | if self.dimx != other.dimx or self.dimx != other.dimx: 154 | raise ValueError, "Matrices must be of equal dimension to subtract" 155 | else: 156 | # subtract if correct dimensions 157 | res = matrix() 158 | res.zero(self.dimx, self.dimy) 159 | for i in range(self.dimx): 160 | for j in range(self.dimy): 161 | res.value[i][j] = self.value[i][j] - other.value[i][j] 162 | return res 163 | 164 | # ------------ 165 | # 166 | # defines multiplication. Both matrices must be of fitting dimensions 167 | # 168 | 169 | def __mul__(self, other): 170 | # check if correct dimensions 171 | if self.dimy != other.dimx: 172 | raise ValueError, "Matrices must be m*n and n*p to multiply" 173 | else: 174 | # multiply if correct dimensions 175 | res = matrix() 176 | res.zero(self.dimx, other.dimy) 177 | for i in range(self.dimx): 178 | for j in range(other.dimy): 179 | for k in range(self.dimy): 180 | res.value[i][j] += self.value[i][k] * other.value[k][j] 181 | return res 182 | 183 | # ------------ 184 | # 185 | # returns a matrix transpose 186 | # 187 | 188 | def transpose(self): 189 | # compute transpose 190 | res = matrix() 191 | res.zero(self.dimy, self.dimx) 192 | for i in range(self.dimx): 193 | for j in range(self.dimy): 194 | res.value[j][i] = self.value[i][j] 195 | return res 196 | 197 | # ------------ 198 | # 199 | # creates a new matrix from the existing matrix elements. 200 | # 201 | # Example: 202 | # l = matrix([[ 1, 2, 3, 4, 5], 203 | # [ 6, 7, 8, 9, 10], 204 | # [11, 12, 13, 14, 15]]) 205 | # 206 | # l.take([0, 2], [0, 2, 3]) 207 | # 208 | # results in: 209 | # 210 | # [[1, 3, 4], 211 | # [11, 13, 14]] 212 | # 213 | # 214 | # take is used to remove rows and columns from existing matrices 215 | # list1/list2 define a sequence of rows/columns that shall be taken 216 | # is no list2 is provided, then list2 is set to list1 (good for symmetric matrices) 217 | # 218 | 219 | def take(self, list1, list2 = []): 220 | if list2 == []: 221 | list2 = list1 222 | if len(list1) > self.dimx or len(list2) > self.dimy: 223 | raise ValueError, "list invalid in take()" 224 | 225 | res = matrix() 226 | res.zero(len(list1), len(list2)) 227 | for i in range(len(list1)): 228 | for j in range(len(list2)): 229 | res.value[i][j] = self.value[list1[i]][list2[j]] 230 | return res 231 | 232 | # ------------ 233 | # 234 | # creates a new matrix from the existing matrix elements. 235 | # 236 | # Example: 237 | # l = matrix([[1, 2, 3], 238 | # [4, 5, 6]]) 239 | # 240 | # l.expand(3, 5, [0, 2], [0, 2, 3]) 241 | # 242 | # results in: 243 | # 244 | # [[1, 0, 2, 3, 0], 245 | # [0, 0, 0, 0, 0], 246 | # [4, 0, 5, 6, 0]] 247 | # 248 | # expand is used to introduce new rows and columns into an existing matrix 249 | # list1/list2 are the new indexes of row/columns in which the matrix 250 | # elements are being mapped. Elements for rows and columns 251 | # that are not listed in list1/list2 252 | # will be initialized by 0.0. 253 | # 254 | 255 | def expand(self, dimx, dimy, list1, list2 = []): 256 | if list2 == []: 257 | list2 = list1 258 | if len(list1) > self.dimx or len(list2) > self.dimy: 259 | raise ValueError, "list invalid in expand()" 260 | 261 | res = matrix() 262 | res.zero(dimx, dimy) 263 | for i in range(len(list1)): 264 | for j in range(len(list2)): 265 | res.value[list1[i]][list2[j]] = self.value[i][j] 266 | return res 267 | 268 | # ------------ 269 | # 270 | # Computes the upper triangular Cholesky factorization of 271 | # a positive definite matrix. 272 | # This code is based on http://adorio-research.org/wordpress/?p=4560 273 | 274 | def Cholesky(self, ztol= 1.0e-5): 275 | 276 | res = matrix() 277 | res.zero(self.dimx, self.dimx) 278 | 279 | for i in range(self.dimx): 280 | S = sum([(res.value[k][i])**2 for k in range(i)]) 281 | d = self.value[i][i] - S 282 | if abs(d) < ztol: 283 | res.value[i][i] = 0.0 284 | else: 285 | if d < 0.0: 286 | raise ValueError, "Matrix not positive-definite" 287 | res.value[i][i] = sqrt(d) 288 | for j in range(i+1, self.dimx): 289 | S = sum([res.value[k][i] * res.value[k][j] for k in range(i)]) 290 | if abs(S) < ztol: 291 | S = 0.0 292 | res.value[i][j] = (self.value[i][j] - S)/res.value[i][i] 293 | return res 294 | 295 | # ------------ 296 | # 297 | # Computes inverse of matrix given its Cholesky upper Triangular 298 | # decomposition of matrix. 299 | # This code is based on http://adorio-research.org/wordpress/?p=4560 300 | 301 | def CholeskyInverse(self): 302 | # Computes inverse of matrix given its Cholesky upper Triangular 303 | # decomposition of matrix. 304 | # This code is based on http://adorio-research.org/wordpress/?p=4560 305 | 306 | res = matrix() 307 | res.zero(self.dimx, self.dimx) 308 | 309 | # Backward step for inverse. 310 | for j in reversed(range(self.dimx)): 311 | tjj = self.value[j][j] 312 | S = sum([self.value[j][k]*res.value[j][k] for k in range(j+1, self.dimx)]) 313 | res.value[j][j] = 1.0/ tjj**2 - S/ tjj 314 | for i in reversed(range(j)): 315 | res.value[j][i] = res.value[i][j] = \ 316 | -sum([self.value[i][k]*res.value[k][j] for k in \ 317 | range(i+1,self.dimx)])/self.value[i][i] 318 | return res 319 | 320 | # ------------ 321 | # 322 | # comutes and returns the inverse of a square matrix 323 | # 324 | 325 | def inverse(self): 326 | aux = self.Cholesky() 327 | res = aux.CholeskyInverse() 328 | return res 329 | 330 | # ------------ 331 | # 332 | # prints matrix (needs work!) 333 | # 334 | 335 | def __repr__(self): 336 | return repr(self.value) 337 | 338 | # ###################################################################### 339 | 340 | # ------------------------------------------------ 341 | # 342 | # this is the robot class 343 | # 344 | # our robot lives in x-y space, and its motion is 345 | # pointed in a random direction. It moves on a straight line 346 | # until is comes close to a wall at which point it turns 347 | # away from the wall and continues to move. 348 | # 349 | # For measurements, it simply senses the x- and y-distance 350 | # to landmarks. This is different from range and bearing as 351 | # commonly studies in the literature, but this makes it much 352 | # easier to implement the essentials of SLAM without 353 | # cluttered math 354 | # 355 | 356 | class robot: 357 | 358 | # -------- 359 | # init: 360 | # creates robot and initializes location to 0, 0 361 | # 362 | 363 | def __init__(self, world_size = 100.0, measurement_range = 30.0, 364 | motion_noise = 1.0, measurement_noise = 1.0): 365 | self.measurement_noise = 0.0 366 | self.world_size = world_size 367 | self.measurement_range = measurement_range 368 | self.x = world_size / 2.0 369 | self.y = world_size / 2.0 370 | self.motion_noise = motion_noise 371 | self.measurement_noise = measurement_noise 372 | self.landmarks = [] 373 | self.num_landmarks = 0 374 | 375 | 376 | def rand(self): 377 | return random.random() * 2.0 - 1.0 378 | 379 | # -------- 380 | # 381 | # make random landmarks located in the world 382 | # 383 | 384 | def make_landmarks(self, num_landmarks): 385 | self.landmarks = [] 386 | for i in range(num_landmarks): 387 | self.landmarks.append([round(random.random() * self.world_size), 388 | round(random.random() * self.world_size)]) 389 | self.num_landmarks = num_landmarks 390 | 391 | # -------- 392 | # 393 | # move: attempts to move robot by dx, dy. If outside world 394 | # boundary, then the move does nothing and instead returns failure 395 | # 396 | 397 | def move(self, dx, dy): 398 | 399 | x = self.x + dx + self.rand() * self.motion_noise 400 | y = self.y + dy + self.rand() * self.motion_noise 401 | 402 | if x < 0.0 or x > self.world_size or y < 0.0 or y > self.world_size: 403 | return False 404 | else: 405 | self.x = x 406 | self.y = y 407 | return True 408 | 409 | # -------- 410 | # 411 | # sense: returns x- and y- distances to landmarks within visibility range 412 | # because not all landmarks may be in this range, the list of measurements 413 | # is of variable length. Set measurement_range to -1 if you want all 414 | # landmarks to be visible at all times 415 | # 416 | 417 | def sense(self): 418 | Z = [] 419 | for i in range(self.num_landmarks): 420 | dx = self.landmarks[i][0] - self.x + self.rand() * self.measurement_noise 421 | dy = self.landmarks[i][1] - self.y + self.rand() * self.measurement_noise 422 | if self.measurement_range < 0.0 or abs(dx) + abs(dy) <= self.measurement_range: 423 | Z.append([i, dx, dy]) 424 | return Z 425 | 426 | # -------- 427 | # 428 | # print robot location 429 | # 430 | 431 | def __repr__(self): 432 | return 'Robot: [x=%.5f y=%.5f]' % (self.x, self.y) 433 | 434 | 435 | # ###################################################################### 436 | 437 | # -------- 438 | # this routine makes the robot data 439 | # 440 | 441 | def make_data(N, num_landmarks, world_size, measurement_range, motion_noise, 442 | measurement_noise, distance): 443 | 444 | complete = False 445 | 446 | while not complete: 447 | 448 | data = [] 449 | 450 | # make robot and landmarks 451 | r = robot(world_size, measurement_range, motion_noise, measurement_noise) 452 | r.make_landmarks(num_landmarks) 453 | seen = [False for row in range(num_landmarks)] 454 | 455 | # guess an initial motion 456 | orientation = random.random() * 2.0 * pi 457 | dx = cos(orientation) * distance 458 | dy = sin(orientation) * distance 459 | 460 | for k in range(N-1): 461 | 462 | # sense 463 | Z = r.sense() 464 | 465 | # check off all landmarks that were observed 466 | for i in range(len(Z)): 467 | seen[Z[i][0]] = True 468 | 469 | # move 470 | while not r.move(dx, dy): 471 | # if we'd be leaving the robot world, pick instead a new direction 472 | orientation = random.random() * 2.0 * pi 473 | dx = cos(orientation) * distance 474 | dy = sin(orientation) * distance 475 | 476 | # memorize data 477 | data.append([Z, [dx, dy]]) 478 | 479 | # we are done when all landmarks were observed; otherwise re-run 480 | complete = (sum(seen) == num_landmarks) 481 | 482 | print ' ' 483 | print 'Landmarks: ', r.landmarks 484 | print r 485 | 486 | return data 487 | 488 | # ###################################################################### 489 | 490 | # -------------------------------- 491 | # 492 | # full_slam - retains entire path and all landmarks 493 | # Feel free to use this for comparison. 494 | # 495 | 496 | def slam(data, N, num_landmarks, motion_noise, measurement_noise): 497 | 498 | # Set the dimension of the filter 499 | dim = 2 * (N + num_landmarks) 500 | 501 | # make the constraint information matrix and vector 502 | Omega = matrix() 503 | Omega.zero(dim, dim) 504 | Omega.value[0][0] = 1.0 505 | Omega.value[1][1] = 1.0 506 | 507 | Xi = matrix() 508 | Xi.zero(dim, 1) 509 | Xi.value[0][0] = world_size / 2.0 510 | Xi.value[1][0] = world_size / 2.0 511 | 512 | # process the data 513 | 514 | for k in range(len(data)): 515 | 516 | # n is the index of the robot pose in the matrix/vector 517 | n = k * 2 518 | 519 | measurement = data[k][0] 520 | motion = data[k][1] 521 | 522 | # integrate the measurements 523 | for i in range(len(measurement)): 524 | 525 | # m is the index of the landmark coordinate in the matrix/vector 526 | m = 2 * (N + measurement[i][0]) 527 | 528 | # update the information maxtrix/vector based on the measurement 529 | for b in range(2): 530 | Omega.value[n+b][n+b] += 1.0 / measurement_noise 531 | Omega.value[m+b][m+b] += 1.0 / measurement_noise 532 | Omega.value[n+b][m+b] += -1.0 / measurement_noise 533 | Omega.value[m+b][n+b] += -1.0 / measurement_noise 534 | Xi.value[n+b][0] += -measurement[i][1+b] / measurement_noise 535 | Xi.value[m+b][0] += measurement[i][1+b] / measurement_noise 536 | 537 | 538 | # update the information maxtrix/vector based on the robot motion 539 | for b in range(4): 540 | Omega.value[n+b][n+b] += 1.0 / motion_noise 541 | for b in range(2): 542 | Omega.value[n+b ][n+b+2] += -1.0 / motion_noise 543 | Omega.value[n+b+2][n+b ] += -1.0 / motion_noise 544 | Xi.value[n+b ][0] += -motion[b] / motion_noise 545 | Xi.value[n+b+2][0] += motion[b] / motion_noise 546 | 547 | # compute best estimate 548 | mu = Omega.inverse() * Xi 549 | 550 | # return the result 551 | return mu 552 | 553 | # -------------------------------- 554 | # 555 | # online_slam - retains all landmarks but only most recent robot pose 556 | # 557 | 558 | def online_slam(data, N, num_landmarks, motion_noise, measurement_noise): 559 | # 560 | # 561 | # Enter your code here! 562 | # 563 | # 564 | dim = 2 * (1 + num_landmarks) 565 | 566 | # make the constraint information matrix and vector 567 | Omega = matrix() 568 | Omega.zero(dim, dim) 569 | Omega.value[0][0] = 1.0 570 | Omega.value[1][1] = 1.0 571 | 572 | Xi = matrix() 573 | Xi.zero(dim, 1) 574 | Xi.value[0][0] = world_size / 2.0 575 | Xi.value[1][0] = world_size / 2.0 576 | 577 | #process the data 578 | 579 | 580 | for k in range(len(data)): 581 | 582 | measurement = data[k][0] 583 | motion = data[k][1] 584 | 585 | # integrate the measurements 586 | for i in range(len(measurement)): 587 | 588 | # m is the index of the landmark coordinate in the matrix/vector 589 | m = 2 * (1 + measurement[i][0]) 590 | 591 | # update the information maxtrix/vector based on the measurement 592 | for b in range(2): 593 | Omega.value[b][b] += 1.0 / measurement_noise 594 | Omega.value[m+b][m+b] += 1.0 / measurement_noise 595 | Omega.value[b][m+b] += -1.0 / measurement_noise 596 | Omega.value[m+b][b] += -1.0 / measurement_noise 597 | Xi.value[b][0] += -measurement[i][1+b] / measurement_noise 598 | Xi.value[m+b][0] += measurement[i][1+b] / measurement_noise 599 | 600 | 601 | # expand the information matrix and vector by one new position 602 | 603 | list = [0, 1] + range(4,dim+2) 604 | Omega = Omega.expand(dim+2, dim+2, list, list) 605 | Xi = Xi.expand(dim+2, 1, list, [0]) 606 | 607 | # update the information maxtrix/vector based on the robot motion 608 | for b in range(4): 609 | Omega.value[b][b] += 1.0 / motion_noise 610 | for b in range(2): 611 | Omega.value[b ][b+2] += -1.0 / motion_noise 612 | Omega.value[b+2][b ] += -1.0 / motion_noise 613 | Xi.value[b ][0] += -motion[b] / motion_noise 614 | Xi.value[b+2][0] += motion[b] / motion_noise 615 | 616 | # now factor out the previous pose 617 | newlist = range(2, len(Omega.value)) 618 | a = Omega.take([0, 1], newlist) 619 | b = Omega.take([0, 1]) 620 | c = Xi.take([0, 1], [0]) 621 | Omega = Omega.take(newlist) - a.transpose() * b.inverse() * a 622 | Xi = Xi.take(newlist, [0]) - a.transpose() * b.inverse() * c 623 | 624 | # compute best estimate 625 | 626 | mu = Omega.inverse() * Xi 627 | 628 | 629 | 630 | 631 | 632 | return mu, Omega # make sure you return both of these matrices to be marked correct. 633 | 634 | # -------------------------------- 635 | # 636 | # print the result of SLAM, the robot pose(s) and the landmarks 637 | # 638 | 639 | def print_result(N, num_landmarks, result): 640 | print 641 | print 'Estimated Pose(s):' 642 | for i in range(N): 643 | print ' ['+ ', '.join('%.3f'%x for x in result.value[2*i]) + ', ' \ 644 | + ', '.join('%.3f'%x for x in result.value[2*i+1]) +']' 645 | print 646 | print 'Estimated Landmarks:' 647 | for i in range(num_landmarks): 648 | print ' ['+ ', '.join('%.3f'%x for x in result.value[2*(N+i)]) + ', ' \ 649 | + ', '.join('%.3f'%x for x in result.value[2*(N+i)+1]) +']' 650 | 651 | # ------------------------------------------------------------------------ 652 | # 653 | # Main routines 654 | # 655 | 656 | num_landmarks = 5 # number of landmarks 657 | N = 20 # time steps 658 | world_size = 100.0 # size of world 659 | measurement_range = 50.0 # range at which we can sense landmarks 660 | motion_noise = 2.0 # noise in robot motion 661 | measurement_noise = 2.0 # noise in the measurements 662 | distance = 20.0 # distance by which robot (intends to) move each iteratation 663 | 664 | 665 | # Uncomment the following three lines to run the full slam routine. 666 | 667 | #data = make_data(N, num_landmarks, world_size, measurement_range, motion_noise, measurement_noise, distance) 668 | #result = slam(data, N, num_landmarks, motion_noise, measurement_noise) 669 | #print_result(N, num_landmarks, result) 670 | 671 | # Uncomment the following three lines to run the online_slam routine. 672 | 673 | #data = make_data(N, num_landmarks, world_size, measurement_range, motion_noise, measurement_noise, distance) 674 | #result = online_slam(data, N, num_landmarks, motion_noise, measurement_noise) 675 | #print_result(1, num_landmarks, result[0]) 676 | 677 | ########################################################## 678 | 679 | # ------------ 680 | # TESTING 681 | # 682 | # Uncomment one of the test cases below to check that your 683 | # online_slam function works as expected. 684 | 685 | def solution_check(result, answer_mu, answer_omega): 686 | 687 | if len(result) != 2: 688 | print "Your function must return TWO matrices, mu and Omega" 689 | return False 690 | 691 | user_mu = result[0] 692 | user_omega = result[1] 693 | 694 | if user_mu.dimx == answer_omega.dimx and user_mu.dimy == answer_omega.dimy: 695 | print "It looks like you returned your results in the wrong order. Make sure to return mu then Omega." 696 | return False 697 | 698 | if user_mu.dimx != answer_mu.dimx or user_mu.dimy != answer_mu.dimy: 699 | print "Your mu matrix doesn't have the correct dimensions. Mu should be a", answer_mu.dimx, " x ", answer_mu.dimy, "matrix." 700 | return False 701 | else: 702 | print "Mu has correct dimensions." 703 | 704 | if user_omega.dimx != answer_omega.dimx or user_omega.dimy != answer_omega.dimy: 705 | print "Your Omega matrix doesn't have the correct dimensions. Omega should be a", answer_omega.dimx, " x ", answer_omega.dimy, "matrix." 706 | return False 707 | else: 708 | print "Omega has correct dimensions." 709 | 710 | if user_mu != answer_mu: 711 | print "Mu has incorrect entries." 712 | return False 713 | else: 714 | print "Mu correct." 715 | 716 | if user_omega != answer_omega: 717 | print "Omega has incorrect entries." 718 | return False 719 | else: 720 | print "Omega correct." 721 | 722 | print "Test case passed!" 723 | return True 724 | 725 | # ----------- 726 | # Test Case 1 727 | 728 | testdata1 = [[[[1, 21.796713239511305, 25.32184135169971], [2, 15.067410969755826, -27.599928007267906]], [16.4522379034509, -11.372065246394495]], 729 | [[[1, 6.1286996178786755, 35.70844618389858], [2, -0.7470113490937167, -17.709326161950294]], [16.4522379034509, -11.372065246394495]], 730 | [[[0, 16.305692184072235, -11.72765549112342], [2, -17.49244296888888, -5.371360408288514]], [16.4522379034509, -11.372065246394495]], 731 | [[[0, -0.6443452578030207, -2.542378369361001], [2, -32.17857547483552, 6.778675958806988]], [-16.66697847355152, 11.054945886894709]]] 732 | 733 | answer_mu1 = matrix([[81.63549976607898], 734 | [27.175270706192254], 735 | [98.09737507003692], 736 | [14.556272940621195], 737 | [71.97926631050574], 738 | [75.07644206765099], 739 | [65.30397603859097], 740 | [22.150809430682695]]) 741 | 742 | answer_omega1 = matrix([[0.36603773584905663, 0.0, -0.169811320754717, 0.0, -0.011320754716981133, 0.0, -0.1811320754716981, 0.0], 743 | [0.0, 0.36603773584905663, 0.0, -0.169811320754717, 0.0, -0.011320754716981133, 0.0, -0.1811320754716981], 744 | [-0.169811320754717, 0.0, 0.6509433962264151, 0.0, -0.05660377358490567, 0.0, -0.40566037735849064, 0.0], 745 | [0.0, -0.169811320754717, 0.0, 0.6509433962264151, 0.0, -0.05660377358490567, 0.0, -0.40566037735849064], 746 | [-0.011320754716981133, 0.0, -0.05660377358490567, 0.0, 0.6962264150943396, 0.0, -0.360377358490566, 0.0], 747 | [0.0, -0.011320754716981133, 0.0, -0.05660377358490567, 0.0, 0.6962264150943396, 0.0, -0.360377358490566], 748 | [-0.1811320754716981, 0.0, -0.4056603773584906, 0.0, -0.360377358490566, 0.0, 1.2339622641509433, 0.0], 749 | [0.0, -0.1811320754716981, 0.0, -0.4056603773584906, 0.0, -0.360377358490566, 0.0, 1.2339622641509433]]) 750 | 751 | #result = online_slam(testdata1, 5, 3, 2.0, 2.0) 752 | #solution_check(result, answer_mu1, answer_omega1) 753 | 754 | 755 | # ----------- 756 | # Test Case 2 757 | 758 | testdata2 = [[[[0, 12.637647070797396, 17.45189715769647], [1, 10.432982633935133, -25.49437383412288]], [17.232472057089492, 10.150955955063045]], 759 | [[[0, -4.104607680013634, 11.41471295488775], [1, -2.6421937245699176, -30.500310738397154]], [17.232472057089492, 10.150955955063045]], 760 | [[[0, -27.157759429499166, -1.9907376178358271], [1, -23.19841267128686, -43.2248146183254]], [-17.10510363812527, 10.364141523975523]], 761 | [[[0, -2.7880265859173763, -16.41914969572965], [1, -3.6771540967943794, -54.29943770172535]], [-17.10510363812527, 10.364141523975523]], 762 | [[[0, 10.844236516370763, -27.19190207903398], [1, 14.728670653019343, -63.53743222490458]], [14.192077112147086, -14.09201714598981]]] 763 | 764 | answer_mu2 = matrix([[63.37479912250136], 765 | [78.17644539069596], 766 | [61.33207502170053], 767 | [67.10699675357239], 768 | [62.57455560221361], 769 | [27.042758786080363]]) 770 | 771 | answer_omega2 = matrix([[0.22871751620895048, 0.0, -0.11351536555795691, 0.0, -0.11351536555795691, 0.0], 772 | [0.0, 0.22871751620895048, 0.0, -0.11351536555795691, 0.0, -0.11351536555795691], 773 | [-0.11351536555795691, 0.0, 0.7867205207948973, 0.0, -0.46327947920510265, 0.0], 774 | [0.0, -0.11351536555795691, 0.0, 0.7867205207948973, 0.0, -0.46327947920510265], 775 | [-0.11351536555795691, 0.0, -0.46327947920510265, 0.0, 0.7867205207948973, 0.0], 776 | [0.0, -0.11351536555795691, 0.0, -0.46327947920510265, 0.0, 0.7867205207948973]]) 777 | 778 | #result = online_slam(testdata2, 6, 2, 3.0, 4.0) 779 | #solution_check(result, answer_mu2, answer_omega2) 780 | 781 | 782 | -------------------------------------------------------------------------------- /Graph SLAM/slam.py: -------------------------------------------------------------------------------- 1 | #SLAM: Simultaneous Localization and Mapping 2 | #Builds 2-D, 3D maps 3 | 4 | #Algorithm: Graph SLAM 5 | 6 | # SLAM in a 2 dimensional 7 | # world. Please define a function, slam, which takes five 8 | # parameters as input and returns the vector mu. This vector 9 | # should have x, y coordinates interlaced, so for example, 10 | # if there were 2 poses and 2 landmarks, mu would look like: 11 | # 12 | # mu = matrix([[Px0], 13 | # [Py0], 14 | # [Px1], 15 | # [Py1], 16 | # [Lx0], 17 | # [Ly0], 18 | # [Lx1], 19 | # [Ly1]]) 20 | # 21 | # data - This is the data that is generated with the included 22 | # make_data function. You can also use test_data to 23 | # make sure your function gives the correct result. 24 | # 25 | # N - The number of time steps. 26 | # 27 | # num_landmarks - The number of landmarks. 28 | # 29 | # motion_noise - The noise associated with motion. The update 30 | # strength for motion should be 1.0 / motion_noise. 31 | # 32 | # measurement_noise - The noise associated with measurement. 33 | # The update strength for measurement should be 34 | # 1.0 / measurement_noise. 35 | # 36 | # 37 | 38 | # -------------- 39 | # Testing 40 | # 41 | # Uncomment the test cases at the bottom of this document. 42 | # Your output should be identical to the given results. 43 | 44 | 45 | from math import * 46 | import random 47 | 48 | 49 | #=============================================================== 50 | # 51 | # SLAM in a rectolinear world (we avoid non-linearities) 52 | # 53 | # 54 | #=============================================================== 55 | 56 | 57 | # ------------------------------------------------ 58 | # 59 | # this is the matrix class 60 | # we use it because it makes it easier to collect constraints in GraphSLAM 61 | # and to calculate solutions (albeit inefficiently) 62 | # 63 | 64 | class matrix: 65 | 66 | # implements basic operations of a matrix class 67 | 68 | # ------------ 69 | # 70 | # initialization - can be called with an initial matrix 71 | # 72 | 73 | def __init__(self, value = [[]]): 74 | self.value = value 75 | self.dimx = len(value) 76 | self.dimy = len(value[0]) 77 | if value == [[]]: 78 | self.dimx = 0 79 | 80 | # ------------ 81 | # 82 | # makes matrix of a certain size and sets each element to zero 83 | # 84 | 85 | def zero(self, dimx, dimy): 86 | if dimy == 0: 87 | dimy = dimx 88 | # check if valid dimensions 89 | if dimx < 1 or dimy < 1: 90 | raise ValueError, "Invalid size of matrix" 91 | else: 92 | self.dimx = dimx 93 | self.dimy = dimy 94 | self.value = [[0.0 for row in range(dimy)] for col in range(dimx)] 95 | 96 | # ------------ 97 | # 98 | # makes matrix of a certain (square) size and turns matrix into identity matrix 99 | # 100 | 101 | def identity(self, dim): 102 | # check if valid dimension 103 | if dim < 1: 104 | raise ValueError, "Invalid size of matrix" 105 | else: 106 | self.dimx = dim 107 | self.dimy = dim 108 | self.value = [[0.0 for row in range(dim)] for col in range(dim)] 109 | for i in range(dim): 110 | self.value[i][i] = 1.0 111 | # ------------ 112 | # 113 | # prints out values of matrix 114 | # 115 | 116 | def show(self, txt = ''): 117 | for i in range(len(self.value)): 118 | print txt + '['+ ', '.join('%.3f'%x for x in self.value[i]) + ']' 119 | print ' ' 120 | 121 | # ------------ 122 | # 123 | # defines elmement-wise matrix addition. Both matrices must be of equal dimensions 124 | # 125 | 126 | def __add__(self, other): 127 | # check if correct dimensions 128 | if self.dimx != other.dimx or self.dimx != other.dimx: 129 | raise ValueError, "Matrices must be of equal dimension to add" 130 | else: 131 | # add if correct dimensions 132 | res = matrix() 133 | res.zero(self.dimx, self.dimy) 134 | for i in range(self.dimx): 135 | for j in range(self.dimy): 136 | res.value[i][j] = self.value[i][j] + other.value[i][j] 137 | return res 138 | 139 | # ------------ 140 | # 141 | # defines elmement-wise matrix subtraction. Both matrices must be of equal dimensions 142 | # 143 | 144 | def __sub__(self, other): 145 | # check if correct dimensions 146 | if self.dimx != other.dimx or self.dimx != other.dimx: 147 | raise ValueError, "Matrices must be of equal dimension to subtract" 148 | else: 149 | # subtract if correct dimensions 150 | res = matrix() 151 | res.zero(self.dimx, self.dimy) 152 | for i in range(self.dimx): 153 | for j in range(self.dimy): 154 | res.value[i][j] = self.value[i][j] - other.value[i][j] 155 | return res 156 | 157 | # ------------ 158 | # 159 | # defines multiplication. Both matrices must be of fitting dimensions 160 | # 161 | 162 | def __mul__(self, other): 163 | # check if correct dimensions 164 | if self.dimy != other.dimx: 165 | raise ValueError, "Matrices must be m*n and n*p to multiply" 166 | else: 167 | # multiply if correct dimensions 168 | res = matrix() 169 | res.zero(self.dimx, other.dimy) 170 | for i in range(self.dimx): 171 | for j in range(other.dimy): 172 | for k in range(self.dimy): 173 | res.value[i][j] += self.value[i][k] * other.value[k][j] 174 | return res 175 | 176 | 177 | # ------------ 178 | # 179 | # returns a matrix transpose 180 | # 181 | 182 | def transpose(self): 183 | # compute transpose 184 | res = matrix() 185 | res.zero(self.dimy, self.dimx) 186 | for i in range(self.dimx): 187 | for j in range(self.dimy): 188 | res.value[j][i] = self.value[i][j] 189 | return res 190 | 191 | # ------------ 192 | # 193 | # creates a new matrix from the existing matrix elements. 194 | # 195 | # Example: 196 | # l = matrix([[ 1, 2, 3, 4, 5], 197 | # [ 6, 7, 8, 9, 10], 198 | # [11, 12, 13, 14, 15]]) 199 | # 200 | # l.take([0, 2], [0, 2, 3]) 201 | # 202 | # results in: 203 | # 204 | # [[1, 3, 4], 205 | # [11, 13, 14]] 206 | # 207 | # 208 | # take is used to remove rows and columns from existing matrices 209 | # list1/list2 define a sequence of rows/columns that shall be taken 210 | # is no list2 is provided, then list2 is set to list1 (good for 211 | # symmetric matrices) 212 | # 213 | 214 | def take(self, list1, list2 = []): 215 | if list2 == []: 216 | list2 = list1 217 | if len(list1) > self.dimx or len(list2) > self.dimy: 218 | raise ValueError, "list invalid in take()" 219 | 220 | res = matrix() 221 | res.zero(len(list1), len(list2)) 222 | for i in range(len(list1)): 223 | for j in range(len(list2)): 224 | res.value[i][j] = self.value[list1[i]][list2[j]] 225 | return res 226 | 227 | # ------------ 228 | # 229 | # creates a new matrix from the existing matrix elements. 230 | # 231 | # Example: 232 | # l = matrix([[1, 2, 3], 233 | # [4, 5, 6]]) 234 | # 235 | # l.expand(3, 5, [0, 2], [0, 2, 3]) 236 | # 237 | # results in: 238 | # 239 | # [[1, 0, 2, 3, 0], 240 | # [0, 0, 0, 0, 0], 241 | # [4, 0, 5, 6, 0]] 242 | # 243 | # expand is used to introduce new rows and columns into an existing matrix 244 | # list1/list2 are the new indexes of row/columns in which the matrix 245 | # elements are being mapped. Elements for rows and columns 246 | # that are not listed in list1/list2 247 | # will be initialized by 0.0. 248 | # 249 | 250 | def expand(self, dimx, dimy, list1, list2 = []): 251 | if list2 == []: 252 | list2 = list1 253 | if len(list1) > self.dimx or len(list2) > self.dimy: 254 | raise ValueError, "list invalid in expand()" 255 | 256 | res = matrix() 257 | res.zero(dimx, dimy) 258 | for i in range(len(list1)): 259 | for j in range(len(list2)): 260 | res.value[list1[i]][list2[j]] = self.value[i][j] 261 | return res 262 | 263 | # ------------ 264 | # 265 | # Computes the upper triangular Cholesky factorization of 266 | # a positive definite matrix. 267 | # This code is based on http://adorio-research.org/wordpress/?p=4560 268 | # 269 | 270 | def Cholesky(self, ztol= 1.0e-5): 271 | 272 | res = matrix() 273 | res.zero(self.dimx, self.dimx) 274 | 275 | for i in range(self.dimx): 276 | S = sum([(res.value[k][i])**2 for k in range(i)]) 277 | d = self.value[i][i] - S 278 | if abs(d) < ztol: 279 | res.value[i][i] = 0.0 280 | else: 281 | if d < 0.0: 282 | raise ValueError, "Matrix not positive-definite" 283 | res.value[i][i] = sqrt(d) 284 | for j in range(i+1, self.dimx): 285 | S = sum([res.value[k][i] * res.value[k][j] for k in range(i)]) 286 | if abs(S) < ztol: 287 | S = 0.0 288 | res.value[i][j] = (self.value[i][j] - S)/res.value[i][i] 289 | return res 290 | 291 | # ------------ 292 | # 293 | # Computes inverse of matrix given its Cholesky upper Triangular 294 | # decomposition of matrix. 295 | # This code is based on http://adorio-research.org/wordpress/?p=4560 296 | # 297 | 298 | def CholeskyInverse(self): 299 | 300 | res = matrix() 301 | res.zero(self.dimx, self.dimx) 302 | 303 | # Backward step for inverse. 304 | for j in reversed(range(self.dimx)): 305 | tjj = self.value[j][j] 306 | S = sum([self.value[j][k]*res.value[j][k] for k in range(j+1, self.dimx)]) 307 | res.value[j][j] = 1.0/ tjj**2 - S/ tjj 308 | for i in reversed(range(j)): 309 | res.value[j][i] = res.value[i][j] = \ 310 | -sum([self.value[i][k]*res.value[k][j] for k in \ 311 | range(i+1,self.dimx)])/self.value[i][i] 312 | return res 313 | 314 | # ------------ 315 | # 316 | # comutes and returns the inverse of a square matrix 317 | # 318 | def inverse(self): 319 | aux = self.Cholesky() 320 | res = aux.CholeskyInverse() 321 | return res 322 | 323 | # ------------ 324 | # 325 | # prints matrix (needs work!) 326 | # 327 | def __repr__(self): 328 | return repr(self.value) 329 | 330 | # ------------------------------------------------ 331 | # 332 | # this is the robot class 333 | # 334 | # our robot lives in x-y space, and its motion is 335 | # pointed in a random direction. It moves on a straight line 336 | # until is comes close to a wall at which point it turns 337 | # away from the wall and continues to move. 338 | # 339 | # For measurements, it simply senses the x- and y-distance 340 | # to landmarks. This is different from range and bearing as 341 | # commonly studied in the literature, but this makes it much 342 | # easier to implement the essentials of SLAM without 343 | # cluttered math 344 | # 345 | 346 | class robot: 347 | 348 | # -------- 349 | # init: 350 | # creates robot and initializes location to 0, 0 351 | # 352 | 353 | def __init__(self, world_size = 100.0, measurement_range = 30.0, 354 | motion_noise = 1.0, measurement_noise = 1.0): 355 | self.measurement_noise = 0.0 356 | self.world_size = world_size 357 | self.measurement_range = measurement_range 358 | self.x = world_size / 2.0 359 | self.y = world_size / 2.0 360 | self.motion_noise = motion_noise 361 | self.measurement_noise = measurement_noise 362 | self.landmarks = [] 363 | self.num_landmarks = 0 364 | 365 | 366 | def rand(self): 367 | return random.random() * 2.0 - 1.0 368 | 369 | # -------- 370 | # 371 | # make random landmarks located in the world 372 | # 373 | 374 | def make_landmarks(self, num_landmarks): 375 | self.landmarks = [] 376 | for i in range(num_landmarks): 377 | self.landmarks.append([round(random.random() * self.world_size), 378 | round(random.random() * self.world_size)]) 379 | self.num_landmarks = num_landmarks 380 | 381 | 382 | # -------- 383 | # 384 | # move: attempts to move robot by dx, dy. If outside world 385 | # boundary, then the move does nothing and instead returns failure 386 | # 387 | 388 | def move(self, dx, dy): 389 | 390 | x = self.x + dx + self.rand() * self.motion_noise 391 | y = self.y + dy + self.rand() * self.motion_noise 392 | 393 | if x < 0.0 or x > self.world_size or y < 0.0 or y > self.world_size: 394 | return False 395 | else: 396 | self.x = x 397 | self.y = y 398 | return True 399 | 400 | 401 | # -------- 402 | # 403 | # sense: returns x- and y- distances to landmarks within visibility range 404 | # because not all landmarks may be in this range, the list of measurements 405 | # is of variable length. Set measurement_range to -1 if you want all 406 | # landmarks to be visible at all times 407 | # 408 | 409 | def sense(self): 410 | Z = [] 411 | for i in range(self.num_landmarks): 412 | dx = self.landmarks[i][0] - self.x + self.rand() * self.measurement_noise 413 | dy = self.landmarks[i][1] - self.y + self.rand() * self.measurement_noise 414 | if self.measurement_range < 0.0 or abs(dx) + abs(dy) <= self.measurement_range: 415 | Z.append([i, dx, dy]) 416 | return Z 417 | 418 | # -------- 419 | # 420 | # print robot location 421 | # 422 | 423 | def __repr__(self): 424 | return 'Robot: [x=%.5f y=%.5f]' % (self.x, self.y) 425 | 426 | ###################################################### 427 | 428 | # -------- 429 | # this routine makes the robot data 430 | # 431 | 432 | def make_data(N, num_landmarks, world_size, measurement_range, motion_noise, 433 | measurement_noise, distance): 434 | 435 | 436 | complete = False 437 | 438 | while not complete: 439 | 440 | data = [] 441 | 442 | # make robot and landmarks 443 | r = robot(world_size, measurement_range, motion_noise, measurement_noise) 444 | r.make_landmarks(num_landmarks) 445 | seen = [False for row in range(num_landmarks)] 446 | 447 | # guess an initial motion 448 | orientation = random.random() * 2.0 * pi 449 | dx = cos(orientation) * distance 450 | dy = sin(orientation) * distance 451 | 452 | for k in range(N-1): 453 | 454 | # sense 455 | Z = r.sense() 456 | 457 | # check off all landmarks that were observed 458 | for i in range(len(Z)): 459 | seen[Z[i][0]] = True 460 | 461 | # move 462 | while not r.move(dx, dy): 463 | # if we'd be leaving the robot world, pick instead a new direction 464 | orientation = random.random() * 2.0 * pi 465 | dx = cos(orientation) * distance 466 | dy = sin(orientation) * distance 467 | 468 | # memorize data 469 | data.append([Z, [dx, dy]]) 470 | 471 | # we are done when all landmarks were observed; otherwise re-run 472 | complete = (sum(seen) == num_landmarks) 473 | 474 | print ' ' 475 | print 'Landmarks: ', r.landmarks 476 | print r 477 | 478 | 479 | return data 480 | 481 | #################################################### 482 | 483 | # -------------------------------- 484 | # 485 | # print the result of SLAM, the robot pose(s) and the landmarks 486 | # 487 | 488 | def print_result(N, num_landmarks, result): 489 | print 490 | print 'Estimated Pose(s):' 491 | for i in range(N): 492 | print ' ['+ ', '.join('%.3f'%x for x in result.value[2*i]) + ', ' \ 493 | + ', '.join('%.3f'%x for x in result.value[2*i+1]) +']' 494 | print 495 | print 'Estimated Landmarks:' 496 | for i in range(num_landmarks): 497 | print ' ['+ ', '.join('%.3f'%x for x in result.value[2*(N+i)]) + ', ' \ 498 | + ', '.join('%.3f'%x for x in result.value[2*(N+i)+1]) +']' 499 | 500 | # -------------------------------- 501 | # 502 | # slam - retains entire path and all landmarks 503 | # 504 | 505 | ############## ENTER YOUR CODE BELOW HERE ################### 506 | 507 | def slam(data, N, num_landmarks, motion_noise, measurement_noise): 508 | # 509 | # 510 | # Add your code here! 511 | # 512 | # 513 | # 514 | #set the dimension of the filter 515 | dim = 2 * (N + num_landmarks) 516 | #make the constraint information matrix and vector 517 | Omega = matrix() 518 | Omega.zero(dim,dim) 519 | Omega.value[0][0] = 1.0 520 | Omega.value[1][1] = 1.0 521 | Xi = matrix() 522 | Xi.zero(dim, 1) 523 | Xi.value[0][0] = world_size / 2 524 | Xi.value[1][0] = world_size / 2 525 | for k in range(len(data)): 526 | #n is the index of the robots pose in the matrix/vector 527 | n = k * 2 528 | measurement = data[k][0] 529 | motion = data[k][1] 530 | # integrate measurements 531 | for i in range(len(measurement)): 532 | #m is the index of the landmark coordinate in the matrix/vector 533 | m = 2 * (N + measurement[i][0]) 534 | # update the information matrix according to measurement 535 | for b in range(2): 536 | Omega.value[n+b][n+b] += 1.0 / measurement_noise 537 | Omega.value[m+b][m+b] += 1.0 / measurement_noise 538 | Omega.value[n+b][m+b] += -1.0 / measurement_noise 539 | Omega.value[m+b][n+b] += -1.0 / measurement_noise 540 | Xi.value[n+b][0] += -measurement[i][1+b] / measurement_noise 541 | Xi.value[m+b][0] += measurement[i][1+b] / measurement_noise 542 | 543 | # update the information matrix according to motion 544 | for b in range(4): 545 | Omega.value[n+b][n+b] += 1.0 / motion_noise 546 | for b in range(2): 547 | Omega.value[n+b ][n+b+2] += -1.0 / motion_noise 548 | Omega.value[n+b+2][n+b ] += -1.0 / motion_noise 549 | Xi.value[n+b ][0] += -motion[b] / motion_noise 550 | Xi.value[n+b+2][0] += motion[b] / motion_noise 551 | 552 | mu = Omega.inverse() * Xi 553 | 554 | return mu # Make sure you return mu for grading! 555 | 556 | ############### ENTER YOUR CODE ABOVE HERE ################### 557 | 558 | # ------------------------------------------------------------------------ 559 | # ------------------------------------------------------------------------ 560 | # ------------------------------------------------------------------------ 561 | # 562 | # Main routines 563 | # 564 | 565 | 566 | num_landmarks = 5 # number of landmarks 567 | N = 20 # time steps 568 | world_size = 100.0 # size of world 569 | measurement_range = 50.0 # range at which we can sense landmarks 570 | motion_noise = 2.0 # noise in robot motion 571 | measurement_noise = 2.0 # noise in the measurements 572 | distance = 20.0 # distance by which robot (intends to) move each iteratation 573 | 574 | data = make_data(N, num_landmarks, world_size, measurement_range, motion_noise, measurement_noise, distance) 575 | result = slam(data, N, num_landmarks, motion_noise, measurement_noise) 576 | print_result(N, num_landmarks, result) 577 | 578 | # ------------- 579 | # Testing 580 | # 581 | # Uncomment one of the test cases below to compare your results to 582 | # the results shown for Test Case 1 and Test Case 2. 583 | 584 | test_data1 = [[[[1, 19.457599255548065, 23.8387362100849], [2, -13.195807561967236, 11.708840328458608], [3, -30.0954905279171, 15.387879242505843]], [-12.2607279422326, -15.801093326936487]], [[[2, -0.4659930049620491, 28.088559771215664], [4, -17.866382374890936, -16.384904503932]], [-12.2607279422326, -15.801093326936487]], [[[4, -6.202512900833806, -1.823403210274639]], [-12.2607279422326, -15.801093326936487]], [[[4, 7.412136480918645, 15.388585962142429]], [14.008259661173426, 14.274756084260822]], [[[4, -7.526138813444998, -0.4563942429717849]], [14.008259661173426, 14.274756084260822]], [[[2, -6.299793150150058, 29.047830407717623], [4, -21.93551130411791, -13.21956810989039]], [14.008259661173426, 14.274756084260822]], [[[1, 15.796300959032276, 30.65769689694247], [2, -18.64370821983482, 17.380022987031367]], [14.008259661173426, 14.274756084260822]], [[[1, 0.40311325410337906, 14.169429532679855], [2, -35.069349468466235, 2.4945558982439957]], [14.008259661173426, 14.274756084260822]], [[[1, -16.71340983241936, -2.777000269543834]], [-11.006096015782283, 16.699276945166858]], [[[1, -3.611096830835776, -17.954019226763958]], [-19.693482634035977, 3.488085684573048]], [[[1, 18.398273354362416, -22.705102332550947]], [-19.693482634035977, 3.488085684573048]], [[[2, 2.789312482883833, -39.73720193121324]], [12.849049222879723, -15.326510824972983]], [[[1, 21.26897046581808, -10.121029799040915], [2, -11.917698965880655, -23.17711662602097], [3, -31.81167947898398, -16.7985673023331]], [12.849049222879723, -15.326510824972983]], [[[1, 10.48157743234859, 5.692957082575485], [2, -22.31488473554935, -5.389184118551409], [3, -40.81803984305378, -2.4703329790238118]], [12.849049222879723, -15.326510824972983]], [[[0, 10.591050242096598, -39.2051798967113], [1, -3.5675572049297553, 22.849456408289125], [2, -38.39251065320351, 7.288990306029511]], [12.849049222879723, -15.326510824972983]], [[[0, -3.6225556479370766, -25.58006865235512]], [-7.8874682868419965, -18.379005523261092]], [[[0, 1.9784503557879374, -6.5025974151499]], [-7.8874682868419965, -18.379005523261092]], [[[0, 10.050665232782423, 11.026385307998742]], [-17.82919359778298, 9.062000642947142]], [[[0, 26.526838150174818, -0.22563393232425621], [4, -33.70303936886652, 2.880339841013677]], [-17.82919359778298, 9.062000642947142]]] 585 | test_data2 = [[[[0, 26.543274387283322, -6.262538160312672], [3, 9.937396825799755, -9.128540360867689]], [18.92765331253674, -6.460955043986683]], [[[0, 7.706544739722961, -3.758467215445748], [1, 17.03954411948937, 31.705489938553438], [3, -11.61731288777497, -6.64964096716416]], [18.92765331253674, -6.460955043986683]], [[[0, -12.35130507136378, 2.585119104239249], [1, -2.563534536165313, 38.22159657838369], [3, -26.961236804740935, -0.4802312626141525]], [-11.167066095509824, 16.592065417497455]], [[[0, 1.4138633151721272, -13.912454837810632], [1, 8.087721200818589, 20.51845934354381], [3, -17.091723454402302, -16.521500551709707], [4, -7.414211721400232, 38.09191602674439]], [-11.167066095509824, 16.592065417497455]], [[[0, 12.886743222179561, -28.703968411636318], [1, 21.660953298391387, 3.4912891084614914], [3, -6.401401414569506, -32.321583037341625], [4, 5.034079343639034, 23.102207946092893]], [-11.167066095509824, 16.592065417497455]], [[[1, 31.126317672358578, -10.036784369535214], [2, -38.70878528420893, 7.4987265861424595], [4, 17.977218575473767, 6.150889254289742]], [-6.595520680493778, -18.88118393939265]], [[[1, 41.82460922922086, 7.847527392202475], [3, 15.711709540417502, -30.34633659912818]], [-6.595520680493778, -18.88118393939265]], [[[0, 40.18454208294434, -6.710999804403755], [3, 23.019508919299156, -10.12110867290604]], [-6.595520680493778, -18.88118393939265]], [[[3, 27.18579315312821, 8.067219022708391]], [-6.595520680493778, -18.88118393939265]], [[], [11.492663265706092, 16.36822198838621]], [[[3, 24.57154567653098, 13.461499960708197]], [11.492663265706092, 16.36822198838621]], [[[0, 31.61945290413707, 0.4272295085799329], [3, 16.97392299158991, -5.274596836133088]], [11.492663265706092, 16.36822198838621]], [[[0, 22.407381798735177, -18.03500068379259], [1, 29.642444125196995, 17.3794951934614], [3, 4.7969752441371645, -21.07505361639969], [4, 14.726069092569372, 32.75999422300078]], [11.492663265706092, 16.36822198838621]], [[[0, 10.705527984670137, -34.589764174299596], [1, 18.58772336795603, -0.20109708164787765], [3, -4.839806195049413, -39.92208742305105], [4, 4.18824810165454, 14.146847823548889]], [11.492663265706092, 16.36822198838621]], [[[1, 5.878492140223764, -19.955352450942357], [4, -7.059505455306587, -0.9740849280550585]], [19.628527845173146, 3.83678180657467]], [[[1, -11.150789592446378, -22.736641053247872], [4, -28.832815721158255, -3.9462962046291388]], [-19.841703647091965, 2.5113335861604362]], [[[1, 8.64427397916182, -20.286336970889053], [4, -5.036917727942285, -6.311739993868336]], [-5.946642674882207, -19.09548221169787]], [[[0, 7.151866679283043, -39.56103232616369], [1, 16.01535401373368, -3.780995345194027], [4, -3.04801331832137, 13.697362774960865]], [-5.946642674882207, -19.09548221169787]], [[[0, 12.872879480504395, -19.707592098123207], [1, 22.236710716903136, 16.331770792606406], [3, -4.841206109583004, -21.24604435851242], [4, 4.27111163223552, 32.25309748614184]], [-5.946642674882207, -19.09548221169787]]] 586 | 587 | ## Test Case 1 588 | ## 589 | ## Estimated Pose(s): 590 | ## [49.999, 49.999] 591 | ## [37.971, 33.650] 592 | ## [26.183, 18.153] 593 | ## [13.743, 2.114] 594 | ## [28.095, 16.781] 595 | ## [42.383, 30.900] 596 | ## [55.829, 44.494] 597 | ## [70.855, 59.697] 598 | ## [85.695, 75.540] 599 | ## [74.010, 92.431] 600 | ## [53.543, 96.451] 601 | ## [34.523, 100.078] 602 | ## [48.621, 83.951] 603 | ## [60.195, 68.105] 604 | ## [73.776, 52.932] 605 | ## [87.130, 38.536] 606 | ## [80.301, 20.506] 607 | ## [72.797, 2.943] 608 | ## [55.244, 13.253] 609 | ## [37.414, 22.315] 610 | ## 611 | ## Estimated Landmarks: 612 | ## [82.954, 13.537] 613 | ## [70.493, 74.139] 614 | ## [36.738, 61.279] 615 | ## [18.696, 66.057] 616 | ## [20.633, 16.873] 617 | 618 | 619 | ## Test Case 2 620 | ## 621 | ## Estimated Pose(s): 622 | ## [49.999, 49.999] 623 | ## [69.180, 45.664] 624 | ## [87.742, 39.702] 625 | ## [76.269, 56.309] 626 | ## [64.316, 72.174] 627 | ## [52.256, 88.151] 628 | ## [44.058, 69.399] 629 | ## [37.001, 49.916] 630 | ## [30.923, 30.953] 631 | ## [23.507, 11.417] 632 | ## [34.179, 27.131] 633 | ## [44.154, 43.844] 634 | ## [54.805, 60.919] 635 | ## [65.697, 78.544] 636 | ## [77.467, 95.624] 637 | ## [96.801, 98.819] 638 | ## [75.956, 99.969] 639 | ## [70.199, 81.179] 640 | ## [64.053, 61.721] 641 | ## [58.106, 42.626] 642 | ## 643 | ## Estimated Landmarks: 644 | ## [76.778, 42.885] 645 | ## [85.064, 77.436] 646 | ## [13.546, 95.649] 647 | ## [59.448, 39.593] 648 | ## [69.262, 94.238] 649 | 650 | 651 | 652 | ### Uncomment the following three lines for test case 1 ### 653 | 654 | #result = slam(test_data1, 20, 5, 2.0, 2.0) 655 | #print_result(20, 5, result) 656 | #print result 657 | 658 | 659 | ### Uncomment the following three lines for test case 2 ### 660 | 661 | #result = slam(test_data2, 20, 5, 2.0, 2.0) 662 | #print_result(20, 5, result) 663 | #print result 664 | 665 | 666 | -------------------------------------------------------------------------------- /Kalman Filter/kalmanfilter.py: -------------------------------------------------------------------------------- 1 | from math import * 2 | 3 | class matrix: 4 | 5 | # implements basic operations of a matrix class 6 | 7 | def __init__(self, value): 8 | self.value = value 9 | self.dimx = len(value) 10 | self.dimy = len(value[0]) 11 | if value == [[]]: 12 | self.dimx = 0 13 | 14 | def zero(self, dimx, dimy): 15 | # check if valid dimensions 16 | if dimx < 1 or dimy < 1: 17 | raise ValueError, "Invalid size of matrix" 18 | else: 19 | self.dimx = dimx 20 | self.dimy = dimy 21 | self.value = [[0 for row in range(dimy)] for col in range(dimx)] 22 | 23 | def identity(self, dim): 24 | # check if valid dimension 25 | if dim < 1: 26 | raise ValueError, "Invalid size of matrix" 27 | else: 28 | self.dimx = dim 29 | self.dimy = dim 30 | self.value = [[0 for row in range(dim)] for col in range(dim)] 31 | for i in range(dim): 32 | self.value[i][i] = 1 33 | 34 | def show(self): 35 | for i in range(self.dimx): 36 | print self.value[i] 37 | print ' ' 38 | 39 | def __add__(self, other): 40 | # check if correct dimensions 41 | if self.dimx != other.dimx or self.dimy != other.dimy: 42 | raise ValueError, "Matrices must be of equal dimensions to add" 43 | else: 44 | # add if correct dimensions 45 | res = matrix([[]]) 46 | res.zero(self.dimx, self.dimy) 47 | for i in range(self.dimx): 48 | for j in range(self.dimy): 49 | res.value[i][j] = self.value[i][j] + other.value[i][j] 50 | return res 51 | 52 | def __sub__(self, other): 53 | # check if correct dimensions 54 | if self.dimx != other.dimx or self.dimy != other.dimy: 55 | raise ValueError, "Matrices must be of equal dimensions to subtract" 56 | else: 57 | # subtract if correct dimensions 58 | res = matrix([[]]) 59 | res.zero(self.dimx, self.dimy) 60 | for i in range(self.dimx): 61 | for j in range(self.dimy): 62 | res.value[i][j] = self.value[i][j] - other.value[i][j] 63 | return res 64 | 65 | def __mul__(self, other): 66 | # check if correct dimensions 67 | if self.dimy != other.dimx: 68 | raise ValueError, "Matrices must be m*n and n*p to multiply" 69 | else: 70 | # subtract if correct dimensions 71 | res = matrix([[]]) 72 | res.zero(self.dimx, other.dimy) 73 | for i in range(self.dimx): 74 | for j in range(other.dimy): 75 | for k in range(self.dimy): 76 | res.value[i][j] += self.value[i][k] * other.value[k][j] 77 | return res 78 | 79 | def transpose(self): 80 | # compute transpose 81 | res = matrix([[]]) 82 | res.zero(self.dimy, self.dimx) 83 | for i in range(self.dimx): 84 | for j in range(self.dimy): 85 | res.value[j][i] = self.value[i][j] 86 | return res 87 | 88 | # Thanks to Ernesto P. Adorio for use of Cholesky and CholeskyInverse functions 89 | 90 | def Cholesky(self, ztol=1.0e-5): 91 | # Computes the upper triangular Cholesky factorization of 92 | # a positive definite matrix. 93 | res = matrix([[]]) 94 | res.zero(self.dimx, self.dimx) 95 | 96 | for i in range(self.dimx): 97 | S = sum([(res.value[k][i])**2 for k in range(i)]) 98 | d = self.value[i][i] - S 99 | if abs(d) < ztol: 100 | res.value[i][i] = 0.0 101 | else: 102 | if d < 0.0: 103 | raise ValueError, "Matrix not positive-definite" 104 | res.value[i][i] = sqrt(d) 105 | for j in range(i+1, self.dimx): 106 | S = sum([res.value[k][i] * res.value[k][j] for k in range(self.dimx)]) 107 | if abs(S) < ztol: 108 | S = 0.0 109 | res.value[i][j] = (self.value[i][j] - S)/res.value[i][i] 110 | return res 111 | 112 | def CholeskyInverse(self): 113 | # Computes inverse of matrix given its Cholesky upper Triangular 114 | # decomposition of matrix. 115 | res = matrix([[]]) 116 | res.zero(self.dimx, self.dimx) 117 | 118 | # Backward step for inverse. 119 | for j in reversed(range(self.dimx)): 120 | tjj = self.value[j][j] 121 | S = sum([self.value[j][k]*res.value[j][k] for k in range(j+1, self.dimx)]) 122 | res.value[j][j] = 1.0/tjj**2 - S/tjj 123 | for i in reversed(range(j)): 124 | res.value[j][i] = res.value[i][j] = -sum([self.value[i][k]*res.value[k][j] for k in range(i+1, self.dimx)])/self.value[i][i] 125 | return res 126 | 127 | def inverse(self): 128 | aux = self.Cholesky() 129 | res = aux.CholeskyInverse() 130 | return res 131 | 132 | def __repr__(self): 133 | return repr(self.value) 134 | 135 | 136 | ######################################## 137 | 138 | def filter(x, P): 139 | for n in range(len(measurements)): 140 | 141 | # prediction 142 | x = (F * x) + u 143 | P = F * P * F.transpose() 144 | 145 | # measurement update 146 | Z = matrix([measurements[n]]) 147 | y = Z.transpose() - (H * x) 148 | S = H * P * H.transpose() + R 149 | K = P * H.transpose() * S.inverse() 150 | x = x + (K * y) 151 | P = (I - (K * H)) * P 152 | 153 | print 'x= ' 154 | x.show() 155 | print 'P= ' 156 | P.show() 157 | 158 | ######################################## 159 | 160 | print "### 4-dimensional example ###" 161 | 162 | measurements = [[5., 10.], [6., 8.], [7., 6.], [8., 4.], [9., 2.], [10., 0.]] 163 | initial_xy = [4., 12.] 164 | 165 | # measurements = [[1., 4.], [6., 0.], [11., -4.], [16., -8.]] 166 | # initial_xy = [-4., 8.] 167 | 168 | # measurements = [[1., 17.], [1., 15.], [1., 13.], [1., 11.]] 169 | # initial_xy = [1., 19.] 170 | 171 | dt = 0.1 172 | 173 | x = matrix([[initial_xy[0]], [initial_xy[1]], [0.], [0.]]) # initial state (location and velocity) 174 | u = matrix([[0.], [0.], [0.], [0.]]) # external motion 175 | 176 | #### DO NOT MODIFY ANYTHING ABOVE HERE #### 177 | #### fill this in, remember to use the matrix() function!: #### 178 | 179 | P = matrix([[0.,0.,0.,0.], 180 | [0.,0.,0.,0.], 181 | [0.,0.,1000.,0.], 182 | [0.,0.,0.,1000.]])# initial uncertainty 183 | F = matrix([[1.,0.,dt,0.], 184 | [0.,1.,0.,dt], 185 | [0.,0.,1.,0.], 186 | [0.,0.,0.,1.]])# next state function 187 | 188 | H = matrix([[1.,0.,0.,0.], 189 | [0.,1.,0.,0.]])# measurement function 190 | R =matrix([[0.1,0.], 191 | [0.,0.1]])# measurement uncertainty 192 | I = matrix([[1.,0.,0.,0.], 193 | [0.,1.,0.,0.], 194 | [0.,0.,1.,0.], 195 | [0.,0.,0.,1.]])# identity matrix 196 | 197 | ###### DO NOT MODIFY ANYTHING HERE ####### 198 | 199 | filter(x, P) 200 | -------------------------------------------------------------------------------- /Particle Filter/particlefilter.py: -------------------------------------------------------------------------------- 1 | from math import * 2 | import random 3 | 4 | 5 | landmarks = [[20.0, 20.0], [80.0, 80.0], [20.0, 80.0], [80.0, 20.0]] 6 | world_size = 100.0 7 | 8 | class robot: 9 | def __init__(self): 10 | self.x = random.random() * world_size 11 | self.y = random.random() * world_size 12 | self.orientation = random.random() * 2.0 * pi 13 | self.forward_noise = 0.0; 14 | self.turn_noise = 0.0; 15 | self.sense_noise = 0.0; 16 | 17 | def set(self, new_x, new_y, new_orientation): 18 | if new_x < 0 or new_x >= world_size: 19 | raise ValueError, 'X coordinate out of bound' 20 | if new_y < 0 or new_y >= world_size: 21 | raise ValueError, 'Y coordinate out of bound' 22 | if new_orientation < 0 or new_orientation >= 2 * pi: 23 | raise ValueError, 'Orientation must be in [0..2pi]' 24 | self.x = float(new_x) 25 | self.y = float(new_y) 26 | self.orientation = float(new_orientation) 27 | 28 | def set_noise(self, new_f_noise, new_t_noise, new_s_noise): 29 | # makes it possible to change the noise parameters 30 | # this is often useful in particle filters self.forward_noise = float(new_f_noise); 31 | self.turn_noise = float(new_t_noise); 32 | self.sense_noise = float(new_s_noise); 33 | 34 | def sense(self): 35 | Z = [] 36 | for i in range(len(landmarks)): 37 | dist = sqrt((self.x - landmarks[i][0]) ** 2 + (self.y - landmarks[i][1]) ** 2) 38 | dist += random.gauss(0.0, self.sense_noise) 39 | Z.append(dist) 40 | return Z 41 | 42 | 43 | def move(self, turn, forward): 44 | if forward < 0: 45 | raise ValueError, 'Robot cant move backwards' 46 | # turn, and add randomness to the turning command 47 | orientation = self.orientation + float(turn) + random.gauss(0.0, self.turn_noise) 48 | orientation %= 2 * pi 49 | 50 | # move, and add randomness to the motion command 51 | dist = float(forward) + random.gauss(0.0, self.forward_noise) 52 | x = self.x + (cos(orientation) * dist) 53 | y = self.y + (sin(orientation) * dist) 54 | x %= world_size # cyclic truncate 55 | y %= world_size # set particle 56 | res = robot() res.set(x, y, orientation) 57 | res.set_noise(self.forward_noise, self.turn_noise, self.sense_noise) return res 58 | 59 | def Gaussian(self, mu, sigma, x): 60 | # calculates the probability of x for 1-dim Gaussian with mean mu and var. sigma 61 | return exp(- ((mu - x) ** 2) / (sigma ** 2) / 2.0) / sqrt(2.0 * pi * (sigma ** 2)) 62 | 63 | 64 | def measurement_prob(self, measurement): 65 | # calculates how likely a measurement should be 66 | prob = 1.0; 67 | 68 | for i in range(len(landmarks)): 69 | dist = sqrt((self.x - landmarks[i][0]) ** 2 + (self.y - landmarks[i][1]) ** 2) 70 | prob *= self.Gaussian(dist, self.sense_noise, measurement[i]) 71 | return prob 72 | 73 | 74 | def __repr__(self): 75 | return '[x=%.6s y=%.6s orient=%.6s]' % (str(self.x), str(self.y), str(self.orientation)) 76 | 77 | 78 | def eval(r, p): 79 | sum = 0.0; 80 | for i in range(len(p)): # calculate mean error 81 | dx = (p[i].x - r.x + (world_size/2.0)) % world_size - (world_size/2.0) 82 | dy = (p[i].y - r.y + (world_size/2.0)) % world_size - (world_size/2.0) 83 | err = sqrt(dx * dx + dy * dy) 84 | sum += err 85 | return sum / float(len(p)) 86 | 87 | # -------- 88 | 89 | 90 | 91 | N = 1000 92 | T = 10 93 | 94 | myrobot = robot() 95 | 96 | p = [] 97 | for i in range(N): 98 | r = robot() 99 | #r.set_noise(0.01,0.01,1.0) 100 | r.set_noise(0.05, 0.05, 5.0)#Sebastian's provided noise. 101 | p.append(r) 102 | 103 | 104 | for t in range(T): 105 | myrobot= myrobot.move(0.1, 5.0) 106 | Z = myrobot.sense() 107 | 108 | p2 = [] 109 | for i in range(N): 110 | p2.append(p[i].move(0.1, 5.0)) 111 | p = p2 112 | 113 | w = [] 114 | for i in range(N): 115 | w.append(p[i].measurement_prob(Z)) 116 | 117 | 118 | p3 = [] 119 | index = int(random.random() * N) 120 | beta = 0.0 121 | mw = max(w) 122 | for i in range(N): 123 | beta += random.random() * 2.0 * mw 124 | while beta > w[index]: 125 | beta -= w[index] index = (index + 1) % N 126 | p3.append(p[index]) 127 | p = p3 128 | 129 | print eval(myrobot, p) 130 | 131 | if eval(myrobot, p) > 15.0: 132 | for i in range(N): 133 | print '#', i, p[i] 134 | print 'R', myrobot -------------------------------------------------------------------------------- /Put All Together/alltogether.py: -------------------------------------------------------------------------------- 1 | #Includes A* instead of Dynamic Programming 2 | #Includes PID Controller with Smoothing and CTE Calculation 3 | #Includes Twiddle or Coordinate Ascent (Algorithm to find good Control Gains for PID Controller) 4 | 5 | 6 | from math import * 7 | import random 8 | 9 | 10 | # don't change the noise paameters 11 | 12 | steering_noise = 0.1 13 | distance_noise = 0.03 14 | measurement_noise = 0.3 15 | 16 | 17 | class plan: 18 | 19 | # -------- 20 | # init: 21 | # creates an empty plan 22 | # 23 | 24 | def __init__(self, grid, init, goal, cost = 1): 25 | self.cost = cost 26 | self.grid = grid 27 | self.init = init 28 | self.goal = goal 29 | self.make_heuristic(grid, goal, self.cost) 30 | self.path = [] 31 | self.spath = [] 32 | 33 | # -------- 34 | # 35 | # make heuristic function for a grid 36 | 37 | def make_heuristic(self, grid, goal, cost): 38 | self.heuristic = [[0 for row in range(len(grid[0]))] 39 | for col in range(len(grid))] 40 | for i in range(len(self.grid)): 41 | for j in range(len(self.grid[0])): 42 | self.heuristic[i][j] = abs(i - self.goal[0]) + \ 43 | abs(j - self.goal[1]) 44 | 45 | 46 | 47 | # ------------------------------------------------ 48 | # 49 | # A* for searching a path to the goal 50 | # 51 | # 52 | 53 | def astar(self): 54 | 55 | 56 | if self.heuristic == []: 57 | raise ValueError, "Heuristic must be defined to run A*" 58 | 59 | # internal motion parameters 60 | delta = [[-1, 0], # go up 61 | [ 0, -1], # go left 62 | [ 1, 0], # go down 63 | [ 0, 1]] # do right 64 | 65 | 66 | # open list elements are of the type: [f, g, h, x, y] 67 | 68 | closed = [[0 for row in range(len(self.grid[0]))] 69 | for col in range(len(self.grid))] 70 | action = [[0 for row in range(len(self.grid[0]))] 71 | for col in range(len(self.grid))] 72 | 73 | closed[self.init[0]][self.init[1]] = 1 74 | 75 | 76 | x = self.init[0] 77 | y = self.init[1] 78 | h = self.heuristic[x][y] 79 | g = 0 80 | f = g + h 81 | 82 | open = [[f, g, h, x, y]] 83 | 84 | found = False # flag that is set when search complete 85 | resign = False # flag set if we can't find expand 86 | count = 0 87 | 88 | 89 | while not found and not resign: 90 | 91 | # check if we still have elements on the open list 92 | if len(open) == 0: 93 | resign = True 94 | print '###### Search terminated without success' 95 | 96 | else: 97 | # remove node from list 98 | open.sort() 99 | open.reverse() 100 | next = open.pop() 101 | x = next[3] 102 | y = next[4] 103 | g = next[1] 104 | 105 | # check if we are done 106 | 107 | if x == goal[0] and y == goal[1]: 108 | found = True 109 | # print '###### A* search successful' 110 | 111 | else: 112 | # expand winning element and add to new open list 113 | for i in range(len(delta)): 114 | x2 = x + delta[i][0] 115 | y2 = y + delta[i][1] 116 | if x2 >= 0 and x2 < len(self.grid) and y2 >= 0 \ 117 | and y2 < len(self.grid[0]): 118 | if closed[x2][y2] == 0 and self.grid[x2][y2] == 0: 119 | g2 = g + self.cost 120 | h2 = self.heuristic[x2][y2] 121 | f2 = g2 + h2 122 | open.append([f2, g2, h2, x2, y2]) 123 | closed[x2][y2] = 1 124 | action[x2][y2] = i 125 | 126 | count += 1 127 | 128 | # extract the path 129 | 130 | 131 | 132 | invpath = [] 133 | x = self.goal[0] 134 | y = self.goal[1] 135 | invpath.append([x, y]) 136 | while x != self.init[0] or y != self.init[1]: 137 | x2 = x - delta[action[x][y]][0] 138 | y2 = y - delta[action[x][y]][1] 139 | x = x2 140 | y = y2 141 | invpath.append([x, y]) 142 | 143 | self.path = [] 144 | for i in range(len(invpath)): 145 | self.path.append(invpath[len(invpath) - 1 - i]) 146 | 147 | 148 | 149 | 150 | # ------------------------------------------------ 151 | # 152 | # this is the smoothing function 153 | # 154 | 155 | 156 | 157 | 158 | def smooth(self, weight_data = 0.1, weight_smooth = 0.1, 159 | tolerance = 0.000001): 160 | 161 | if self.path == []: 162 | raise ValueError, "Run A* first before smoothing path" 163 | 164 | self.spath = [[0 for row in range(len(self.path[0]))] \ 165 | for col in range(len(self.path))] 166 | for i in range(len(self.path)): 167 | for j in range(len(self.path[0])): 168 | self.spath[i][j] = self.path[i][j] 169 | 170 | change = tolerance 171 | while change >= tolerance: 172 | change = 0.0 173 | for i in range(1, len(self.path)-1): 174 | for j in range(len(self.path[0])): 175 | aux = self.spath[i][j] 176 | 177 | self.spath[i][j] += weight_data * \ 178 | (self.path[i][j] - self.spath[i][j]) 179 | 180 | self.spath[i][j] += weight_smooth * \ 181 | (self.spath[i-1][j] + self.spath[i+1][j] 182 | - (2.0 * self.spath[i][j])) 183 | if i >= 2: 184 | self.spath[i][j] += 0.5 * weight_smooth * \ 185 | (2.0 * self.spath[i-1][j] - self.spath[i-2][j] 186 | - self.spath[i][j]) 187 | if i <= len(self.path) - 3: 188 | self.spath[i][j] += 0.5 * weight_smooth * \ 189 | (2.0 * self.spath[i+1][j] - self.spath[i+2][j] 190 | - self.spath[i][j]) 191 | 192 | change += abs(aux - self.spath[i][j]) 193 | 194 | 195 | 196 | 197 | 198 | 199 | 200 | # ------------------------------------------------ 201 | # 202 | # this is the robot class 203 | # 204 | 205 | class robot: 206 | 207 | # -------- 208 | # init: 209 | # creates robot and initializes location/orientation to 0, 0, 0 210 | # 211 | 212 | def __init__(self, length = 0.5): 213 | self.x = 0.0 214 | self.y = 0.0 215 | self.orientation = 0.0 216 | self.length = length 217 | self.steering_noise = 0.0 218 | self.distance_noise = 0.0 219 | self.measurement_noise = 0.0 220 | self.num_collisions = 0 221 | self.num_steps = 0 222 | 223 | # -------- 224 | # set: 225 | # sets a robot coordinate 226 | # 227 | 228 | def set(self, new_x, new_y, new_orientation): 229 | 230 | self.x = float(new_x) 231 | self.y = float(new_y) 232 | self.orientation = float(new_orientation) % (2.0 * pi) 233 | 234 | 235 | # -------- 236 | # set_noise: 237 | # sets the noise parameters 238 | # 239 | 240 | def set_noise(self, new_s_noise, new_d_noise, new_m_noise): 241 | # makes it possible to change the noise parameters 242 | # this is often useful in particle filters 243 | self.steering_noise = float(new_s_noise) 244 | self.distance_noise = float(new_d_noise) 245 | self.measurement_noise = float(new_m_noise) 246 | 247 | # -------- 248 | # check: 249 | # checks of the robot pose collides with an obstacle, or 250 | # is too far outside the plane 251 | 252 | def check_collision(self, grid): 253 | for i in range(len(grid)): 254 | for j in range(len(grid[0])): 255 | if grid[i][j] == 1: 256 | dist = sqrt((self.x - float(i)) ** 2 + 257 | (self.y - float(j)) ** 2) 258 | if dist < 0.5: 259 | self.num_collisions += 1 260 | return False 261 | return True 262 | 263 | def check_goal(self, goal, threshold = 1.0): 264 | dist = sqrt((float(goal[0]) - self.x) ** 2 + (float(goal[1]) - self.y) ** 2) 265 | return dist < threshold 266 | 267 | # -------- 268 | # move: 269 | # steering = front wheel steering angle, limited by max_steering_angle 270 | # distance = total distance driven, most be non-negative 271 | 272 | def move(self, grid, steering, distance, 273 | tolerance = 0.001, max_steering_angle = pi / 4.0): 274 | 275 | if steering > max_steering_angle: 276 | steering = max_steering_angle 277 | if steering < -max_steering_angle: 278 | steering = -max_steering_angle 279 | if distance < 0.0: 280 | distance = 0.0 281 | 282 | 283 | # make a new copy 284 | res = robot() 285 | res.length = self.length 286 | res.steering_noise = self.steering_noise 287 | res.distance_noise = self.distance_noise 288 | res.measurement_noise = self.measurement_noise 289 | res.num_collisions = self.num_collisions 290 | res.num_steps = self.num_steps + 1 291 | 292 | # apply noise 293 | steering2 = random.gauss(steering, self.steering_noise) 294 | distance2 = random.gauss(distance, self.distance_noise) 295 | 296 | 297 | # Execute motion 298 | turn = tan(steering2) * distance2 / res.length 299 | 300 | if abs(turn) < tolerance: 301 | 302 | # approximate by straight line motion 303 | 304 | res.x = self.x + (distance2 * cos(self.orientation)) 305 | res.y = self.y + (distance2 * sin(self.orientation)) 306 | res.orientation = (self.orientation + turn) % (2.0 * pi) 307 | 308 | else: 309 | 310 | # approximate bicycle model for motion 311 | 312 | radius = distance2 / turn 313 | cx = self.x - (sin(self.orientation) * radius) 314 | cy = self.y + (cos(self.orientation) * radius) 315 | res.orientation = (self.orientation + turn) % (2.0 * pi) 316 | res.x = cx + (sin(res.orientation) * radius) 317 | res.y = cy - (cos(res.orientation) * radius) 318 | 319 | # check for collision 320 | # res.check_collision(grid) 321 | 322 | return res 323 | 324 | # -------- 325 | # sense: 326 | # 327 | 328 | def sense(self): 329 | 330 | return [random.gauss(self.x, self.measurement_noise), 331 | random.gauss(self.y, self.measurement_noise)] 332 | 333 | # -------- 334 | # measurement_prob 335 | # computes the probability of a measurement 336 | # 337 | 338 | def measurement_prob(self, measurement): 339 | 340 | # compute errors 341 | error_x = measurement[0] - self.x 342 | error_y = measurement[1] - self.y 343 | 344 | # calculate Gaussian 345 | error = exp(- (error_x ** 2) / (self.measurement_noise ** 2) / 2.0) \ 346 | / sqrt(2.0 * pi * (self.measurement_noise ** 2)) 347 | error *= exp(- (error_y ** 2) / (self.measurement_noise ** 2) / 2.0) \ 348 | / sqrt(2.0 * pi * (self.measurement_noise ** 2)) 349 | 350 | return error 351 | 352 | 353 | 354 | def __repr__(self): 355 | # return '[x=%.5f y=%.5f orient=%.5f]' % (self.x, self.y, self.orientation) 356 | return '[%.5f, %.5f]' % (self.x, self.y) 357 | 358 | 359 | 360 | 361 | 362 | 363 | # ------------------------------------------------ 364 | # 365 | # this is the particle filter class 366 | # 367 | 368 | class particles: 369 | 370 | # -------- 371 | # init: 372 | # creates particle set with given initial position 373 | # 374 | 375 | def __init__(self, x, y, theta, 376 | steering_noise, distance_noise, measurement_noise, N = 100): 377 | self.N = N 378 | self.steering_noise = steering_noise 379 | self.distance_noise = distance_noise 380 | self.measurement_noise = measurement_noise 381 | 382 | self.data = [] 383 | for i in range(self.N): 384 | r = robot() 385 | r.set(x, y, theta) 386 | r.set_noise(steering_noise, distance_noise, measurement_noise) 387 | self.data.append(r) 388 | 389 | 390 | # -------- 391 | # 392 | # extract position from a particle set 393 | # 394 | 395 | def get_position(self): 396 | x = 0.0 397 | y = 0.0 398 | orientation = 0.0 399 | 400 | for i in range(self.N): 401 | x += self.data[i].x 402 | y += self.data[i].y 403 | # orientation is tricky because it is cyclic. By normalizing 404 | # around the first particle we are somewhat more robust to 405 | # the 0=2pi problem 406 | orientation += (((self.data[i].orientation 407 | - self.data[0].orientation + pi) % (2.0 * pi)) 408 | + self.data[0].orientation - pi) 409 | return [x / self.N, y / self.N, orientation / self.N] 410 | 411 | # -------- 412 | # 413 | # motion of the particles 414 | # 415 | 416 | def move(self, grid, steer, speed): 417 | newdata = [] 418 | 419 | for i in range(self.N): 420 | r = self.data[i].move(grid, steer, speed) 421 | newdata.append(r) 422 | self.data = newdata 423 | 424 | # -------- 425 | # 426 | # sensing and resampling 427 | # 428 | 429 | def sense(self, Z): 430 | w = [] 431 | for i in range(self.N): 432 | w.append(self.data[i].measurement_prob(Z)) 433 | 434 | # resampling (careful, this is using shallow copy) 435 | p3 = [] 436 | index = int(random.random() * self.N) 437 | beta = 0.0 438 | mw = max(w) 439 | 440 | for i in range(self.N): 441 | beta += random.random() * 2.0 * mw 442 | while beta > w[index]: 443 | beta -= w[index] 444 | index = (index + 1) % self.N 445 | p3.append(self.data[index]) 446 | self.data = p3 447 | 448 | 449 | 450 | 451 | 452 | 453 | 454 | # -------- 455 | # 456 | # run: runs control program for the robot 457 | # 458 | 459 | 460 | def run(grid, goal, spath, params, printflag = False, speed = 0.1, timeout = 1000): 461 | 462 | myrobot = robot() 463 | myrobot.set(0., 0., 0.) 464 | myrobot.set_noise(steering_noise, distance_noise, measurement_noise) 465 | filter = particles(myrobot.x, myrobot.y, myrobot.orientation, 466 | steering_noise, distance_noise, measurement_noise) 467 | 468 | cte = 0.0 469 | err = 0.0 470 | N = 0 471 | 472 | index = 0 # index into the path 473 | 474 | while not myrobot.check_goal(goal) and N < timeout: 475 | 476 | diff_cte = - cte 477 | 478 | 479 | # ---------------------------------------- 480 | # compute the CTE 481 | 482 | # start with the present robot estimate 483 | estimate = filter.get_position() 484 | 485 | # some basic vector calculations 486 | dx = spath[index+1][0] - spath[index][0] 487 | dy = spath[index+1][1] - spath[index][1] 488 | drx = estimate[0] - spath[index][0] 489 | dry = estimate[1] - spath[index][1] 490 | 491 | # u is the robot estimate projectes onto the path segment 492 | u = (drx * dx + dry * dy) / (dx * dx + dy * dy) 493 | 494 | # the cte is the estimate projected onto the normal of the path segment 495 | cte = (dry * dx - drx * dy) / (dx * dx + dy * dy) 496 | 497 | # pick the next path segment 498 | if u > 1.0 and index < len(spath) - 1: 499 | index += 1 500 | 501 | 502 | # ---------------------------------------- 503 | 504 | 505 | diff_cte += cte 506 | 507 | steer = - params[0] * cte - params[1] * diff_cte 508 | 509 | myrobot = myrobot.move(grid, steer, speed) 510 | filter.move(grid, steer, speed) 511 | 512 | Z = myrobot.sense() 513 | filter.sense(Z) 514 | 515 | if not myrobot.check_collision(grid): 516 | print '##### Collision ####' 517 | 518 | err += (cte ** 2) 519 | N += 1 520 | 521 | if printflag: 522 | print myrobot, cte, index, u 523 | 524 | return [myrobot.check_goal(goal), myrobot.num_collisions, myrobot.num_steps] 525 | 526 | 527 | # ------------------------------------------------ 528 | # 529 | # this is our main routine 530 | # 531 | 532 | def main(grid, init, goal, steering_noise, distance_noise, measurement_noise, 533 | weight_data, weight_smooth, p_gain, d_gain): 534 | 535 | path = plan(grid, init, goal) 536 | path.astar() 537 | path.smooth(weight_data, weight_smooth) 538 | return run(grid, goal, path.spath, [p_gain, d_gain]) 539 | 540 | 541 | 542 | 543 | # ------------------------------------------------ 544 | # 545 | # input data and parameters 546 | # 547 | 548 | 549 | # grid format: 550 | # 0 = navigable space 551 | # 1 = occupied space 552 | 553 | grid = [[0, 1, 0, 0, 0, 0], 554 | [0, 1, 0, 1, 1, 0], 555 | [0, 1, 0, 1, 0, 0], 556 | [0, 0, 0, 1, 0, 1], 557 | [0, 1, 0, 1, 0, 0]] 558 | 559 | 560 | init = [0, 0] 561 | goal = [len(grid)-1, len(grid[0])-1] 562 | 563 | 564 | steering_noise = 0.1 565 | distance_noise = 0.03 566 | measurement_noise = 0.3 567 | 568 | #### ADJUST THESE PARAMETERS ###### 569 | 570 | weight_data = 0.1 571 | weight_smooth = 0.2 572 | p_gain = 2.0 573 | d_gain = 6.0 574 | 575 | ################################### 576 | 577 | print main(grid, init, goal, steering_noise, distance_noise, measurement_noise, 578 | weight_data, weight_smooth, p_gain, d_gain) 579 | 580 | 581 | 582 | def twiddle(init_params): 583 | n_params = len(init_params) 584 | dparams = [1.0 for row in range(n_params)] 585 | params = [0.0 for row in range(n_params)] 586 | K = 10 587 | 588 | for i in range(n_params): 589 | params[i] = init_params[i] 590 | 591 | 592 | best_error = 0.0; 593 | for k in range(K): 594 | ret = main(grid, init, goal, 595 | steering_noise, distance_noise, measurement_noise, 596 | params[0], params[1], params[2], params[3]) 597 | if ret[0]: 598 | best_error += ret[1] * 100 + ret[2] 599 | else: 600 | best_error += 99999 601 | best_error = float(best_error) / float(k+1) 602 | print best_error 603 | 604 | n = 0 605 | while sum(dparams) > 0.0000001: 606 | for i in range(len(params)): 607 | params[i] += dparams[i] 608 | err = 0 609 | for k in range(K): 610 | ret = main(grid, init, goal, 611 | steering_noise, distance_noise, measurement_noise, 612 | params[0], params[1], params[2], params[3], best_error) 613 | if ret[0]: 614 | err += ret[1] * 100 + ret[2] 615 | else: 616 | err += 99999 617 | print float(err) / float(k+1) 618 | if err < best_error: 619 | best_error = float(err) / float(k+1) 620 | dparams[i] *= 1.1 621 | else: 622 | params[i] -= 2.0 * dparams[i] 623 | err = 0 624 | for k in range(K): 625 | ret = main(grid, init, goal, 626 | steering_noise, distance_noise, measurement_noise, 627 | params[0], params[1], params[2], params[3], best_error) 628 | if ret[0]: 629 | err += ret[1] * 100 + ret[2] 630 | else: 631 | err += 99999 632 | print float(err) / float(k+1) 633 | if err < best_error: 634 | best_error = float(err) / float(k+1) 635 | dparams[i] *= 1.1 636 | else: 637 | params[i] += dparams[i] 638 | dparams[i] *= 0.5 639 | n += 1 640 | print 'Twiddle #', n, params, ' -> ', best_error 641 | print ' ' 642 | return params 643 | 644 | 645 | #twiddle([weight_data, weight_smooth, p_gain, d_gain]) 646 | 647 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/salimchedrawi/Python-AI-Robots/c3305df6069b5fd48055eda72bf8949ef38806f1/README.md -------------------------------------------------------------------------------- /main.py: -------------------------------------------------------------------------------- 1 | import math, random 2 | import direct.directbase.DirectStart 3 | from pandac.PandaModules import * 4 | from direct.showbase.DirectObject import DirectObject 5 | from direct.task.Task import Task 6 | 7 | prime = render.attachNewNode('prime') 8 | #Global Collision setup 9 | base.cTrav = CollisionTraverser('Collision Detection') 10 | Pusher = CollisionHandlerPusher() #this handler will generate a a panda event for each collision that can be accepted by the various classes in addition to providing simple real world collisions. 11 | Pusher.addInPattern('%fn') #the format of the actual event string. Just provide the name of the "from" node 12 | 13 | collisionMASK = BitMask32.bit(1) 14 | 15 | class Guide2D: 16 | 17 | def __init__(self, pos = None, hpr = None,): 18 | self.guide = prime.attachNewNode('Guide'+str(self.name)) #This node will be the moving and turning node. Pos and Hpr should be set to self.guide 19 | self.guide.setPos(pos) 20 | self.guide.setHpr(hpr) 21 | self.pointer = NodePath('pointer'+str(self.name)) 22 | self.pointer.reparentTo(prime) 23 | self.speed = self.maxspeed #some functions require speeds other than the max speed. This speed is the actual speed of the unit. 24 | def Guidestep(self,dt): 25 | if self.pointer.isEmpty(): return 26 | if self.guide.isEmpty():return 27 | self.pointer.setPos(self.guide.getPos()) 28 | for methods in self.behavior: 29 | methodname = '_steerFor' + methods 30 | method = getattr(self,methodname) 31 | method(dt) 32 | self.guide.setFluidPos(self.guide,Vec3(0,1*dt*self.speed,0)) 33 | 34 | def _steerForSeek(self,dt): 35 | self.speed = self.maxspeed 36 | if self.target is None: return 37 | if self.target.isEmpty() == False: 38 | 39 | self.pointer.lookAt(self.target) 40 | PH = self.pointer.getH() 41 | GH = self.guide.getH() 42 | H = None 43 | add = 0 44 | if GH < 0: add = 1 45 | if PH < 0: add = add + 1 46 | if add == 1: 47 | #Different sides of the divide 48 | a = abs(GH) + abs(PH) 49 | else: 50 | #Same side of the divide 51 | a = abs(GH - PH) 52 | b = 360 - a 53 | if b < 180: Switch = -1 54 | else: Switch = 1 55 | 56 | if PH > GH: 57 | if abs(PH-GH) > self.rotation * dt: H = GH+self.rotation*dt*Switch #This will hopefully prevent jitter when approximating the pointer position. 58 | else: H = GH + self.rotation/5 * dt * Switch 59 | 60 | if PH < GH: 61 | if abs(PH-GH) > self.rotation * dt: H = GH-self.rotation*dt*Switch #This will hopefully prevent jitter when approximating the pointer position. 62 | else: H = GH - self.rotation/5 * dt * Switch 63 | 64 | if H is not None: 65 | if H < -180: H = 179 66 | if H > 180 : H = -179 67 | self.guide.setH(H) 68 | def _steerForFlee(self,dt): 69 | self.speed = self.maxspeed 70 | if self.target is None: return 71 | if self.target.isEmpty() == False: 72 | self.pointer.lookAt(self.target) 73 | PH = self.pointer.getH() 74 | GH = self.guide.getH() 75 | H = None 76 | add = 0 77 | if GH < 0: add = 1 78 | if PH < 0: add = add + 1 79 | if add == 1: 80 | #Different sides of the divide 81 | a = abs(GH) + abs(PH) 82 | else: 83 | #Same side of the divide 84 | a = abs(GH - PH) 85 | b = 360 - a 86 | if b < 180: Switch = -1 87 | else: Switch = 1 88 | 89 | if PH > GH: 90 | H = GH - self.rotation*dt*Switch*5 91 | 92 | if PH < GH: 93 | H = GH + self.rotation*dt*Switch*5 94 | 95 | if H is not None: 96 | if H < -180: H = 179 97 | if H > 180 : H = -179 98 | self.guide.setH(H) 99 | 100 | 101 | def _steerForWander(self,dt): 102 | self.speed = self.maxspeed/2 103 | GH = self.guide.getH() 104 | PH = self.pointer.getH() 105 | if abs(GH-PH) < 10: self.pointer.setH(random.random()*360-180) 106 | H = None 107 | 108 | add = 0 109 | if GH < 0: add = 1 110 | if PH < 0: add = add + 1 111 | if add == 1: 112 | #Different sides of the divide 113 | a = abs(GH) + abs(PH) 114 | else: 115 | #Same side of the divide 116 | a = abs(GH - PH) 117 | b = 360 - a 118 | if b < 180: Switch = -1 119 | else: Switch = 1 120 | 121 | if PH > GH: 122 | 123 | if abs(PH-GH) > self.rotation * dt: H = GH+self.rotation/2*dt*Switch 124 | else: H = GH + self.rotation/5 * dt * Switch 125 | 126 | if PH < GH: 127 | if abs(PH-GH) > self.rotation * dt: H = GH-self.rotation/2*dt*Switch 128 | else: H = GH - self.rotation/5 * dt * Switch 129 | 130 | if H is not None: 131 | if H < -180: H = 179 132 | if H > 180 : H = -179 133 | self.guide.setH(H) 134 | 135 | def _steerForStop(self,dt): 136 | self.speed = 0 137 | 138 | class Player(Guide2D, DirectObject): 139 | def __init__(self, world, name): 140 | 141 | self.maxspeed = 10 142 | self.rotation = 90 143 | self.behavior = ['Wander'] 144 | self.world = world 145 | 146 | X = random.random()* 50 - 25 147 | Y = random.random()* 50 - 25 148 | H = random.random()*360 - 180 149 | pos = Vec3(X,Y,0) 150 | hpr = Vec3(H,0,0) 151 | 152 | self.center = prime.attachNewNode('center'+str(X)) 153 | self.model = loader.loadModel("models/robot.egg.pz") 154 | self.model.setHpr(90,0,0) 155 | self.model.reparentTo(self.center) 156 | 157 | self.center.reparentTo(prime) 158 | self.center.setPos(X,Y,0) 159 | self.name = name 160 | Guide2D.__init__(self,pos,hpr) #the guide2d object handles movement. We need to give it the starting position and the orientation. 161 | #Set up a task for each player. Set up some starting values for timing. Set the player as "not It" as default. 162 | self.stepTask = taskMgr.add(self.step, "UnitStepLoop") 163 | self.stepTask.last = 0 164 | self.onefourth = 0 165 | self.Im_not_it() 166 | self.timer = 2 167 | 168 | #Collision stuff 169 | self.center.setTag("Name",self.name) 170 | self.colsphere = CollisionSphere(0,0,0,1) 171 | self.csnodepath = self.center.attachNewNode(CollisionNode('cnode')) 172 | self.csnodepath.node().addSolid(self.colsphere) 173 | self.csnodepath.node().setCollideMask(collisionMASK) 174 | 175 | Pusher.addCollider(self.csnodepath, self.guide) 176 | base.cTrav.addCollider(self.csnodepath, Pusher) 177 | 178 | self.csnodepath.setTag("Name",self.name) 179 | self.csnodepath.setName(self.name) 180 | 181 | self.accept(self.name, self.Collision) 182 | 183 | # default stuff 184 | self.it = False 185 | self.isLeader = False 186 | self.myLeader = None 187 | 188 | def Im_it(self): 189 | self.it = True 190 | self.center.setColor(1,0,0,1) 191 | self.maxspeed = 11 192 | self.world.whoisit = self 193 | self.ingame = True 194 | def Im_it2(self): 195 | self.it = True 196 | self.center.setColor(1,0,0,1) 197 | self.maxspeed = 11 198 | self.world.whoisit2 = self 199 | self.ingame = True 200 | def Im_not_it(self): 201 | self.it = False 202 | self.iAmAleader = False 203 | self.center.setColor(0,0,1,1) 204 | self.maxspeed = 10 205 | self.ingame = True 206 | def iAmALeader(self): 207 | self.isLeader = True 208 | self.it = False 209 | self.center.setColor(1,0,5,1) 210 | self.maxspeed = 10 211 | self.ingame = True 212 | def setLeader(self,leader): 213 | self.isLeader = False 214 | self.myLeader = leader 215 | def Im_out(self): #A temporary state for when a player just gets done tagging another player. 216 | self.it = False 217 | self.isLeader = False 218 | self.center.setColor(0,1,0,1) 219 | self.maxspeed = 10 220 | self.ingame = False 221 | self.timer = 0 222 | def step(self,task): 223 | dt = task.time - task.last #obtains the time since that last frame. 224 | task.last = task.time 225 | self.timer = self.timer + dt 226 | if self.timer >= 1 and self.ingame == False: 227 | self.Im_not_it() #put players that were just it back into play after 1 sec. 228 | self.Guidestep(dt) 229 | self.center.setPos(self.guide.getPos()) 230 | self.center.setHpr(self.guide.getHpr()) 231 | 232 | self.onefourth = self.onefourth + dt # Every 1/4 second update AI 233 | if self.onefourth > .5: 234 | self.onefourth = 0 235 | self.reset() 236 | return Task.cont #finished, the loop goes back to the start. 237 | def Collision(self,entry): 238 | intonode = entry.getIntoNodePath() 239 | name = intonode.getTag("Name") 240 | if name is not None: 241 | if not self.it and not self.isLeader: 242 | if self.ingame: 243 | if name == self.world.whoisit.name: 244 | self.world.whoisit.Im_out() 245 | self.Im_it() 246 | elif name == self.world.whoisit2.name: 247 | self.world.whoisit2.Im_out() 248 | self.Im_it2() 249 | 250 | def reset(self): #This is the key AI routine regarding the rules of Tag. 251 | if self.it: 252 | distance = 100 253 | temporaryTarget = None 254 | for player in self.world.players: 255 | if player.ingame: 256 | if player != self.world.whoisit and player != self.world.whoisit2 and not player.isLeader: 257 | if self.center.getDistance(player.center) < distance: 258 | temporaryTarget = player 259 | distance = self.center.getDistance(player.center) 260 | self.Target = temporaryTarget 261 | self.target = temporaryTarget.center 262 | self.behavior = ['Seek'] 263 | elif self.myLeader is not None: 264 | self.Target = self.myLeader 265 | self.target = self.myLeader.center 266 | self.behavior = ['Seek'] 267 | elif self.isLeader: 268 | if self.center.getDistance(self.world.whoisit.center) <= 30: 269 | self.target = self.world.whoisit.center 270 | self.behavior = ['Flee'] 271 | elif self.center.getDistance(self.world.whoisit2.center) <= 30: 272 | self.target = self.world.whoisit2.center 273 | self.behavior = ['Flee'] 274 | else: 275 | self.behavior = ['Wander'] 276 | elif self.center.getDistance(self.world.whoisit.center) <= 10: 277 | self.target = self.world.whoisit.center 278 | self.behavior = ['Flee'] 279 | elif self.center.getDistance(self.world.whoisit2.center) <= 10: 280 | self.target = self.world.whoisit2.center 281 | self.behavior = ['Flee'] 282 | else: 283 | self.behavior = ['Wander'] 284 | 285 | class World: 286 | def __init__(self): 287 | base.setBackgroundColor(1, 1, 1) #Set the background to white 288 | self.setupLights() 289 | base.camera.setPos(0,-100,100) 290 | base.camera.setHpr(0,-45,0) 291 | base.disableMouse() 292 | 293 | 294 | #Load up the model for the playing field. 295 | field = loader.loadModel("models/field.egg") 296 | field.reparentTo(prime) 297 | field.setPosHprScale(0,0,-.5,0,-90,0,83,83,83) 298 | 299 | self.players = [] 300 | for i in range(20): 301 | p = Player(self,str(i)) #create a new player- pass the player the this world class for reference. 302 | self.players.append(p) 303 | 304 | self.players[0].Im_it() #just assign the number 10 player to be IT to start off the game. 305 | self.players[1].Im_it2() #just assign the number 15 player to be IT to start off the game. 306 | 307 | self.players[5].iAmALeader() 308 | self.players[6].setLeader(self.players[5]) 309 | self.players[7].setLeader(self.players[5]) 310 | self.players[8].setLeader(self.players[5]) 311 | 312 | self.players[10].iAmALeader() 313 | self.players[11].setLeader(self.players[10]) 314 | self.players[12].setLeader(self.players[10]) 315 | self.players[13].setLeader(self.players[10]) 316 | 317 | self.setupPlayField() 318 | 319 | def setupLights(self): 320 | prime.setLightOff() #clears the field so that only these lights are ever in play. 321 | # Setup ambient and directional lights. 322 | self.alight = AmbientLight('alight') 323 | self.alight.setColor(VBase4(0.35, 0.35, 0.35, 1)) #formerly .15 324 | self.alnp = prime.attachNewNode(self.alight) 325 | prime.setLight(self.alnp) 326 | 327 | self.dlight = DirectionalLight('dlight') 328 | self.dlight.setColor(VBase4(0.9, 0.9, 0.9, 1)) 329 | self.dlnp = prime.attachNewNode(self.dlight) 330 | self.dlnp.setHpr(45, -15, 0) 331 | prime.setLight(self.dlnp) 332 | def setupPlayField(self): #this sets the outer bounds of the playfield as an inverse collision sphere. 333 | inv = CollisionInvSphere(0, 0, 0, 40) 334 | cnodePath = prime.attachNewNode(CollisionNode('inv')) 335 | cnodePath.node().addSolid(inv) 336 | 337 | 338 | w = World() 339 | run() -------------------------------------------------------------------------------- /models/field.egg: -------------------------------------------------------------------------------- 1 | { 2 | "egg-texture-cards -o field.egg field.png" 3 | } 4 | field { 5 | field.png 6 | } 7 | { 8 | vpool { 9 | 0 { 10 | -0.5 0.5 0 11 | { 0 1 } 12 | } 13 | 1 { 14 | -0.5 -0.5 0 15 | { 0 0 } 16 | } 17 | 2 { 18 | 0.5 -0.5 0 19 | { 1 0 } 20 | } 21 | 3 { 22 | 0.5 0.5 0 23 | { 1 1 } 24 | } 25 | } 26 | field { 27 | { 28 | { 1 1 1 1 } 29 | { field } 30 | { 0 1 2 3 { vpool } } 31 | } 32 | } 33 | } 34 | -------------------------------------------------------------------------------- /models/robot.egg.pz: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/salimchedrawi/Python-AI-Robots/c3305df6069b5fd48055eda72bf8949ef38806f1/models/robot.egg.pz --------------------------------------------------------------------------------