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

Beam endpoint works.

Browse Source
This commit is contained in:
Danny Staple
2022-12-14 23:54:31 +00:00
parent f57add5106
commit b2228889e8
5 changed files with 123 additions and 168 deletions
Download Patch File Download Diff File Expand all files Collapse all files
ch-13/full-version/computer/render_distance_grid.py
+27
View File
@@ -0,0 +1,27 @@
import arena
from matplotlib import pyplot as plt
import numpy as np
def draw_arena_boundaries(arena):
for line in arena:
plt.plot([line[0][0], line[1][0]], [line[0][1], line[1][1]], color="red")
def draw_distance_grid(ax):
overscan_size = arena.overscan * arena.grid_cell_size
ax.imshow(
arena.distance_grid.T,
extent = [-overscan_size, arena.width + overscan_size, -overscan_size, arena.height + overscan_size],
origin="lower",
cmap="gray",
norm="log",
)
fig, ax = plt.subplots()
draw_arena_boundaries(arena.boundary_lines)
print("Value at 0, 1500 is", arena.get_distance_grid_at_point(0, 1500))
# print("Value at 1000, 500 is", arena.get_distance_grid_at_point(1000, 500))
# print("Value at 500, 1000 is", arena.get_distance_grid_at_point(500, 1000))
# print("Value at 550, 1000 is", arena.get_distance_grid_at_point(550, 1000))
draw_distance_grid(ax)
plt.show()
ch-13/full-version/computer/test_arena.py
+24
View File
@@ -0,0 +1,24 @@
from unittest import TestCase
import math
import arena
class TestArena(TestCase):
def test_get_point_to_distance_segment_1(self):
segment = ((0, 0), (0, 1500))
for x in range(0, 1500):
for y in (0, 500, 1000):
self.assertEqual(arena.get_point_distance_to_segment(x, y, segment), x)
def test_get_point_to_distance_segment_2(self):
segment = ((0, 0), (1500, 0))
for y in range(0, 1500):
for x in (0, 500, 1000):
self.assertEqual(arena.get_point_distance_to_segment(x, y, segment), y)
def test_get_point_distance_to_nearest_segment(self):
segments = [
[(0, 1500), (1500, 1500)],
]
for y in range(1500):
for x in (0, 500, 1000):
self.assertEqual(arena.get_point_distance_to_nearest_segment(segments, x, y), 1500 - y)
ch-13/full-version/robot/arena.py
+50 -52
View File
@@ -1,5 +1,8 @@
"""Represent the lines and target zone of the arena"""
import math
try:
from ulab import numpy as np
except ImportError:
import numpy as np
boundary_lines = [
[(0,0), (0, 1500)],
@@ -29,60 +32,55 @@ def point_is_inside_arena(x, y):
return False
return True
## intention - we can use a distance squared function to avoid the square root, and just square the distance sensor readings too.
def get_ray_distance_to_segment_squared(ray_x, ray_y, ray_tan, ray_heading, segment):
"""Return the distance squared from the ray origin to the intersection point along the given ray heading.
The segments are boundary lines, which will be horizontal or vertical, and have known lengths.
The ray can have any heading, and will be infinite in length.
Ray -> (x, y, heading)
ray_tan -> tangent of the heading (optimization)
def get_point_distance_to_segment(x, y, segment):
"""Return the distance squared from the point to the segment.
Segment -> ((x1, y1), (x2, y2))
All segments are horizontal or vertical.
"""
segment_x1, segment_y1 = segment[0]
segment_x2, segment_y2 = segment[1]
# if the segment is horizontal, the ray will intersect it at a known y value
if segment_y1 == segment_y2:
# if the ray is horizontal, it will never intersect the segment
if ray_heading == 0:
return None
# calculate the x value of the intersection point
intersection_x = ray_x + (segment_y1 - ray_y) / ray_tan
# is the intersection point on the segment?
if intersection_x > max(segment_x1, segment_x2) or intersection_x < min(segment_x1, segment_x2):
return None
# calculate the distance from the ray origin to the intersection point
return (intersection_x - ray_x) ** 2 + (segment_y1 - ray_y) ** 2
# if the segment is vertical, the ray will intersect it at a known x value
if segment_x1 == segment_x2:
# if the ray is vertical, it will never intersect the segment
if ray_heading == math.pi / 2:
return None
# calculate the y value of the intersection point
intersection_y = ray_y + (segment_x1 - ray_x) * ray_tan
# is the intersection point on the segment?
if intersection_y > max(segment_y1, segment_y2) or intersection_y < min(segment_y1, segment_y2):
return None
# calculate the distance from the ray origin to the intersection point
return (intersection_y - ray_y) ** 2 + (segment_x1 - ray_x) ** 2
else:
raise Exception("Segment is not horizontal or vertical")
# if the segment is horizontal, the point will be closest to the y value of the segment
if segment_y1 == segment_y2 and x >= min(segment_x1, segment_x2) and x <= max(segment_x1, segment_x2):
return abs(y - segment_y1)
# if the segment is vertical, the point will be closest to the x value of the segment
if segment_x1 == segment_x2 and y >= min(segment_y1, segment_y2) and y <= max(segment_y1, segment_y2):
return abs(x - segment_x1)
# the point will be closest to one of the end points
return np.sqrt(min((x - segment_x1) ** 2 + (y - segment_y1) ** 2, (x - segment_x2) ** 2 + (y - segment_y2) ** 2))
def get_ray_distance_squared_to_nearest_boundary_segment(ray):
"""Return the distance from the ray origin to the intersection point along the given ray heading.
The segments are boundary lines, which will be horizontal or vertical, and have known lengths.
The ray can have any heading, and will be infinite in length.
Ray -> (x, y, heading)
def get_point_decay_from_nearest_segment(segments, x, y):
"""Return the distance from the point to the nearest segment as a decay function."""
max_decay = None
for segment in segments:
decay = 1.0 / max(1, get_point_distance_to_segment(x, y, segment))
if max_decay is None or decay > max_decay:
max_decay = decay
return max_decay
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.
"""
# find the distance to each segment
distances = []
ray_x, ray_y, ray_heading = ray
ray_tan = math.tan(ray_heading)
for segment in boundary_lines:
distance_squared = get_ray_distance_to_segment_squared(ray_x, ray_y, ray_tan, ray_heading, segment)
if distance_squared is not None:
distances.append(distance_squared)
# return the minimum distance
if distances:
return min(distances)
else:
return None
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]):
value = get_point_decay_from_nearest_segment(boundary_lines, column_x, y * grid_cell_size - (overscan * grid_cell_size))
grid[x, y] = value
return grid
distance_grid = make_distance_grid()
def get_distance_grid_at_point(x, y):
"""Return the distance grid value at the given point."""
grid_x = int(x // grid_cell_size + overscan)
grid_y = int(y // grid_cell_size + overscan)
if grid_x < 0 or grid_x >= distance_grid.shape[0] or grid_y < 0 or grid_y >= distance_grid.shape[1]:
return 0
return distance_grid[grid_x, grid_y]
ch-13/full-version/robot/code.py
+22 -86
View File
@@ -4,19 +4,10 @@ import random
from ulab import numpy as np
import arena
import robot
import math
import time
# initial sample set - uniform
# then apply sensor model
# then resample
# then apply motion model
# and repeat
class VaryingWallAvoid:
def __init__(self):
self.speed = 0.6
self.last_call = time.monotonic()
def speed_from_distance(self, distance):
limited_error = min(distance, 300) * self.speed
@@ -26,23 +17,19 @@ class VaryingWallAvoid:
return motor_speed
def update(self, left_distance, right_distance):
# Currently being called every 1.6 seconds - that is far too long.
print("Since last call:", time.monotonic() - self.last_call)
left = self.speed_from_distance(left_distance)
right = self.speed_from_distance(right_distance)
# print("left speed:", left, "right speed:", right)
robot.set_left(left)
robot.set_right(right)
self.last_call = time.monotonic()
triangular_proportion = math.sqrt(6) / 2
triangular_proportion = np.sqrt(6) / 2
def get_triangular_sample(mean, standard_deviation):
base = triangular_proportion * (random.uniform(-standard_deviation, standard_deviation) + random.uniform(-standard_deviation, standard_deviation))
return mean + base
class Simulation:
def __init__(self):
self.population_size = 50
self.population_size = 100
self.left_distance = 100
self.right_distance = 100
self.imu_mix = 0.3 * 0.5
@@ -61,7 +48,6 @@ class Simulation:
self.collision_avoider = VaryingWallAvoid()
async def apply_sensor_model(self):
# Timing is about 0.65s
# Based on vl53l1x sensor readings, create weight for each pose.
# vl53l1x standard dev is +/- 5 mm. Each distance is a mean reading
# we will first determine sensor positions based on poses
@@ -69,69 +55,49 @@ class Simulation:
# then check this projected position against occupancy grid
# and weight accordingly
# distance sensor positions projected forward. x, y, heading, reading
fn_start = time.monotonic()
print("Starting apply sensor model")
distance_sensor_left_rays = np.zeros(
(self.poses.shape[0], 3), dtype=np.float)
distance_sensor_right_rays = np.zeros(
(self.poses.shape[0], 3), dtype=np.float)
# distance sensor positions projected forward. x, y
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)
# sensors - they are facing forward, either side of the robot. Project them out to the sides
# based on each poses heading and turn sensors into rays,
# left sensor
poses_left_90 = np.radians(self.poses[:, 2] + 90)
# print("poses_left_90_shape:",poses_left_90.shape, "distance_sensor_positions_shape:",distance_sensor_positions.shape, "poses_shape:",self.poses.shape)
distance_sensor_left_rays[:, 0] = self.poses[:, 0] + np.cos(poses_left_90) * robot.distance_sensor_from_middle
distance_sensor_left_rays[:, 1] = self.poses[:, 1] + np.sin(poses_left_90) * robot.distance_sensor_from_middle
distance_sensor_left_rays[:, 2] = np.radians(self.poses[:, 2])
distance_sensor_left[:, 0] = self.poses[:, 0] + np.cos(poses_left_90) * robot.distance_sensor_from_middle
distance_sensor_left[:, 1] = self.poses[:, 1] + np.sin(poses_left_90) * robot.distance_sensor_from_middle
# now project forward by distance sensor range
distance_sensor_left[:, 0] += np.cos(self.poses[:, 2]) * self.left_distance
distance_sensor_left[:, 1] += np.sin(self.poses[:, 2]) * self.left_distance
# right sensor
poses_right_90 = np.radians(self.poses[:, 2] - 90)
distance_sensor_right_rays[:, 0] = self.poses[:, 0] + np.cos(poses_right_90) * robot.distance_sensor_from_middle
distance_sensor_right_rays[:, 1] = self.poses[:, 1] + np.sin(poses_right_90) * robot.distance_sensor_from_middle
distance_sensor_right_rays[:, 2] = np.radians(self.poses[:, 2])
# for each sensor position, find the distance to the nearest obstacle
distance_sensor_standard_dev = 5
dl_squared = self.left_distance ** 2
dr_squared = self.right_distance ** 2
distance_sensor_right[:, 0] = self.poses[:, 0] + np.cos(poses_right_90) * robot.distance_sensor_from_middle
distance_sensor_right[:, 1] = self.poses[:, 1] + np.sin(poses_right_90) * robot.distance_sensor_from_middle
# now project forward by distance sensor range
distance_sensor_right[:, 0] += np.cos(self.poses[:, 2]) * self.left_distance
distance_sensor_right[:, 1] += np.sin(self.poses[:, 2]) * self.left_distance
await asyncio.sleep(0)
print("Time to calculate sensor positions:", time.monotonic() - fn_start)
fn_start = time.monotonic()
# weighted poses a numpy array of weights for each pose
weights = np.empty(self.poses.shape[0], dtype=np.float)
# 0.6 seconds in this loop!
for index in range(self.poses.shape[0]):
# remove any that are outside the arena
if not arena.point_is_inside_arena(self.poses[index,0], self.poses[index,1]) or \
not arena.point_is_inside_arena(distance_sensor_left_rays[index,0], distance_sensor_left_rays[index,1]) or \
not arena.point_is_inside_arena(distance_sensor_right_rays[index,0], distance_sensor_right_rays[index,1]):
if not arena.point_is_inside_arena(self.poses[index,0], self.poses[index,1]):
weights[index] = 0
continue
# difference between this distance and the distance sensed is the error
# add noise to this error
# left sensor
noise = get_triangular_sample(0, distance_sensor_standard_dev)
left_actual = arena.get_ray_distance_squared_to_nearest_boundary_segment(distance_sensor_left_rays[index])
left_error = abs(left_actual - dl_squared + noise)
# right sensor
noise = get_triangular_sample(0, distance_sensor_standard_dev)
right_actual = arena.get_ray_distance_squared_to_nearest_boundary_segment(distance_sensor_right_rays[index])
right_error = abs(right_actual - dr_squared + noise)
# weight is the inverse of the error
weights[index] = 1 / (left_error + right_error)
print("Time to calculate pose weights", time.monotonic() - fn_start)
weights[index] = arena.get_distance_grid_at_point(distance_sensor_left[index,0], distance_sensor_left[index,1])
weights[index] += arena.get_distance_grid_at_point(distance_sensor_left[index,0], distance_sensor_left[index,1])
await asyncio.sleep(0)
#normalise the weights
# print("Weights sum before normalising:", np.sum(weights))
weights = weights / np.sum(weights)
# print("Weights sum:", np.sum(weights))
return weights
def resample(self, weights):
# Fast - 0.01 to 0.035 seconds
# based on the weights, resample the poses
# weights is a numpy array of weights
# resample is a numpy array of indices into the poses array
# fn_start = time.monotonic()
samples = []
# use low variance resampling
start = random.uniform(0, 1 / self.population_size)
@@ -145,7 +111,6 @@ class Simulation:
samples.append(source_index)
# set poses to the resampled poses
self.poses = np.array([self.poses[n] for n in samples])
# print("resample time", time.monotonic() - fn_start)
def convert_odometry_to_motion(self, left_encoder_delta, right_encoder_delta):
# convert odometry to motion
@@ -175,16 +140,11 @@ class Simulation:
async def motion_model(self):
"""move forward, apply the motion model"""
# fn_start = time.monotonic()
# Reading sensors - 0.001 to 0.002 seconds.
starting_heading = robot.imu.euler[0]
encoder_left = robot.left_encoder.read()
encoder_right = robot.right_encoder.read()
# print("Reading sensors time", time.monotonic() - fn_start)
await asyncio.sleep(0.01)
# fn_start = time.monotonic()
# record sensor changes - 0.001 to 0.002 seconds
rot1, trans, rot2 = self.convert_odometry_to_motion(
robot.left_encoder.read() - encoder_left,
robot.right_encoder.read() - encoder_right)
@@ -199,10 +159,7 @@ class Simulation:
rot1 = rot1 * self.encoder_mix + heading_change * self.imu_mix
rot2 = rot2 * self.encoder_mix + heading_change * self.imu_mix
else:
print("Failed to get heading")
# print("Got headings time", time.monotonic() - fn_start)
# fn_start = time.monotonic()
# move poses 0.07 - 0.08 seconds
print("Failed to get heading")
rot1_model = np.array([get_triangular_sample(rot1, self.rotation_standard_dev) for _ in range(self.poses.shape[0])])
trans_model = np.array([get_triangular_sample(trans, self.speed_standard_dev) for _ in range(self.poses.shape[0])])
rot2_model = np.array([get_triangular_sample(rot2, self.rotation_standard_dev) for _ in range(self.poses.shape[0])])
@@ -213,7 +170,6 @@ class Simulation:
self.poses[:,2] += rot2_model
self.poses[:,2] = np.vectorize(lambda n: float(n % 360))(self.poses[:,2])
self.poses = np.array(self.poses, dtype=np.int16)
# print("Move poses times", time.monotonic() - fn_start)
async def distance_sensor_updater(self):
robot.left_distance.distance_mode = 2
@@ -223,32 +179,22 @@ class Simulation:
robot.left_distance.start_ranging()
robot.right_distance.start_ranging()
while True:
# About 0.02 seconds
# loop_start = time.monotonic()
if robot.left_distance.data_ready and robot.left_distance.distance:
self.left_distance = robot.left_distance.distance * 10 # convert to mm
robot.left_distance.clear_interrupt()
if robot.right_distance.data_ready and robot.right_distance.distance:
self.right_distance = robot.right_distance.distance * 10
robot.right_distance.clear_interrupt()
print("left_distance:", self.left_distance, "right_distance:", self.right_distance)
# move forward - with collision avoidance 0.03 to 0.04 seconds
self.collision_avoider.update(self.left_distance, self.right_distance)
# print("distance_sensor_updater_used_time: ", time.monotonic() - loop_start)
await asyncio.sleep(0.01)
async def run(self):
asyncio.create_task(self.distance_sensor_updater())
try:
while True:
# print("Applying sensor model")
weights = await self.apply_sensor_model()
# print("Sensor model complete.\nResampling")
self.resample(weights)
# print("Resampling complete.\nMoving robot")
await self.motion_model()
# print("Robot move complete")
finally:
robot.stop()
@@ -277,8 +223,6 @@ def read_command():
async def updater(simulation):
print("starting updater")
while True:
loop_start = time.monotonic()
# Imu calibration and send - 0.0625 seconds
sys_status, gyro, accel, mag = robot.imu.calibration_status
if sys_status < 3:
send_json(
@@ -291,18 +235,11 @@ async def updater(simulation):
}
}
)
print("Sent imu calibration in", time.monotonic() - loop_start)
# The big time delay is in sending the poses.
print("Sending poses", simulation.poses.shape[0])
for n in range(0, simulation.poses.shape[0], 10):
loop_start = time.monotonic()
# each pose group is 0.2 seconds.
# print("Sending poses from ", n, "to", n+10, "of", simulation.poses.shape[0], "poses")
send_json({
"poses": simulation.poses[n:n+10].tolist(),
"offset": n,
})
print("Sent poses in", time.monotonic() - loop_start)
await asyncio.sleep(0.01)
await asyncio.sleep(0.5)
@@ -311,7 +248,6 @@ async def command_handler(simulation):
print("Starting handler")
update_task = None
simulation_task = None
# simulation_task = asyncio.create_task(simulation.run())
while True:
if robot.uart.in_waiting:
print("Receiving data...")
ch-13/full-version/robot/test_arena.py
-30
View File
@@ -1,30 +0,0 @@
from unittest import TestCase
import math
import arena
class TestArena(TestCase):
def test_get_ray_distance_to_segment_squared_is_not_none(self):
"""Use an example ray, test we get a distance squared (not none)"""
ray = (253.415, 85.2855, 0.479889)
segment = arena.boundary_lines[4]
distance_squared = arena.get_ray_distance_to_segment_squared(ray, segment)
self.assertIsNotNone(distance_squared)
def test_get_distance_squared_for_vertical_ray(self):
"""Make a vertical ray, say at y=1000, x=500, heading=pi/2, and test we get the correct distance squared"""
ray = (500, 1000, math.pi / 2)
segment = arena.boundary_lines[1]
distance_squared = arena.get_ray_distance_to_segment_squared(ray, segment)
self.assertEqual(distance_squared, 500 ** 2)
def test_get_distance_squared_for_vertical_with_nearest_segment(self):
"""Make a vertical ray, say at y=1000, x=500, heading=pi/2, and test we get the correct distance squared"""
ray = (500, 1000, math.pi / 2)
distance_squared = arena.get_ray_distance_squared_to_nearest_boundary_segment(ray)
self.assertEqual(distance_squared, 500 ** 2)
def test_get_distance_squared_for_horizontal_ray(self):
"""Make a horizontal ray, say at y=500, x=1000, heading=0, and test we get the correct distance squared"""
ray = (500, 250, 0)
distance_squared = arena.get_ray_distance_squared_to_nearest_boundary_segment(ray)
self.assertEqual(distance_squared, 500 ** 2)