# Udacity CS373 unit 6-7 with visualization

By: brouhaha on Mar 26th, 2012  |  syntax: Python  |  size: 19.28 KB  |  hits: 459  |  expires: Never
Text below is selected. Please press Ctrl+C to copy to your clipboard. (⌘+C on Mac)
1. #!/usr/bin/python
2. # -----------
3. # User Instructions
4. #
5. # The point of this exercise is to find the optimal
6. # parameters! You can use twiddle or any other method
7. # that you like. Since we don't know what the optimal
8. # parameters are, we will be very loose with the
9. # grading. If you find parameters that work well, post
10. # them in the forums!
11. #
12. # You can find the parameters at line 575.
13.
14. from math import *
15. import random
16.
17. # don't change the noise paameters
18.
19. steering_noise    = 0.1
20. distance_noise    = 0.03
21. measurement_noise = 0.3
22.
23.
24. class plan:
25.
26.     # --------
27.     # init:
28.     #    creates an empty plan
29.     #
30.
31.     def __init__(self, grid, init, goal, cost = 1):
32.         self.cost = cost
33.         self.grid = grid
34.         self.init = init
35.         self.goal = goal
36.         self.make_heuristic(grid, goal, self.cost)
37.         self.path = []
38.         self.spath = []
39.
40.     # --------
41.     #
42.     # make heuristic function for a grid
43.
44.     def make_heuristic(self, grid, goal, cost):
45.         self.heuristic = [[0 for row in range(len(grid[0]))]
46.                           for col in range(len(grid))]
47.         for i in range(len(self.grid)):
48.             for j in range(len(self.grid[0])):
49.                 self.heuristic[i][j] = abs(i - self.goal[0]) + \
50.                     abs(j - self.goal[1])
51.
52.
53.
54.     # ------------------------------------------------
55.     #
56.     # A* for searching a path to the goal
57.     #
58.     #
59.
60.     def astar(self):
61.
62.
63.         if self.heuristic == []:
64.             raise ValueError, "Heuristic must be defined to run A*"
65.
66.         # internal motion parameters
67.         delta = [[-1,  0], # go up
68.                  [ 0,  -1], # go left
69.                  [ 1,  0], # go down
70.                  [ 0,  1]] # do right
71.
72.
73.         # open list elements are of the type: [f, g, h, x, y]
74.
75.         closed = [[0 for row in range(len(self.grid[0]))]
76.                   for col in range(len(self.grid))]
77.         action = [[0 for row in range(len(self.grid[0]))]
78.                   for col in range(len(self.grid))]
79.
80.         closed[self.init[0]][self.init[1]] = 1
81.
82.
83.         x = self.init[0]
84.         y = self.init[1]
85.         h = self.heuristic[x][y]
86.         g = 0
87.         f = g + h
88.
89.         open = [[f, g, h, x, y]]
90.
91.         found  = False # flag that is set when search complete
92.         resign = False # flag set if we can't find expand
93.         count  = 0
94.
95.
97.
98.             # check if we still have elements on the open list
99.             if len(open) == 0:
100.                 resign = True
101.                 print '###### Search terminated without success'
102.
103.             else:
104.                 # remove node from list
105.                 open.sort()
106.                 open.reverse()
107.                 next = open.pop()
108.                 x = next[3]
109.                 y = next[4]
110.                 g = next[1]
111.
112.             # check if we are done
113.
114.             if x == goal[0] and y == goal[1]:
115.                 found = True
116.                 # print '###### A* search successful'
117.
118.             else:
119.                 # expand winning element and add to new open list
120.                 for i in range(len(delta)):
121.                     x2 = x + delta[i][0]
122.                     y2 = y + delta[i][1]
123.                     if x2 >= 0 and x2 < len(self.grid) and y2 >= 0 \
124.                             and y2 < len(self.grid[0]):
125.                         if closed[x2][y2] == 0 and self.grid[x2][y2] == 0:
126.                             g2 = g + self.cost
127.                             h2 = self.heuristic[x2][y2]
128.                             f2 = g2 + h2
129.                             open.append([f2, g2, h2, x2, y2])
130.                             closed[x2][y2] = 1
131.                             action[x2][y2] = i
132.
133.             count += 1
134.
135.         # extract the path
136.
137.
138.
139.         invpath = []
140.         x = self.goal[0]
141.         y = self.goal[1]
142.         invpath.append([x, y])
143.         while x != self.init[0] or y != self.init[1]:
144.             x2 = x - delta[action[x][y]][0]
145.             y2 = y - delta[action[x][y]][1]
146.             x = x2
147.             y = y2
148.             invpath.append([x, y])
149.
150.         self.path = []
151.         for i in range(len(invpath)):
152.             self.path.append(invpath[len(invpath) - 1 - i])
153.
154.
155.
156.
157.     # ------------------------------------------------
158.     #
159.     # this is the smoothing function
160.     #
161.
162.
163.
164.
165.     def smooth(self, weight_data = 0.1, weight_smooth = 0.1,
166.                tolerance = 0.000001):
167.
168.         if self.path == []:
169.             raise ValueError, "Run A* first before smoothing path"
170.
171.         self.spath = [[0 for row in range(len(self.path[0]))] \
172.                            for col in range(len(self.path))]
173.         for i in range(len(self.path)):
174.             for j in range(len(self.path[0])):
175.                 self.spath[i][j] = self.path[i][j]
176.
177.         change = tolerance
178.         while change >= tolerance:
179.             change = 0.0
180.             for i in range(1, len(self.path)-1):
181.                 for j in range(len(self.path[0])):
182.                     aux = self.spath[i][j]
183.
184.                     self.spath[i][j] += weight_data * \
185.                         (self.path[i][j] - self.spath[i][j])
186.
187.                     self.spath[i][j] += weight_smooth * \
188.                         (self.spath[i-1][j] + self.spath[i+1][j]
189.                          - (2.0 * self.spath[i][j]))
190.                     if i >= 2:
191.                         self.spath[i][j] += 0.5 * weight_smooth * \
192.                             (2.0 * self.spath[i-1][j] - self.spath[i-2][j]
193.                              - self.spath[i][j])
194.                     if i <= len(self.path) - 3:
195.                         self.spath[i][j] += 0.5 * weight_smooth * \
196.                             (2.0 * self.spath[i+1][j] - self.spath[i+2][j]
197.                              - self.spath[i][j])
198.
199.             change += abs(aux - self.spath[i][j])
200.
201.
202.
203.
204.
205.
206.
207. # ------------------------------------------------
208. #
209. # this is the robot class
210. #
211.
212. class robot:
213.
214.     # --------
215.     # init:
216.     #   creates robot and initializes location/orientation to 0, 0, 0
217.     #
218.
219.     def __init__(self, length = 0.5):
220.         self.x = 0.0
221.         self.y = 0.0
222.         self.orientation = 0.0
223.         self.length = length
224.         self.steering_noise    = 0.0
225.         self.distance_noise    = 0.0
226.         self.measurement_noise = 0.0
227.         self.num_collisions    = 0
228.         self.num_steps         = 0
229.
230.     # --------
231.     # set:
232.     #   sets a robot coordinate
233.     #
234.
235.     def set(self, new_x, new_y, new_orientation):
236.
237.         self.x = float(new_x)
238.         self.y = float(new_y)
239.         self.orientation = float(new_orientation) % (2.0 * pi)
240.
241.
242.     # --------
243.     # set_noise:
244.     #   sets the noise parameters
245.     #
246.
247.     def set_noise(self, new_s_noise, new_d_noise, new_m_noise):
248.         # makes it possible to change the noise parameters
249.         # this is often useful in particle filters
250.         self.steering_noise     = float(new_s_noise)
251.         self.distance_noise    = float(new_d_noise)
252.         self.measurement_noise = float(new_m_noise)
253.
254.     # --------
255.     # check:
256.     #    checks of the robot pose collides with an obstacle, or
257.     # is too far outside the plane
258.
259.     def check_collision(self, grid):
260.         for i in range(len(grid)):
261.             for j in range(len(grid[0])):
262.                 if grid[i][j] == 1:
263.                     dist = sqrt((self.x - float(i)) ** 2 +
264.                                 (self.y - float(j)) ** 2)
265.                     if dist < 0.5:
266.                         self.num_collisions += 1
267.                         return False
268.         return True
269.
270.     def check_goal(self, goal, threshold = 1.0):
271.         dist =  sqrt((float(goal[0]) - self.x) ** 2 + (float(goal[1]) - self.y) ** 2)
272.         return dist < threshold
273.
274.     # --------
275.     # move:
276.     #    steering = front wheel steering angle, limited by max_steering_angle
277.     #    distance = total distance driven, most be non-negative
278.
279.     def move(self, grid, steering, distance,
280.              tolerance = 0.001, max_steering_angle = pi / 4.0):
281.
282.         if steering > max_steering_angle:
283.             steering = max_steering_angle
284.         if steering < -max_steering_angle:
285.             steering = -max_steering_angle
286.         if distance < 0.0:
287.             distance = 0.0
288.
289.
290.         # make a new copy
291.         res = robot()
292.         res.length            = self.length
293.         res.steering_noise    = self.steering_noise
294.         res.distance_noise    = self.distance_noise
295.         res.measurement_noise = self.measurement_noise
296.         res.num_collisions    = self.num_collisions
297.         res.num_steps         = self.num_steps + 1
298.
299.         # apply noise
300.         steering2 = random.gauss(steering, self.steering_noise)
301.         distance2 = random.gauss(distance, self.distance_noise)
302.
303.
304.         # Execute motion
305.         turn = tan(steering2) * distance2 / res.length
306.
307.         if abs(turn) < tolerance:
308.
309.             # approximate by straight line motion
310.
311.             res.x = self.x + (distance2 * cos(self.orientation))
312.             res.y = self.y + (distance2 * sin(self.orientation))
313.             res.orientation = (self.orientation + turn) % (2.0 * pi)
314.
315.         else:
316.
317.             # approximate bicycle model for motion
318.
319.             radius = distance2 / turn
320.             cx = self.x - (sin(self.orientation) * radius)
321.             cy = self.y + (cos(self.orientation) * radius)
322.             res.orientation = (self.orientation + turn) % (2.0 * pi)
323.             res.x = cx + (sin(res.orientation) * radius)
324.             res.y = cy - (cos(res.orientation) * radius)
325.
326.         # check for collision
327.         # res.check_collision(grid)
328.
329.         return res
330.
331.     # --------
332.     # sense:
333.     #
334.
335.     def sense(self):
336.
337.         return [random.gauss(self.x, self.measurement_noise),
338.                 random.gauss(self.y, self.measurement_noise)]
339.
340.     # --------
341.     # measurement_prob
342.     #    computes the probability of a measurement
343.     #
344.
345.     def measurement_prob(self, measurement):
346.
347.         # compute errors
348.         error_x = measurement[0] - self.x
349.         error_y = measurement[1] - self.y
350.
351.         # calculate Gaussian
352.         error = exp(- (error_x ** 2) / (self.measurement_noise ** 2) / 2.0) \
353.             / sqrt(2.0 * pi * (self.measurement_noise ** 2))
354.         error *= exp(- (error_y ** 2) / (self.measurement_noise ** 2) / 2.0) \
355.             / sqrt(2.0 * pi * (self.measurement_noise ** 2))
356.
357.         return error
358.
359.
360.
361.     def __repr__(self):
362.         # return '[x=%.5f y=%.5f orient=%.5f]'  % (self.x, self.y, self.orientation)
363.         return '[%.5f, %.5f]'  % (self.x, self.y)
364.
365.
366.
367.
368.
369.
370. # ------------------------------------------------
371. #
372. # this is the particle filter class
373. #
374.
375. class particles:
376.
377.     # --------
378.     # init:
379.     #   creates particle set with given initial position
380.     #
381.
382.     def __init__(self, x, y, theta,
383.                  steering_noise, distance_noise, measurement_noise, N = 100):
384.         self.N = N
385.         self.steering_noise    = steering_noise
386.         self.distance_noise    = distance_noise
387.         self.measurement_noise = measurement_noise
388.
389.         self.data = []
390.         for i in range(self.N):
391.             r = robot()
392.             r.set(x, y, theta)
393.             r.set_noise(steering_noise, distance_noise, measurement_noise)
394.             self.data.append(r)
395.
396.
397.     # --------
398.     #
399.     # extract position from a particle set
400.     #
401.
402.     def get_position(self):
403.         x = 0.0
404.         y = 0.0
405.         orientation = 0.0
406.
407.         for i in range(self.N):
408.             x += self.data[i].x
409.             y += self.data[i].y
410.             # orientation is tricky because it is cyclic. By normalizing
411.             # around the first particle we are somewhat more robust to
412.             # the 0=2pi problem
413.             orientation += (((self.data[i].orientation
414.                               - self.data[0].orientation + pi) % (2.0 * pi))
415.                             + self.data[0].orientation - pi)
416.         return [x / self.N, y / self.N, orientation / self.N]
417.
418.     # --------
419.     #
420.     # motion of the particles
421.     #
422.
423.     def move(self, grid, steer, speed):
424.         newdata = []
425.
426.         for i in range(self.N):
427.             r = self.data[i].move(grid, steer, speed)
428.             newdata.append(r)
429.         self.data = newdata
430.
431.     # --------
432.     #
433.     # sensing and resampling
434.     #
435.
436.     def sense(self, Z):
437.         w = []
438.         for i in range(self.N):
439.             w.append(self.data[i].measurement_prob(Z))
440.
441.         # resampling (careful, this is using shallow copy)
442.         p3 = []
443.         index = int(random.random() * self.N)
444.         beta = 0.0
445.         mw = max(w)
446.
447.         for i in range(self.N):
448.             beta += random.random() * 2.0 * mw
449.             while beta > w[index]:
450.                 beta -= w[index]
451.                 index = (index + 1) % self.N
452.             p3.append(self.data[index])
453.         self.data = p3
454.
455.
456.
457.
458.
459.
460.
461. # --------
462. #
463. # run:  runs control program for the robot
464. #
465.
466.
467. def run(grid, goal, spath, params, printflag = False, speed = 0.1, timeout = 1000):
468.
469.     myrobot = robot()
470.     myrobot.set(0., 0., 0.)
471.     myrobot.set_noise(steering_noise, distance_noise, measurement_noise)
472.     filter = particles(myrobot.x, myrobot.y, myrobot.orientation,
473.                        steering_noise, distance_noise, measurement_noise)
474.
475.     cte  = 0.0
476.     err  = 0.0
477.     N    = 0
478.
479.     index = 0 # index into the path
480.
481.     actualpath = []
482.     actualpath.append ([myrobot.x, myrobot.y])
483.
484.     while not myrobot.check_goal(goal) and N < timeout:
485.
486.         diff_cte = - cte
487.
488.
489.         # ----------------------------------------
490.         # compute the CTE
491.
493.         estimate = filter.get_position()
494.
495.         while True:
496.             # some basic vector calculations
497.             dx = spath[index+1][0] - spath[index][0]
498.             dy = spath[index+1][1] - spath[index][1]
499.             drx = estimate[0] - spath[index][0]
500.             dry = estimate[1] - spath[index][1]
501.
502.             # u is the robot estimate projectes onto the path segment
503.             u = (drx * dx + dry * dy) / (dx * dx + dy * dy)
504.
505.             # the cte is the estimate projected onto the normal of the path segment
506.             cte = (dry * dx - drx * dy) / (dx * dx + dy * dy)
507.
508.             # pick the next path segment
509.             if u > 1.0:
510.                 index += 1
511.             else:
512.                 break
513.
514.
515.         # ----------------------------------------
516.
517.
518.         diff_cte += cte
519.
520.         steer = - params[0] * cte - params[1] * diff_cte
521.
522.         myrobot = myrobot.move(grid, steer, speed)
523.         filter.move(grid, steer, speed)
524.
525.         Z = myrobot.sense()
526.         filter.sense(Z)
527.
528.         if not myrobot.check_collision(grid):
529.             print '##### Collision ####'
530.
531.         err += (cte ** 2)
532.         N += 1
533.
534.         actualpath.append ([myrobot.x, myrobot.y])
535.
536.         if printflag:
537.             print myrobot, cte, index, u
538.
539.     return [actualpath, myrobot.check_goal(goal), myrobot.num_collisions, myrobot.num_steps]
540.
541.
542. # ------------------------------------------------
543. #
544. # this is our main routine
545. #
546.
547. def main(grid, init, goal, steering_noise, distance_noise, measurement_noise,
548.      weight_data, weight_smooth, p_gain, d_gain):
549.
550.     path = plan(grid, init, goal)
551.     path.astar()
552.     path.smooth(weight_data, weight_smooth)
553.
554.     import matplotlib.pyplot as mtpl
555.     mtpl.figure()
556.     mtpl.hold(True)
557.     obstacles = []
558.     for y in range(len(grid)):
559.         for x in range(len(grid[0])):
560.             if grid [y][x] != 0:
561.                 obstacles.append ([x, y])
562.     mtpl.scatter([p[0] for p in obstacles],[p[1] for p in obstacles], c='r', s=200.0, marker='o')
563.
564.     mtpl.plot([p[1] for p in path.spath],[p[0] for p in path.spath], 'g', label='smoothed path')
565.
566.     result = run(grid, goal, path.spath, [p_gain, d_gain])
567.     actualpath = result [0]
568.     mtpl.plot([p[1] for p in actualpath], [p[0] for p in actualpath], 'b', label='actual car path')
569.
570.     mtpl.gca().invert_yaxis()
571.
572.     mtpl.legend()
573.     mtpl.show()
574.
575.     return result [1:4]
576.
577.
578.
579.
580. # ------------------------------------------------
581. #
582. # input data and parameters
583. #
584.
585.
586. # grid format:
587. #   0 = navigable space
588. #   1 = occupied space
589.
590. grid = [[0, 1, 0, 0, 0, 0],
591.         [0, 1, 0, 1, 1, 0],
592.         [0, 1, 0, 1, 0, 0],
593.         [0, 0, 0, 1, 0, 1],
594.         [0, 1, 0, 1, 0, 0]]
595.
596.
597. init = [0, 0]
598. goal = [len(grid)-1, len(grid[0])-1]
599.
600.
601. steering_noise    = 0.1
602. distance_noise    = 0.03
603. measurement_noise = 0.3
604.
605. #### ADJUST THESE PARAMETERS ######
606.
607. weight_data       = 0.1
608. weight_smooth     = 0.2
609. p_gain            = 2.0
610. d_gain            = 6.0
611.
612. ###################################
613.
614. print main(grid, init, goal, steering_noise, distance_noise, measurement_noise,
615.            weight_data, weight_smooth, p_gain, d_gain)
616.
617.
618.
619.
620. def twiddle(init_params):
621.     n_params   = len(init_params)
622.     dparams    = [1.0 for row in range(n_params)]
623.     params     = [0.0 for row in range(n_params)]
624.     K = 10
625.
626.     for i in range(n_params):
627.         params[i] = init_params[i]
628.
629.
630.     best_error = 0.0;
631.     for k in range(K):
632.         ret = main(grid, init, goal,
633.                    steering_noise, distance_noise, measurement_noise,
634.                    params[0], params[1], params[2], params[3])
635.         if ret[0]:
636.             best_error += ret[1] * 100 + ret[2]
637.         else:
638.             best_error += 99999
639.     best_error = float(best_error) / float(k+1)
640.     print best_error
641.
642.     n = 0
643.     while sum(dparams) > 0.0000001:
644.         for i in range(len(params)):
645.             params[i] += dparams[i]
646.             err = 0
647.             for k in range(K):
648.                 ret = main(grid, init, goal,
649.                            steering_noise, distance_noise, measurement_noise,
650.                            params[0], params[1], params[2], params[3], best_error)
651.                 if ret[0]:
652.                     err += ret[1] * 100 + ret[2]
653.                 else:
654.                     err += 99999
655.             print float(err) / float(k+1)
656.             if err < best_error:
657.                 best_error = float(err) / float(k+1)
658.                 dparams[i] *= 1.1
659.             else:
660.                 params[i] -= 2.0 * dparams[i]
661.                 err = 0
662.                 for k in range(K):
663.                     ret = main(grid, init, goal,
664.                                steering_noise, distance_noise, measurement_noise,
665.                                params[0], params[1], params[2], params[3], best_error)
666.                     if ret[0]:
667.                         err += ret[1] * 100 + ret[2]
668.                     else:
669.                         err += 99999
670.                 print float(err) / float(k+1)
671.                 if err < best_error:
672.                     best_error = float(err) / float(k+1)
673.                     dparams[i] *= 1.1
674.                 else:
675.                     params[i] += dparams[i]
676.                     dparams[i] *= 0.5
677.         n += 1
678.         print 'Twiddle #', n, params, ' -> ', best_error
679.     print ' '
680.     return params
681.
682.
683. #twiddle([weight_data, weight_smooth, p_gain, d_gain])