bscmoczallarafael / kogrob-ha03 (public) (License: private) (since 2017-05-25) (hash sha1)
Houswork solution Nr. 3 for kognitive Robotik - HU Berlin
Clone URLs: https://rocketgit.com/user/bscmoczallarafael/kogrob-ha03 ssh://rocketgit@ssh.rocketgit.com/user/bscmoczallarafael/kogrob-ha03 git://git.rocketgit.com/user/bscmoczallarafael/kogrob-ha03
master
List of commits:
Subject Hash Author Date (UTC)
first push 69514078177c368ef34ebcb0ea20f3cc853dd78e B.Sc. Moczalla, Rafael 2017-05-25 11:29:50
Commit 69514078177c368ef34ebcb0ea20f3cc853dd78e - first push
Author: B.Sc. Moczalla, Rafael
Author date (UTC): 2017-05-25 11:29
Committer name: B.Sc. Moczalla, Rafael
Committer date (UTC): 2017-05-25 11:29
Parent(s):
Signing key:
Tree: 9bc49a297c710ca7be4abc2a78ac06c016f5fd38
File Lines added Lines deleted
draw.py 139 0
draw.pyc 0 0
nao.gif 0 0
particle_filter.py 204 0
File draw.py added (mode: 100644) (index 0000000..2c95a23)
1 import math
2 import turtle
3 import random
4
5 # add the shape first then set the turtle shape
6 nao = "nao.gif"
7 screen = turtle.Screen()
8 screen.addshape(nao)
9
10
11 turtle.tracer(50000, delay=0)
12 turtle.register_shape("dot", ((-3, -3), (-3, 3), (3, 3), (3, -3)))
13 turtle.register_shape("tri", ((-3, -2), (0, 3), (3, -2), (0, 0)))
14 turtle.speed(0)
15 turtle.title("Particle Filter Example")
16
17 UPDATE_EVERY = 0
18 DRAW_EVERY = 2
19
20
21 class Maze(object):
22 def __init__(self, maze):
23 self.maze = maze
24 self.width = len(maze[0])
25 self.height = len(maze)
26 turtle.setworldcoordinates(0, 0, self.width, self.height)
27 self.blocks = []
28 self.update_cnt = 0
29 self.one_px = float(turtle.window_width()) / float(self.width) / 2
30
31 self.beacons = []
32 for y, line in enumerate(self.maze):
33 for x, block in enumerate(line):
34 if block:
35 nb_y = self.height - y - 1
36 self.blocks.append((x, nb_y))
37 if block == 2:
38 self.beacons.extend(((x, nb_y), (x+1, nb_y), (x, nb_y+1), (x+1, nb_y+1)))
39
40 def draw(self):
41 for x, y in self.blocks:
42 turtle.up()
43 turtle.setposition(x, y)
44 turtle.down()
45 turtle.setheading(90)
46 turtle.begin_fill()
47 for _ in range(0, 4):
48 turtle.fd(1)
49 turtle.right(90)
50 turtle.end_fill()
51 turtle.up()
52
53 turtle.color("#00ffff")
54 for x, y in self.beacons:
55 turtle.setposition(x, y)
56 turtle.dot()
57 turtle.update()
58
59 def weight_to_color(self, weight):
60 return "#%02x00%02x" % (int(weight * 255), int((1 - weight) * 255))
61
62 def is_in(self, x, y):
63 if x < 0 or y < 0 or x > self.width or y > self.height:
64 return False
65 return True
66
67 def is_free(self, x, y):
68 if not self.is_in(x, y):
69 return False
70
71 yy = self.height - int(y) - 1
72 xx = int(x)
73 return self.maze[yy][xx] == 0
74
75 def show_mean(self, x, y, confident=False):
76 if confident:
77 turtle.color("#00AA00")
78 else:
79 turtle.color("#cccccc")
80 turtle.setposition(x, y)
81 turtle.shape("circle")
82 turtle.stamp()
83
84 def show_particles(self, particles):
85 self.update_cnt += 1
86 if UPDATE_EVERY > 0 and self.update_cnt % UPDATE_EVERY != 1:
87 return
88
89 turtle.clearstamps()
90 turtle.shape('tri')
91
92 draw_cnt = 0
93 px = {}
94 for p in particles:
95 draw_cnt += 1
96 if DRAW_EVERY == 0 or draw_cnt % DRAW_EVERY == 1:
97 # Keep track of which positions already have something
98 # drawn to speed up display rendering
99 scaled_x = int(p.x * self.one_px)
100 scaled_y = int(p.y * self.one_px)
101 scaled_xy = scaled_x * 10000 + scaled_y
102 if not scaled_xy in px:
103 px[scaled_xy] = 1
104 turtle.setposition(*p.xy)
105 turtle.setheading(90 - p.h)
106 turtle.color(self.weight_to_color(p.w))
107 turtle.stamp()
108
109 def show_robot(self, robot):
110 turtle.color("green")
111 turtle.shape(nao)
112 turtle.setposition(*robot.xy)
113 turtle.setheading(90 - robot.h)
114 turtle.stamp()
115 turtle.update()
116
117 def random_place(self):
118 x = random.uniform(0, self.width)
119 y = random.uniform(0, self.height)
120 return x, y
121
122 def random_free_place(self):
123 while True:
124 x, y = self.random_place()
125 if self.is_free(x, y):
126 return x, y
127
128 def distance(self, x1, y1, x2, y2):
129 return math.sqrt((x1 - x2) ** 2 + (y1 - y2) ** 2)
130
131 def distance_to_nearest_beacon(self, x, y):
132 d = 99999
133 for c_x, c_y in self.beacons:
134 distance = self.distance(c_x, c_y, x, y)
135 if distance < d:
136 d = distance
137 d_x, d_y = c_x, c_y
138
139 return d
File draw.pyc added (mode: 100644) (index 0000000..7581e7a)
File nao.gif added (mode: 100644) (index 0000000..32f3517)
File particle_filter.py added (mode: 100644) (index 0000000..2856bfb)
1 from __future__ import absolute_import
2 import random
3 import math
4 import bisect
5 from draw import Maze
6
7 # 0 - empty square
8 # 1 - occupied square
9 # 2 - occupied square with a beacon at each corner, detectable by the robot
10
11 maze_data = ((1, 1, 0, 0, 2, 0, 0, 0, 0, 1),
12 (1, 2, 0, 0, 1, 1, 0, 0, 0, 0),
13 (0, 1, 1, 0, 0, 0, 0, 1, 0, 1),
14 (0, 0, 0, 0, 1, 0, 0, 1, 1, 2),
15 (1, 1, 0, 1, 1, 2, 0, 0, 1, 0),
16 (1, 1, 1, 0, 1, 1, 1, 0, 2, 0),
17 (2, 0, 0, 0, 0, 0, 0, 0, 0, 0),
18 (1, 2, 0, 1, 1, 1, 1, 0, 0, 0),
19 (0, 0, 0, 0, 1, 0, 0, 0, 1, 0),
20 (0, 0, 1, 0, 0, 2, 1, 1, 1, 0))
21
22 PARTICLE_COUNT = 2000 # Total number of particles
23
24 ROBOT_HAS_COMPASS = True
25 # ------------------------------------------------------------------------
26 # Some utility functions
27
28
29 def add_noise(level, *coords):
30 return [x + random.uniform(-level, level) for x in coords]
31
32
33 def add_little_noise(*coords):
34 return add_noise(0.02, *coords)
35
36
37 def add_some_noise(*coords):
38 return add_noise(0.1, *coords)
39
40 # This is just a gaussian kernel I pulled out of my hat, to transform
41 # values near to robbie's measurement => 1, further away => 0
42 sigma2 = 0.9 ** 2
43
44
45 def w_gauss(a, b):
46 error = a - b
47 g = math.e ** -(error ** 2 / (2 * sigma2))
48 return g
49 # ------------------------------------------------------------------------
50
51
52 def compute_mean_point(particles):
53 """
54 Compute the mean for all particles that have a reasonably good weight.
55 This is not part of the particle filter algorithm but rather an
56 addition to show the "best belief" for current position.
57 """
58
59 m_x, m_y, m_count = 0, 0, 0
60 for p in particles:
61 m_count += p.w
62 m_x += p.x * p.w
63 m_y += p.y * p.w
64
65 if m_count == 0:
66 return -1, -1, False
67
68 m_x /= m_count
69 m_y /= m_count
70
71 # Now compute how good that mean is -- check how many particles
72 # actually are in the immediate vicinity
73 m_count = 0
74 for p in particles:
75 if world.distance(p.x, p.y, m_x, m_y) < 1:
76 m_count += 1
77
78 return m_x, m_y, m_count > PARTICLE_COUNT * 0.95
79 # ------------------------------------------------------------------------
80
81
82 class Particle(object):
83 def __init__(self, x, y, heading=None, w=1, noisy=False):
84 if heading is None:
85 heading = random.uniform(0, 360)
86 if noisy:
87 x, y, heading = add_some_noise(x, y, heading)
88
89 self.x = x # x-Position
90 self.y = y # y-Position
91 self.h = heading # Ausrichtung
92 self.w = w # Gewicht
93
94 def __repr__(self):
95 return "(%f, %f, w=%f)" % (self.x, self.y, self.w)
96
97 @property
98 def xy(self):
99 return self.x, self.y
100
101 @property
102 def xyh(self):
103 return self.x, self.y, self.h
104
105 @classmethod
106 def create_random(cls, count, maze):
107 return [cls(*maze.random_free_place()) for _ in range(0, count)]
108
109 def read_sensor(self, maze):
110 """
111 Find distance to nearest beacon.
112 """
113 return maze.distance_to_nearest_beacon(*self.xy)
114
115 def advance_by(self, speed, checker=None, noisy=False):
116 h = self.h
117 if noisy:
118 speed, h = add_little_noise(speed, h)
119 h += random.uniform(-3, 3) # needs more noise to disperse better
120 r = math.radians(h)
121 dx = math.sin(r) * speed
122 dy = math.cos(r) * speed
123 if checker is None or checker(self, dx, dy):
124 self.move_by(dx, dy)
125 return True
126 return False
127
128 def move_by(self, x, y):
129 self.x += x
130 self.y += y
131 # ------------------------------------------------------------------------
132
133
134 class Robot(Particle):
135 speed = 0.2
136
137 def __init__(self, maze):
138 super(Robot, self).__init__(*maze.random_free_place(), heading=90)
139 self.chose_random_direction()
140 self.step_count = 0
141
142 def chose_random_direction(self):
143 heading = random.uniform(0, 360)
144 self.h = heading
145
146 def read_sensor(self, maze):
147 """
148 Poor robot, it's sensors are noisy and pretty strange,
149 it only can measure the distance to the nearest beacon(!)
150 and is not very accurate at that too!
151 """
152 return add_little_noise(super(Robot, self).read_sensor(maze))[0]
153
154 def move(self, maze):
155 """
156 Move the robot. Note that the movement is stochastic too.
157 """
158 while True:
159 self.step_count += 1
160 if self.advance_by(self.speed, noisy=True, checker=lambda r, dx, dy: maze.is_free(r.x+dx, r.y+dy)):
161 break
162 # Bumped into something or too long in same direction,
163 # chose random new direction
164 self.chose_random_direction()
165
166 # ------------------------------------------------------------------------
167
168 world = Maze(maze_data)
169 world.draw()
170
171 # initial distribution assigns each particle an equal probability
172 particles = Particle.create_random(PARTICLE_COUNT, world)
173 nao = Robot(world)
174
175 while True:
176 # Read Robot's sensor
177 r_d = nao.read_sensor(world)
178
179 # TODO: Update particle weight according to how good every particle matches
180 # sensorupdate()
181
182 # ---------- Show current state ----------
183 world.show_particles(particles)
184 # world.show_mean(m_x, m_y, m_confident)
185 world.show_robot(nao)
186
187 # ---------- Shuffle particles ----------
188 new_particles = []
189
190 # TODO: Normalise weights
191 # normalize() # fuer Resampling
192
193 # TODO: update Samples
194 # resample()
195
196 # ---------- Move things ----------
197 old_heading = nao.h
198 nao.move(world)
199 d_h = nao.h - old_heading
200
201 # Move particles according to my belief of movement
202 for p in particles:
203 p.h += d_h # in case robot changed heading, swirl particle heading too
204 p.advance_by(nao.speed)
Hints:
Before first commit, do not forget to setup your git environment:
git config --global user.name "your_name_here"
git config --global user.email "your@email_here"

Clone this repository using HTTP(S):
git clone https://rocketgit.com/user/bscmoczallarafael/kogrob-ha03

Clone this repository using ssh (do not forget to upload a key first):
git clone ssh://rocketgit@ssh.rocketgit.com/user/bscmoczallarafael/kogrob-ha03

Clone this repository using git:
git clone git://git.rocketgit.com/user/bscmoczallarafael/kogrob-ha03

You are allowed to anonymously push to this repository.
This means that your pushed commits will automatically be transformed into a merge request:
... clone the repository ...
... make some changes and some commits ...
git push origin main