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

Resampling index error appears to be gone. Still need more dialing in.

Browse Source
This commit is contained in:
Danny Staple 2023-01-29 22:16:12 +00:00
parent d1d13fc618
commit e557eb74a7
7 changed files with 79 additions and 49 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
ch-13/4.3-monte-carlo/robot/arena.py
View File

@ -9,6 +9,8 @@ height = 1500
cutout_width = 500
cutout_height = 500
low_probability = 10 ** -10
boundary_lines = [
[(0,0), (0, height)],
[(0, height), (width, height)],
@ -33,7 +35,7 @@ def contains(x, y):
grid_cell_size = 50
overscan = 10 # 10 each way
overscan = 2 # 2 each way
def get_distance_to_segment(x, y, segment):
@ -65,7 +67,7 @@ def get_distance_likelihood(x, y):
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
return 1.0 / (1 + min_distance/100) ** 2
# beam endpoint model
@ -88,10 +90,10 @@ def make_distance_grid():
distance_grid = make_distance_grid()
def get_distance_grid_at_point(x, y):
def get_distance_likelihood_at(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 low_probability
return distance_grid[grid_x, grid_y]

79
ch-13/4.3-monte-carlo/robot/code.py
View File

@ -77,28 +77,10 @@ class Simulation:
self.collision_avoider = CollisionAvoid(self.distance_sensors)
self.last_encoder_left = robot.left_encoder.read()
self.last_encoder_right = robot.right_encoder.read()
self.alpha_rot = 0.05
self.alpha_rot_trans = 0.01
self.alpha_trans = 0.05
self.alpha_trans_rot = 0.01
def resample(self, weights, sample_count):
"""Return sample_count number of samples from the
poses, based on the weights array.
Uses low variance resampling"""
samples = np.zeros((sample_count, 3))
interval = 1 / sample_count
shift = random.uniform(0, interval)
cumulative_weights = weights[0]
source_index = 0
for current_index in range(sample_count):
weight_index = shift + current_index * interval
while weight_index > cumulative_weights:
source_index += 1
cumulative_weights += weights[source_index]
samples[current_index] = self.poses[source_index]
return samples
self.alpha_rot = 0.09
self.alpha_rot_trans = 0.05
self.alpha_trans = 0.12
self.alpha_trans_rot = 0.05
def convert_odometry_to_motion(self, left_encoder_delta, right_encoder_delta):
"""
@ -163,39 +145,62 @@ class Simulation:
)
def observe_distance_sensors(self, weights):
# Sensor triangle left
opposite = self.distance_sensors.left + robot.dist_forward_mm
adjacent = robot.dist_side_mm
left_angle = np.atan(opposite / adjacent)
left_hypotenuse = np.sqrt(opposite**2 + adjacent**2)
# Sensor triangle right
opposite = self.distance_sensors.right + robot.dist_forward_mm
adjacent = robot.dist_side_mm
right_angle = np.atan(opposite / adjacent)
right_hypotenuse = np.sqrt(opposite**2 + adjacent**2)
# modify the current weights based on the distance sensors
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)
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)
poses_left_angle = np.radians(self.poses[:, 2]) + left_angle
left_sensor[:, 0] = self.poses[:, 0] + np.cos(poses_left_angle) * left_hypotenuse
left_sensor[:, 1] = self.poses[:, 1] + np.sin(poses_left_angle) * left_hypotenuse
# right sensor
poses_right_90 = np.radians(self.poses[:, 2] - 90)
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)
poses_right_angle = np.radians(self.poses[:, 2]) - right_angle
right_sensor[:, 0] = self.poses[:, 0] + np.cos(poses_right_angle) * right_hypotenuse
right_sensor[:, 1] = self.poses[:, 1] + np.sin(poses_right_angle) * right_hypotenuse
# Look up the distance in the arena
for index in range(self.poses.shape[0]):
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])
sensor_weight = arena.get_distance_likelihood_at(left_sensor[index,0], left_sensor[index,1])
sensor_weight += arena.get_distance_likelihood_at(right_sensor[index,0], right_sensor[index,1])
weights[index] *= sensor_weight
return weights
def observation_model(self):
weights = np.ones(self.poses.shape[0], dtype=np.float)
for index, pose in enumerate(self.poses):
if not arena.contains(pose[:1], pose[:2]):
weights[index] = 0.01
weights[index] = arena.low_probability
weights = self.observe_distance_sensors(weights)
weights = weights / np.sum(weights)
return weights
def resample(self, weights, sample_count):
"""Return sample_count number of samples from the
poses, based on the weights array.
Uses low variance resampling"""
samples = np.zeros((sample_count, 3))
interval = np.sum(weights) / sample_count
shift = random.uniform(0, interval)
cumulative_weights = weights[0]
source_index = 0
for current_index in range(sample_count):
weight_index = shift + current_index * interval
while weight_index > cumulative_weights:
source_index += 1
cumulative_weights += weights[source_index]
samples[current_index] = self.poses[source_index]
return samples
async def main(self):
asyncio.create_task(self.distance_sensors.main())
collision_avoider = asyncio.create_task(self.collision_avoider.main())

2
ch-13/4.3-monte-carlo/robot/robot.py
View File

@ -6,7 +6,7 @@ import adafruit_vl53l1x
import math
import adafruit_bno055
uart = busio.UART(board.GP12, board.GP13, baudrate=9600)
uart = busio.UART(board.GP12, board.GP13, baudrate=9600, timeout=0.1)
wheel_diameter_mm = 69.5
wheel_circumference_mm = math.pi * wheel_diameter_mm

23
ch-13/4.3-monte-carlo_perf/resample.py Normal file
View File

@ -0,0 +1,23 @@
import numpy as np
import random
def resample(weights, sample_count):
"""Return sample_count number of samples from the
poses, based on the weights array.
Uses low variance resampling"""
samples = np.zeros((sample_count, 3))
interval = np.sum(weights) / sample_count
shift = random.uniform(0, interval)
cumulative_weights = weights[0]
source_index = 0
for current_index in range(sample_count):
weight_index = shift + current_index * interval
while weight_index >= cumulative_weights:
source_index += 1
source_index = min(len(weights), source_index)
cumulative_weights += weights[source_index]
samples[current_index] = source_index
if samples.shape[0] != sample_count:
raise Exception("Sample count mismatch in resample.")
return samples

2
ch-13/4.3-monte-carlo_perf/robot/arena.py
View File

@ -35,7 +35,7 @@ def contains(x, y):
grid_cell_size = 50
overscan = 10 # 10 each way
overscan = 2 # 2 each way
def get_distance_to_segment(x, y, segment):

10
ch-13/4.3-monte-carlo_perf/robot/code.py
View File

@ -31,7 +31,7 @@ class DistanceSensorTracker:
class CollisionAvoid:
def __init__(self, distance_sensors):
self.speed = 0.6
self.speed = 0.5
self.distance_sensors = distance_sensors
async def main(self):
@ -185,12 +185,12 @@ class Simulation:
# modify the current weights based on the distance sensors
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_angle = np.radians(self.poses[:, 2]) + left_angle
left_sensor[:, 0] = self.poses[:, 0] + np.cos(poses_left_angle) * left_hypotenuse
left_sensor[:, 1] = self.poses[:, 1] + np.sin(poses_left_angle) * left_hypotenuse
# right sensor
poses_right_angle = np.radians(self.poses[:, 2]) - right_angle
right_sensor[:, 0] = self.poses[:, 0] + np.cos(poses_right_angle) * right_hypotenuse
@ -229,14 +229,14 @@ class Simulation:
weight_index = shift + current_index * interval
while weight_index >= cumulative_weights:
source_index += 1
source_index = min(len(weights), source_index)
source_index = min(len(weights) - 1, source_index)
cumulative_weights += weights[source_index]
samples[current_index] = self.poses[source_index]
except IndexError:
send_json({"error": "IndexError in resample.", "weights": [weights.tolist()]})
send_json({"error": "IndexError in resample.", "weights": weights.tolist()})
raise
if samples.shape[0] != sample_count:
send_json({"error": "Sample count mismatch in resample.", "samples": [samples.tolist()]})
send_json({"error": "Sample count mismatch in resample.", "samples": samples.tolist()})
raise Exception("Sample count mismatch in resample.")
self.pc_resample.stop()
return samples

2
ch-13/4.3-monte-carlo_perf/robot/robot.py
View File

@ -8,7 +8,7 @@ import adafruit_bno055
uart = busio.UART(board.GP12, board.GP13, baudrate=9600, timeout=0.1)
wheel_diameter_mm = 69.5
wheel_diameter_mm = 70
wheel_circumference_mm = math.pi * wheel_diameter_mm
gear_ratio = 298
encoder_poles = 28