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

More observations - more fixes.

Browse Source
This commit is contained in:
Danny Staple 2023-01-30 22:34:00 +00:00
parent aa2bc483f0
commit 985b8d844a
2 changed files with 25 additions and 32 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
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 = 2 # 2 each way
overscan = 10 # 2 each way
def get_distance_to_segment(x, y, segment):
@ -67,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/100) ** 2
return 1.0 / (1 + min_distance/250) ** 2
# beam endpoint model

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

@ -79,7 +79,7 @@ class Simulation:
int(random.uniform(0, arena.width)),
int(random.uniform(0, arena.height)),
int(random.uniform(0, 360))) for _ in range(self.population_size)],
dtype=np.int16,
dtype=np.float,
)
self.distance_sensors = DistanceSensorTracker()
self.collision_avoider = CollisionAvoid(self.distance_sensors)
@ -169,44 +169,37 @@ class Simulation:
# )
self.pc_motion_model.stop()
def get_sensor_endpoints(self, sensor_reading, right=False):
# Sensor triangle
adjacent = sensor_reading + robot.dist_forward_mm
angle = np.atan(robot.dist_side_mm / adjacent)
if right:
angle = - angle
hypotenuse = np.sqrt(robot.dist_side_mm**2 + adjacent**2)
# sensor endpoints
pose_angles = np.radians(self.poses[:,2]) + angle
sensor_endpoints = np.zeros((self.poses.shape[0], 2), dtype=np.float)
sensor_endpoints[:,0] = self.poses[:,0] + hypotenuse * np.cos(pose_angles)
sensor_endpoints[:,1] = self.poses[:,1] + hypotenuse * np.sin(pose_angles)
return sensor_endpoints
def observe_distance_sensors(self, weights):
self.pc_observe_distance_sensors.start()
# Sensor triangle left
adjacent = self.distance_sensors.left + robot.dist_forward_mm
opposite = robot.dist_side_mm
left_angle = np.atan(opposite / adjacent)
left_hypotenuse = np.sqrt(opposite**2 + adjacent**2)
# Sensor triangle right
adjacent = self.distance_sensors.right + robot.dist_forward_mm
opposite = 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_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
right_sensor[:, 1] = self.poses[:, 1] + np.sin(poses_right_angle) * right_hypotenuse
left_sensor = self.get_sensor_endpoints(self.distance_sensors.left)
right_sensor = self.get_sensor_endpoints(self.distance_sensors.right, True)
# Look up the distance in the arena
for index in range(self.poses.shape[0]):
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
pick = np.argmax(weights)
send_json({"distance_observation":
{
"weight": weights[0],
"pose": self.poses[0].tolist(),
"left_sensor": left_sensor[0].tolist(),
"right_sensor": right_sensor[0].tolist(),
"weight": weights[pick],
"pose": self.poses[pick].tolist(),
"left_sensor": left_sensor[pick].tolist(),
"right_sensor": right_sensor[pick].tolist(),
"left_distance": self.distance_sensors.left,
"right_distance": self.distance_sensors.right
}
@ -218,7 +211,7 @@ class Simulation:
self.pc_observation_model.start()
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]):
if not arena.contains(pose[:0], pose[:1]):
weights[index] = arena.low_probability
weights = self.observe_distance_sensors(weights)
self.pc_observation_model.stop()