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/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()