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

Further bugfix on the arena contains function

Browse Source
This commit is contained in:
Danny Staple
2023-01-30 22:59:49 +00:00
parent 985b8d844a
commit d507ebec6a
2 changed files with 28 additions and 26 deletions
Download Patch File Download Diff File Expand all files Collapse all files
ch-13/4.3-monte-carlo/robot/code.py
+18 -25
View File
@@ -144,31 +144,24 @@ class Simulation:
)
)
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):
# 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_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]):
@@ -180,7 +173,7 @@ class Simulation:
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]):
if not arena.contains(pose[0], pose[1]):
weights[index] = arena.low_probability
weights = self.observe_distance_sensors(weights)
return weights
ch-13/4.3-monte-carlo_perf/robot/code.py
+10 -1
View File
@@ -197,6 +197,7 @@ class Simulation:
send_json({"distance_observation":
{
"weight": weights[pick],
"index": pick,
"pose": self.poses[pick].tolist(),
"left_sensor": left_sensor[pick].tolist(),
"right_sensor": right_sensor[pick].tolist(),
@@ -211,8 +212,16 @@ 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[:0], pose[:1]):
if not arena.contains(pose[0], pose[1]):
weights[index] = arena.low_probability
pick = np.argmax(weights)
send_json({"arena_observation":
{
"weight": weights[pick],
"index": pick,
"pose": self.poses[pick].tolist(),
}
})
weights = self.observe_distance_sensors(weights)
self.pc_observation_model.stop()
return weights