ecki/Robotics-at-Home-with-Raspberry-Pi-Pico
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Chapter 13 reduction

Browse Source
This commit is contained in:
Danny Staple
2023-01-08 23:11:35 +00:00
parent 68cfd39a19
commit 2db7a0fe3c
10 changed files with 28 additions and 380 deletions
Download Patch File Download Diff File Expand all files Collapse all files
ch-13/2.3-bluetooth/robot/arena.py
-59
View File
@@ -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
-60
View File
@@ -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
-60
View File
@@ -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
-60
View File
@@ -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
-60
View File
@@ -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
-59
View File
@@ -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
+13 -3
View File
@@ -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
+1 -3
View File
@@ -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
+12 -14
View File
@@ -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(
(self.poses.shape[0], 2), dtype=np.float)
distance_sensor_right = np.zeros(
(self.poses.shape[0], 2), dtype=np.float)
left_sensor = np.zeros((self.poses.shape[0], 2), dtype=np.float)
right_sensor = 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
distance_sensor_left[:, 1] = self.poses[:, 1] + np.sin(poses_left_90) * robot.distance_sensor_side_mm
distance_sensor_left[:, 0] += np.cos(self.poses[:, 2]) * (self.distance_sensors.left + robot.distance_sensor_forward_mm)
distance_sensor_left[:, 1] += np.sin(self.poses[:, 2]) * (self.distance_sensors.left + robot.distance_sensor_forward_mm)
left_sensor[:, 0] = self.poses[:, 0] + np.cos(poses_left_90) * robot.dist_side_mm
left_sensor[:, 1] = self.poses[:, 1] + np.sin(poses_left_90) * robot.dist_side_mm
left_sensor[:, 0] += np.cos(self.poses[:, 2]) * (self.distance_sensors.left + robot.dist_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
distance_sensor_right[:, 1] = self.poses[:, 1] + np.sin(poses_right_90) * robot.distance_sensor_side_mm
distance_sensor_right[:, 0] += np.cos(self.poses[:, 2]) * (self.distance_sensors.right + robot.distance_sensor_forward_mm)
distance_sensor_right[:, 1] += np.sin(self.poses[:, 2]) * (self.distance_sensors.right + robot.distance_sensor_forward_mm)
right_sensor[:, 0] = self.poses[:, 0] + np.cos(poses_right_90) * robot.dist_side_mm
right_sensor[:, 1] = self.poses[:, 1] + np.sin(poses_right_90) * robot.dist_side_mm
right_sensor[:, 0] += np.cos(self.poses[:, 2]) * (self.distance_sensors.right + robot.dist_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(distance_sensor_right[index,0], distance_sensor_right[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(right_sensor[index,0], right_sensor[index,1])
weights[index] *= sensor_weight
return weights
ch-13/4.3-monte-carlo/robot/robot.py
+2 -2
View File
@@ -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
distance_sensor_forward_mm = 66 # approx mm
dist_side_mm = 37 # approx mm
dist_forward_mm = 66 # approx mm
motor_A2 = pwmio.PWMOut(board.GP17, frequency=100)
motor_A1 = pwmio.PWMOut(board.GP16, frequency=100)