Chapter 13 reduction

ch-13/2.3-bluetooth/robot/arena.py

@@ -16,62 +16,3 @@ boundary_lines = [
 width = 1500
 height = 1500
 
-
-grid_cell_size = 50
-overscan = 10 # 10 each way
-
-
-def get_distance_to_segment(x, y, segment):
-    """Return the distance from the point to the segment.
-    Segment -> ((x1, y1), (x2, y2))
-    All segments are horizontal or vertical.
-    """
-    x1, y1 = segment[0]
-    x2, y2 = segment[1]
-    # if the segment is horizontal, the point will be closest to the y value of the segment
-    if y1 == y2 and x >= min(x1, x2) and x <= max(x1, x2):
-        return abs(y - y1)
-    # if the segment is vertical, the point will be closest to the x value of the segment
-    if x1 == x2 and y >= min(y1, y2) and y <= max(y1, y2):
-        return abs(x - x1)
-    # the point will be closest to one of the end points
-    return np.sqrt(
-        min(
-            (x - x1) ** 2 + (y - y1) ** 2, 
-            (x - x2) ** 2 + (y - y2) ** 2
-        )
-    )
-
-
-def get_distance_likelihood(x, y):
-    """Return the distance from the point to the nearest segment as a decay function."""
-    min_distance = None
-    for segment in boundary_lines:
-        distance = get_distance_to_segment(x, y, segment)
-        if min_distance is None or distance < min_distance:
-            min_distance = distance
-    return 1.0 / (1 + min_distance/250) ** 2
-
-
-grid_cell_size = 50
-overscan = 10 # 10 each way
-
-# beam endpoint model
-def make_distance_grid():
-    """Take the boundary lines. With and overscan of 10 cells, and grid cell size of 5cm (50mm),
-    make a grid of the distance to the nearest boundary line.
-    """
-    grid = np.zeros((
-            width // grid_cell_size + 2 * overscan, 
-            height // grid_cell_size + 2 * overscan
-        ), dtype=np.float)
-    for x in range(grid.shape[0]):
-        column_x = x * grid_cell_size - (overscan * grid_cell_size)
-        for y in range(grid.shape[1]):
-            row_y = y * grid_cell_size - (overscan * grid_cell_size)
-            grid[x, y] = get_distance_likelihood(
-                column_x, row_y
-            )
-    return grid
-
-distance_grid = make_distance_grid()

ch-13/3.1-displaying-a-pose/robot/arena.py

@@ -15,63 +15,3 @@ boundary_lines = [
 
 width = 1500
 height = 1500
-
-
-grid_cell_size = 50
-overscan = 10 # 10 each way
-
-
-def get_distance_to_segment(x, y, segment):
-    """Return the distance from the point to the segment.
-    Segment -> ((x1, y1), (x2, y2))
-    All segments are horizontal or vertical.
-    """
-    x1, y1 = segment[0]
-    x2, y2 = segment[1]
-    # if the segment is horizontal, the point will be closest to the y value of the segment
-    if y1 == y2 and x >= min(x1, x2) and x <= max(x1, x2):
-        return abs(y - y1)
-    # if the segment is vertical, the point will be closest to the x value of the segment
-    if x1 == x2 and y >= min(y1, y2) and y <= max(y1, y2):
-        return abs(x - x1)
-    # the point will be closest to one of the end points
-    return np.sqrt(
-        min(
-            (x - x1) ** 2 + (y - y1) ** 2, 
-            (x - x2) ** 2 + (y - y2) ** 2
-        )
-    )
-
-
-def get_distance_likelihood(x, y):
-    """Return the distance from the point to the nearest segment as a decay function."""
-    min_distance = None
-    for segment in boundary_lines:
-        distance = get_distance_to_segment(x, y, segment)
-        if min_distance is None or distance < min_distance:
-            min_distance = distance
-    return 1.0 / (1 + min_distance/250) ** 2
-
-
-grid_cell_size = 50
-overscan = 10 # 10 each way
-
-# beam endpoint model
-def make_distance_grid():
-    """Take the boundary lines. With and overscan of 10 cells, and grid cell size of 5cm (50mm),
-    make a grid of the distance to the nearest boundary line.
-    """
-    grid = np.zeros((
-            width // grid_cell_size + 2 * overscan, 
-            height // grid_cell_size + 2 * overscan
-        ), dtype=np.float)
-    for x in range(grid.shape[0]):
-        column_x = x * grid_cell_size - (overscan * grid_cell_size)
-        for y in range(grid.shape[1]):
-            row_y = y * grid_cell_size - (overscan * grid_cell_size)
-            grid[x, y] = get_distance_likelihood(
-                column_x, row_y
-            )
-    return grid
-
-distance_grid = make_distance_grid()

ch-13/3.2-collision-avoidance/robot/arena.py

@@ -15,63 +15,3 @@ boundary_lines = [
 
 width = 1500
 height = 1500
-
-
-grid_cell_size = 50
-overscan = 10 # 10 each way
-
-
-def get_distance_to_segment(x, y, segment):
-    """Return the distance from the point to the segment.
-    Segment -> ((x1, y1), (x2, y2))
-    All segments are horizontal or vertical.
-    """
-    x1, y1 = segment[0]
-    x2, y2 = segment[1]
-    # if the segment is horizontal, the point will be closest to the y value of the segment
-    if y1 == y2 and x >= min(x1, x2) and x <= max(x1, x2):
-        return abs(y - y1)
-    # if the segment is vertical, the point will be closest to the x value of the segment
-    if x1 == x2 and y >= min(y1, y2) and y <= max(y1, y2):
-        return abs(x - x1)
-    # the point will be closest to one of the end points
-    return np.sqrt(
-        min(
-            (x - x1) ** 2 + (y - y1) ** 2, 
-            (x - x2) ** 2 + (y - y2) ** 2
-        )
-    )
-
-
-def get_distance_likelihood(x, y):
-    """Return the distance from the point to the nearest segment as a decay function."""
-    min_distance = None
-    for segment in boundary_lines:
-        distance = get_distance_to_segment(x, y, segment)
-        if min_distance is None or distance < min_distance:
-            min_distance = distance
-    return 1.0 / (1 + min_distance/250) ** 2
-
-
-grid_cell_size = 50
-overscan = 10 # 10 each way
-
-# beam endpoint model
-def make_distance_grid():
-    """Take the boundary lines. With and overscan of 10 cells, and grid cell size of 5cm (50mm),
-    make a grid of the distance to the nearest boundary line.
-    """
-    grid = np.zeros((
-            width // grid_cell_size + 2 * overscan, 
-            height // grid_cell_size + 2 * overscan
-        ), dtype=np.float)
-    for x in range(grid.shape[0]):
-        column_x = x * grid_cell_size - (overscan * grid_cell_size)
-        for y in range(grid.shape[1]):
-            row_y = y * grid_cell_size - (overscan * grid_cell_size)
-            grid[x, y] = get_distance_likelihood(
-                column_x, row_y
-            )
-    return grid
-
-distance_grid = make_distance_grid()

ch-13/3.3-moving-with-encoders/robot/arena.py

@@ -15,63 +15,3 @@ boundary_lines = [
 
 width = 1500
 height = 1500
-
-
-grid_cell_size = 50
-overscan = 10 # 10 each way
-
-
-def get_distance_to_segment(x, y, segment):
-    """Return the distance from the point to the segment.
-    Segment -> ((x1, y1), (x2, y2))
-    All segments are horizontal or vertical.
-    """
-    x1, y1 = segment[0]
-    x2, y2 = segment[1]
-    # if the segment is horizontal, the point will be closest to the y value of the segment
-    if y1 == y2 and x >= min(x1, x2) and x <= max(x1, x2):
-        return abs(y - y1)
-    # if the segment is vertical, the point will be closest to the x value of the segment
-    if x1 == x2 and y >= min(y1, y2) and y <= max(y1, y2):
-        return abs(x - x1)
-    # the point will be closest to one of the end points
-    return np.sqrt(
-        min(
-            (x - x1) ** 2 + (y - y1) ** 2, 
-            (x - x2) ** 2 + (y - y2) ** 2
-        )
-    )
-
-
-def get_distance_likelihood(x, y):
-    """Return the distance from the point to the nearest segment as a decay function."""
-    min_distance = None
-    for segment in boundary_lines:
-        distance = get_distance_to_segment(x, y, segment)
-        if min_distance is None or distance < min_distance:
-            min_distance = distance
-    return 1.0 / (1 + min_distance/250) ** 2
-
-
-grid_cell_size = 50
-overscan = 10 # 10 each way
-
-# beam endpoint model
-def make_distance_grid():
-    """Take the boundary lines. With and overscan of 10 cells, and grid cell size of 5cm (50mm),
-    make a grid of the distance to the nearest boundary line.
-    """
-    grid = np.zeros((
-            width // grid_cell_size + 2 * overscan, 
-            height // grid_cell_size + 2 * overscan
-        ), dtype=np.float)
-    for x in range(grid.shape[0]):
-        column_x = x * grid_cell_size - (overscan * grid_cell_size)
-        for y in range(grid.shape[1]):
-            row_y = y * grid_cell_size - (overscan * grid_cell_size)
-            grid[x, y] = get_distance_likelihood(
-                column_x, row_y
-            )
-    return grid
-
-distance_grid = make_distance_grid()

ch-13/3.4-movement-uncertainty/robot/arena.py

@@ -15,63 +15,3 @@ boundary_lines = [
 
 width = 1500
 height = 1500
-
-
-grid_cell_size = 50
-overscan = 10 # 10 each way
-
-
-def get_distance_to_segment(x, y, segment):
-    """Return the distance from the point to the segment.
-    Segment -> ((x1, y1), (x2, y2))
-    All segments are horizontal or vertical.
-    """
-    x1, y1 = segment[0]
-    x2, y2 = segment[1]
-    # if the segment is horizontal, the point will be closest to the y value of the segment
-    if y1 == y2 and x >= min(x1, x2) and x <= max(x1, x2):
-        return abs(y - y1)
-    # if the segment is vertical, the point will be closest to the x value of the segment
-    if x1 == x2 and y >= min(y1, y2) and y <= max(y1, y2):
-        return abs(x - x1)
-    # the point will be closest to one of the end points
-    return np.sqrt(
-        min(
-            (x - x1) ** 2 + (y - y1) ** 2, 
-            (x - x2) ** 2 + (y - y2) ** 2
-        )
-    )
-
-
-def get_distance_likelihood(x, y):
-    """Return the distance from the point to the nearest segment as a decay function."""
-    min_distance = None
-    for segment in boundary_lines:
-        distance = get_distance_to_segment(x, y, segment)
-        if min_distance is None or distance < min_distance:
-            min_distance = distance
-    return 1.0 / (1 + min_distance/250) ** 2
-
-
-grid_cell_size = 50
-overscan = 10 # 10 each way
-
-# beam endpoint model
-def make_distance_grid():
-    """Take the boundary lines. With and overscan of 10 cells, and grid cell size of 5cm (50mm),
-    make a grid of the distance to the nearest boundary line.
-    """
-    grid = np.zeros((
-            width // grid_cell_size + 2 * overscan, 
-            height // grid_cell_size + 2 * overscan
-        ), dtype=np.float)
-    for x in range(grid.shape[0]):
-        column_x = x * grid_cell_size - (overscan * grid_cell_size)
-        for y in range(grid.shape[1]):
-            row_y = y * grid_cell_size - (overscan * grid_cell_size)
-            grid[x, y] = get_distance_likelihood(
-                column_x, row_y
-            )
-    return grid
-
-distance_grid = make_distance_grid()

ch-13/4.1-generating-weights/robot/arena.py

@@ -28,62 +28,3 @@ def contains(x, y):
   if x > 1000 and y < 500:
     return False
   return True
-
-grid_cell_size = 50
-overscan = 10 # 10 each way
-
-
-def get_distance_to_segment(x, y, segment):
-    """Return the distance from the point to the segment.
-    Segment -> ((x1, y1), (x2, y2))
-    All segments are horizontal or vertical.
-    """
-    x1, y1 = segment[0]
-    x2, y2 = segment[1]
-    # if the segment is horizontal, the point will be closest to the y value of the segment
-    if y1 == y2 and x >= min(x1, x2) and x <= max(x1, x2):
-        return abs(y - y1)
-    # if the segment is vertical, the point will be closest to the x value of the segment
-    if x1 == x2 and y >= min(y1, y2) and y <= max(y1, y2):
-        return abs(x - x1)
-    # the point will be closest to one of the end points
-    return np.sqrt(
-        min(
-            (x - x1) ** 2 + (y - y1) ** 2, 
-            (x - x2) ** 2 + (y - y2) ** 2
-        )
-    )
-
-
-def get_distance_likelihood(x, y):
-    """Return the distance from the point to the nearest segment as a decay function."""
-    min_distance = None
-    for segment in boundary_lines:
-        distance = get_distance_to_segment(x, y, segment)
-        if min_distance is None or distance < min_distance:
-            min_distance = distance
-    return 1.0 / (1 + min_distance/250) ** 2
-
-
-grid_cell_size = 50
-overscan = 10 # 10 each way
-
-# beam endpoint model
-def make_distance_grid():
-    """Take the boundary lines. With and overscan of 10 cells, and grid cell size of 5cm (50mm),
-    make a grid of the distance to the nearest boundary line.
-    """
-    grid = np.zeros((
-            width // grid_cell_size + 2 * overscan, 
-            height // grid_cell_size + 2 * overscan
-        ), dtype=np.float)
-    for x in range(grid.shape[0]):
-        column_x = x * grid_cell_size - (overscan * grid_cell_size)
-        for y in range(grid.shape[1]):
-            row_y = y * grid_cell_size - (overscan * grid_cell_size)
-            grid[x, y] = get_distance_likelihood(
-                column_x, row_y
-            )
-    return grid
-
-distance_grid = make_distance_grid()

ch-13/4.2-modelling-distances/arena.py

@@ -16,6 +16,19 @@ boundary_lines = [
 width = 1500
 height = 1500
 
+def contains(x, y):
+  """Return True if the point is inside the arena.
+  if the point is inside the rectangle, but not inside the cutout, it's inside the arena.
+  """  
+  # is it inside the rectangle?
+  if x < 0 or x > width \
+    or y < 0 or y > height:
+    return False
+  # is it inside the cutout?
+  if x > 1000 and y < 500:
+    return False
+  return True
+
 
 grid_cell_size = 50
 overscan = 10 # 10 each way
@@ -53,9 +66,6 @@ def get_distance_likelihood(x, y):
     return 1.0 / (1 + min_distance/250) ** 2
 
 
-grid_cell_size = 50
-overscan = 10 # 10 each way
-
 # beam endpoint model
 def make_distance_grid():
     """Take the boundary lines. With and overscan of 10 cells, and grid cell size of 5cm (50mm),

ch-13/4.3-monte-carlo/robot/arena.py

@@ -29,6 +29,7 @@ def contains(x, y):
     return False
   return True
 
+
 grid_cell_size = 50
 overscan = 10 # 10 each way
 
@@ -65,9 +66,6 @@ def get_distance_likelihood(x, y):
     return 1.0 / (1 + min_distance/250) ** 2
 
 
-grid_cell_size = 50
-overscan = 10 # 10 each way
-
 # beam endpoint model
 def make_distance_grid():
     """Take the boundary lines. With and overscan of 10 cells, and grid cell size of 5cm (50mm),

ch-13/4.3-monte-carlo/robot/code.py

@@ -166,27 +166,25 @@ class Simulation:
 
     def observe_distance_sensors(self, weights):
         # modify the current weights based on the distance sensors
-        distance_sensor_left = np.zeros(
+        left_sensor = np.zeros((self.poses.shape[0], 2), dtype=np.float)
-            (self.poses.shape[0], 2), dtype=np.float)
+        right_sensor = np.zeros((self.poses.shape[0], 2), dtype=np.float)
-        distance_sensor_right = np.zeros(
-            (self.poses.shape[0], 2), dtype=np.float)
         # left sensor
         poses_left_90 = np.radians(self.poses[:, 2] + 90)
-        distance_sensor_left[:, 0] = self.poses[:, 0] + np.cos(poses_left_90) * robot.distance_sensor_side_mm
+        left_sensor[:, 0] = self.poses[:, 0] + np.cos(poses_left_90) * robot.dist_side_mm
-        distance_sensor_left[:, 1] = self.poses[:, 1] + np.sin(poses_left_90) * robot.distance_sensor_side_mm
+        left_sensor[:, 1] = self.poses[:, 1] + np.sin(poses_left_90) * robot.dist_side_mm
-        distance_sensor_left[:, 0] += np.cos(self.poses[:, 2]) * (self.distance_sensors.left + robot.distance_sensor_forward_mm)
+        left_sensor[:, 0] += np.cos(self.poses[:, 2]) * (self.distance_sensors.left + robot.dist_forward_mm)
-        distance_sensor_left[:, 1] += np.sin(self.poses[:, 2]) * (self.distance_sensors.left + robot.distance_sensor_forward_mm)
+        left_sensor[:, 1] += np.sin(self.poses[:, 2]) * (self.distance_sensors.left + robot.dist_forward_mm)
 
         # right sensor
         poses_right_90 = np.radians(self.poses[:, 2] - 90)
-        distance_sensor_right[:, 0] = self.poses[:, 0] + np.cos(poses_right_90) * robot.distance_sensor_side_mm
+        right_sensor[:, 0] = self.poses[:, 0] + np.cos(poses_right_90) * robot.dist_side_mm
-        distance_sensor_right[:, 1] = self.poses[:, 1] + np.sin(poses_right_90) * robot.distance_sensor_side_mm
+        right_sensor[:, 1] = self.poses[:, 1] + np.sin(poses_right_90) * robot.dist_side_mm
-        distance_sensor_right[:, 0] += np.cos(self.poses[:, 2]) * (self.distance_sensors.right + robot.distance_sensor_forward_mm)
+        right_sensor[:, 0] += np.cos(self.poses[:, 2]) * (self.distance_sensors.right + robot.dist_forward_mm)
-        distance_sensor_right[:, 1] += np.sin(self.poses[:, 2]) * (self.distance_sensors.right + robot.distance_sensor_forward_mm)
+        right_sensor[:, 1] += np.sin(self.poses[:, 2]) * (self.distance_sensors.right + robot.dist_forward_mm)
         # Look up the distance in the arena
         for index in range(self.poses.shape[0]):
-            sensor_weight = arena.get_distance_grid_at_point(distance_sensor_left[index,0], distance_sensor_left[index,1])
+            sensor_weight = arena.get_distance_grid_at_point(left_sensor[index,0], left_sensor[index,1])
-            sensor_weight += arena.get_distance_grid_at_point(distance_sensor_right[index,0], distance_sensor_right[index,1])
+            sensor_weight += arena.get_distance_grid_at_point(right_sensor[index,0], right_sensor[index,1])
             weights[index] *= sensor_weight
         return weights
 

ch-13/4.3-monte-carlo/robot/robot.py

@@ -17,8 +17,8 @@ ticks_to_mm = wheel_circumference_mm / ticks_per_revolution
 ticks_to_m = ticks_to_mm / 1000
 m_to_ticks = 1 / ticks_to_m
 wheelbase_mm = 170
-distance_sensor_side_mm = 37 # approx mm
+dist_side_mm = 37 # approx mm
-distance_sensor_forward_mm = 66 # approx mm
+dist_forward_mm = 66 # approx mm
 
 motor_A2 = pwmio.PWMOut(board.GP17, frequency=100)
 motor_A1 = pwmio.PWMOut(board.GP16, frequency=100)