From f57add5106f6cb21c7618d732d29211b9187596d Mon Sep 17 00:00:00 2001 From: Danny Staple Date: Sun, 11 Dec 2022 20:44:50 +0000 Subject: [PATCH] Instrumented version - to look at timings Including: - replacing guassian with simpler triangle distribution - Timing calculations -resulting in using only 50 points. - Changes (which may need to be reconsidered) for optimization. These are likely to be simplified back. --- .../computer/display_from_robot.py | 7 +- ch-13/full-version/computer/poetry.lock | 64 ++++- ch-13/full-version/computer/pyproject.toml | 1 + ch-13/full-version/robot/arena.py | 12 +- ch-13/full-version/robot/code.py | 238 ++++++++++++------ ch-13/full-version/robot/guassian.py | 16 -- ch-13/full-version/robot/robot.py | 5 +- ch-13/layer-1/robot/code.py | 75 ++++++ ch-13/layer-1/robot/pid_controller.py | 27 ++ ch-13/layer-1/robot/pio_encoder.py | 84 +++++++ ch-13/layer-1/robot/robot.py | 78 ++++++ ch-13/types_of_randomness/msgpack_demo.py | 31 +++ 12 files changed, 534 insertions(+), 104 deletions(-) delete mode 100644 ch-13/full-version/robot/guassian.py create mode 100644 ch-13/layer-1/robot/code.py create mode 100644 ch-13/layer-1/robot/pid_controller.py create mode 100644 ch-13/layer-1/robot/pio_encoder.py create mode 100755 ch-13/layer-1/robot/robot.py create mode 100644 ch-13/types_of_randomness/msgpack_demo.py diff --git a/ch-13/full-version/computer/display_from_robot.py b/ch-13/full-version/computer/display_from_robot.py index 22f45a2..4533a8f 100644 --- a/ch-13/full-version/computer/display_from_robot.py +++ b/ch-13/full-version/computer/display_from_robot.py @@ -14,7 +14,7 @@ class RobotDisplay: self.arena = {} self.display_closed = False self.fig, self.ax = plt.subplots() - self.poses = np.zeros([200, 3], dtype=np.float32) + self.poses = np.zeros([200, 3], dtype=np.int16) def handle_close(self, _): self.display_closed = True @@ -35,7 +35,10 @@ class RobotDisplay: self.arena = message if "poses" in message: print(message) - self.poses[message["offset"]: message["offset"] + len(message["poses"])] = message["poses"] + incoming_poses = np.array(message["poses"], dtype=np.int16) + print("Incoming poses shape", incoming_poses.shape) + print("Existing poses shape", self.poses.shape) + self.poses[message["offset"]: message["offset"] + incoming_poses.shape[0]] = incoming_poses def draw(self): self.ax.clear() diff --git a/ch-13/full-version/computer/poetry.lock b/ch-13/full-version/computer/poetry.lock index 1a2c130..47cc3b7 100644 --- a/ch-13/full-version/computer/poetry.lock +++ b/ch-13/full-version/computer/poetry.lock @@ -158,6 +158,14 @@ pyparsing = ">=2.2.1" python-dateutil = ">=2.7" setuptools_scm = ">=7" +[[package]] +name = "msgpack" +version = "1.0.4" +description = "MessagePack serializer" +category = "main" +optional = false +python-versions = "*" + [[package]] name = "mypy-extensions" version = "0.4.3" @@ -339,7 +347,7 @@ python-versions = ">=3.7" [metadata] lock-version = "1.1" python-versions = "^3.9" -content-hash = "4ec6eb3464cbb49afad347e67bdf7c353d7ea4030fd8ed4257098a5df334eb99" +content-hash = "a2ae390fc260b23b2aea772611acac3c8626867abcb0e477875407c9f24a5fa6" [metadata.files] async-timeout = [ @@ -616,6 +624,60 @@ matplotlib = [ {file = "matplotlib-3.6.1-pp39-pypy39_pp73-win_amd64.whl", hash = "sha256:5f97141e05baf160c3ec125f06ceb2a44c9bb62f42fcb8ee1c05313c73e99432"}, {file = "matplotlib-3.6.1.tar.gz", hash = "sha256:e2d1b7225666f7e1bcc94c0bc9c587a82e3e8691da4757e357e5c2515222ee37"}, ] +msgpack = [ + {file = "msgpack-1.0.4-cp310-cp310-macosx_10_9_universal2.whl", hash = "sha256:4ab251d229d10498e9a2f3b1e68ef64cb393394ec477e3370c457f9430ce9250"}, + {file = "msgpack-1.0.4-cp310-cp310-macosx_10_9_x86_64.whl", hash = "sha256:112b0f93202d7c0fef0b7810d465fde23c746a2d482e1e2de2aafd2ce1492c88"}, + {file = "msgpack-1.0.4-cp310-cp310-macosx_11_0_arm64.whl", hash = "sha256:002b5c72b6cd9b4bafd790f364b8480e859b4712e91f43014fe01e4f957b8467"}, + {file = "msgpack-1.0.4-cp310-cp310-manylinux_2_17_aarch64.manylinux2014_aarch64.whl", hash = "sha256:35bc0faa494b0f1d851fd29129b2575b2e26d41d177caacd4206d81502d4c6a6"}, + {file = "msgpack-1.0.4-cp310-cp310-manylinux_2_17_x86_64.manylinux2014_x86_64.whl", hash = "sha256:4733359808c56d5d7756628736061c432ded018e7a1dff2d35a02439043321aa"}, + {file = "msgpack-1.0.4-cp310-cp310-manylinux_2_5_i686.manylinux1_i686.manylinux_2_17_i686.manylinux2014_i686.whl", hash = "sha256:eb514ad14edf07a1dbe63761fd30f89ae79b42625731e1ccf5e1f1092950eaa6"}, + {file = "msgpack-1.0.4-cp310-cp310-musllinux_1_1_aarch64.whl", hash = "sha256:c23080fdeec4716aede32b4e0ef7e213c7b1093eede9ee010949f2a418ced6ba"}, + {file = "msgpack-1.0.4-cp310-cp310-musllinux_1_1_i686.whl", hash = "sha256:49565b0e3d7896d9ea71d9095df15b7f75a035c49be733051c34762ca95bbf7e"}, + {file = "msgpack-1.0.4-cp310-cp310-musllinux_1_1_x86_64.whl", hash = "sha256:aca0f1644d6b5a73eb3e74d4d64d5d8c6c3d577e753a04c9e9c87d07692c58db"}, + {file = "msgpack-1.0.4-cp310-cp310-win32.whl", hash = "sha256:0dfe3947db5fb9ce52aaea6ca28112a170db9eae75adf9339a1aec434dc954ef"}, + {file = "msgpack-1.0.4-cp310-cp310-win_amd64.whl", hash = "sha256:4dea20515f660aa6b7e964433b1808d098dcfcabbebeaaad240d11f909298075"}, + {file = "msgpack-1.0.4-cp36-cp36m-macosx_10_9_x86_64.whl", hash = "sha256:e83f80a7fec1a62cf4e6c9a660e39c7f878f603737a0cdac8c13131d11d97f52"}, + {file = "msgpack-1.0.4-cp36-cp36m-manylinux_2_17_aarch64.manylinux2014_aarch64.whl", hash = "sha256:3c11a48cf5e59026ad7cb0dc29e29a01b5a66a3e333dc11c04f7e991fc5510a9"}, + {file = "msgpack-1.0.4-cp36-cp36m-manylinux_2_17_x86_64.manylinux2014_x86_64.whl", hash = "sha256:1276e8f34e139aeff1c77a3cefb295598b504ac5314d32c8c3d54d24fadb94c9"}, + {file = "msgpack-1.0.4-cp36-cp36m-manylinux_2_5_i686.manylinux1_i686.manylinux_2_17_i686.manylinux2014_i686.whl", hash = "sha256:6c9566f2c39ccced0a38d37c26cc3570983b97833c365a6044edef3574a00c08"}, + {file = "msgpack-1.0.4-cp36-cp36m-musllinux_1_1_aarch64.whl", hash = "sha256:fcb8a47f43acc113e24e910399376f7277cf8508b27e5b88499f053de6b115a8"}, + {file = "msgpack-1.0.4-cp36-cp36m-musllinux_1_1_i686.whl", hash = "sha256:76ee788122de3a68a02ed6f3a16bbcd97bc7c2e39bd4d94be2f1821e7c4a64e6"}, + {file = "msgpack-1.0.4-cp36-cp36m-musllinux_1_1_x86_64.whl", hash = "sha256:0a68d3ac0104e2d3510de90a1091720157c319ceeb90d74f7b5295a6bee51bae"}, + {file = "msgpack-1.0.4-cp36-cp36m-win32.whl", hash = "sha256:85f279d88d8e833ec015650fd15ae5eddce0791e1e8a59165318f371158efec6"}, + {file = "msgpack-1.0.4-cp36-cp36m-win_amd64.whl", hash = "sha256:c1683841cd4fa45ac427c18854c3ec3cd9b681694caf5bff04edb9387602d661"}, + {file = "msgpack-1.0.4-cp37-cp37m-macosx_10_9_x86_64.whl", hash = "sha256:a75dfb03f8b06f4ab093dafe3ddcc2d633259e6c3f74bb1b01996f5d8aa5868c"}, + {file = "msgpack-1.0.4-cp37-cp37m-manylinux_2_17_aarch64.manylinux2014_aarch64.whl", hash = "sha256:9667bdfdf523c40d2511f0e98a6c9d3603be6b371ae9a238b7ef2dc4e7a427b0"}, + {file = "msgpack-1.0.4-cp37-cp37m-manylinux_2_17_x86_64.manylinux2014_x86_64.whl", hash = "sha256:11184bc7e56fd74c00ead4f9cc9a3091d62ecb96e97653add7a879a14b003227"}, + {file = "msgpack-1.0.4-cp37-cp37m-manylinux_2_5_i686.manylinux1_i686.manylinux_2_17_i686.manylinux2014_i686.whl", hash = "sha256:ac5bd7901487c4a1dd51a8c58f2632b15d838d07ceedaa5e4c080f7190925bff"}, + {file = "msgpack-1.0.4-cp37-cp37m-musllinux_1_1_aarch64.whl", hash = "sha256:1e91d641d2bfe91ba4c52039adc5bccf27c335356055825c7f88742c8bb900dd"}, + {file = "msgpack-1.0.4-cp37-cp37m-musllinux_1_1_i686.whl", hash = "sha256:2a2df1b55a78eb5f5b7d2a4bb221cd8363913830145fad05374a80bf0877cb1e"}, + {file = "msgpack-1.0.4-cp37-cp37m-musllinux_1_1_x86_64.whl", hash = "sha256:545e3cf0cf74f3e48b470f68ed19551ae6f9722814ea969305794645da091236"}, + {file = "msgpack-1.0.4-cp37-cp37m-win32.whl", hash = "sha256:2cc5ca2712ac0003bcb625c96368fd08a0f86bbc1a5578802512d87bc592fe44"}, + {file = "msgpack-1.0.4-cp37-cp37m-win_amd64.whl", hash = "sha256:eba96145051ccec0ec86611fe9cf693ce55f2a3ce89c06ed307de0e085730ec1"}, + {file = "msgpack-1.0.4-cp38-cp38-macosx_10_9_universal2.whl", hash = "sha256:7760f85956c415578c17edb39eed99f9181a48375b0d4a94076d84148cf67b2d"}, + {file = "msgpack-1.0.4-cp38-cp38-macosx_10_9_x86_64.whl", hash = "sha256:449e57cc1ff18d3b444eb554e44613cffcccb32805d16726a5494038c3b93dab"}, + {file = "msgpack-1.0.4-cp38-cp38-macosx_11_0_arm64.whl", hash = "sha256:d603de2b8d2ea3f3bcb2efe286849aa7a81531abc52d8454da12f46235092bcb"}, + {file = "msgpack-1.0.4-cp38-cp38-manylinux_2_17_aarch64.manylinux2014_aarch64.whl", hash = "sha256:48f5d88c99f64c456413d74a975bd605a9b0526293218a3b77220a2c15458ba9"}, + {file = "msgpack-1.0.4-cp38-cp38-manylinux_2_17_x86_64.manylinux2014_x86_64.whl", hash = "sha256:6916c78f33602ecf0509cc40379271ba0f9ab572b066bd4bdafd7434dee4bc6e"}, + {file = "msgpack-1.0.4-cp38-cp38-manylinux_2_5_i686.manylinux1_i686.manylinux_2_17_i686.manylinux2014_i686.whl", hash = "sha256:81fc7ba725464651190b196f3cd848e8553d4d510114a954681fd0b9c479d7e1"}, + {file = "msgpack-1.0.4-cp38-cp38-musllinux_1_1_aarch64.whl", hash = "sha256:d5b5b962221fa2c5d3a7f8133f9abffc114fe218eb4365e40f17732ade576c8e"}, + {file = "msgpack-1.0.4-cp38-cp38-musllinux_1_1_i686.whl", hash = "sha256:77ccd2af37f3db0ea59fb280fa2165bf1b096510ba9fe0cc2bf8fa92a22fdb43"}, + {file = "msgpack-1.0.4-cp38-cp38-musllinux_1_1_x86_64.whl", hash = "sha256:b17be2478b622939e39b816e0aa8242611cc8d3583d1cd8ec31b249f04623243"}, + {file = "msgpack-1.0.4-cp38-cp38-win32.whl", hash = "sha256:2bb8cdf50dd623392fa75525cce44a65a12a00c98e1e37bf0fb08ddce2ff60d2"}, + {file = "msgpack-1.0.4-cp38-cp38-win_amd64.whl", hash = "sha256:26b8feaca40a90cbe031b03d82b2898bf560027160d3eae1423f4a67654ec5d6"}, + {file = "msgpack-1.0.4-cp39-cp39-macosx_10_9_universal2.whl", hash = "sha256:462497af5fd4e0edbb1559c352ad84f6c577ffbbb708566a0abaaa84acd9f3ae"}, + {file = "msgpack-1.0.4-cp39-cp39-macosx_10_9_x86_64.whl", hash = "sha256:2999623886c5c02deefe156e8f869c3b0aaeba14bfc50aa2486a0415178fce55"}, + {file = "msgpack-1.0.4-cp39-cp39-macosx_11_0_arm64.whl", hash = "sha256:f0029245c51fd9473dc1aede1160b0a29f4a912e6b1dd353fa6d317085b219da"}, + {file = "msgpack-1.0.4-cp39-cp39-manylinux_2_17_aarch64.manylinux2014_aarch64.whl", hash = "sha256:ed6f7b854a823ea44cf94919ba3f727e230da29feb4a99711433f25800cf747f"}, + {file = "msgpack-1.0.4-cp39-cp39-manylinux_2_17_x86_64.manylinux2014_x86_64.whl", hash = "sha256:0df96d6eaf45ceca04b3f3b4b111b86b33785683d682c655063ef8057d61fd92"}, + {file = "msgpack-1.0.4-cp39-cp39-manylinux_2_5_i686.manylinux1_i686.manylinux_2_17_i686.manylinux2014_i686.whl", hash = "sha256:6a4192b1ab40f8dca3f2877b70e63799d95c62c068c84dc028b40a6cb03ccd0f"}, + {file = "msgpack-1.0.4-cp39-cp39-musllinux_1_1_aarch64.whl", hash = "sha256:0e3590f9fb9f7fbc36df366267870e77269c03172d086fa76bb4eba8b2b46624"}, + {file = "msgpack-1.0.4-cp39-cp39-musllinux_1_1_i686.whl", hash = "sha256:1576bd97527a93c44fa856770197dec00d223b0b9f36ef03f65bac60197cedf8"}, + {file = "msgpack-1.0.4-cp39-cp39-musllinux_1_1_x86_64.whl", hash = "sha256:63e29d6e8c9ca22b21846234913c3466b7e4ee6e422f205a2988083de3b08cae"}, + {file = "msgpack-1.0.4-cp39-cp39-win32.whl", hash = "sha256:fb62ea4b62bfcb0b380d5680f9a4b3f9a2d166d9394e9bbd9666c0ee09a3645c"}, + {file = "msgpack-1.0.4-cp39-cp39-win_amd64.whl", hash = "sha256:4d5834a2a48965a349da1c5a79760d94a1a0172fbb5ab6b5b33cbf8447e109ce"}, + {file = "msgpack-1.0.4.tar.gz", hash = "sha256:f5d869c18f030202eb412f08b28d2afeea553d6613aee89e200d7aca7ef01f5f"}, +] mypy-extensions = [ {file = "mypy_extensions-0.4.3-py2.py3-none-any.whl", hash = "sha256:090fedd75945a69ae91ce1303b5824f428daf5a028d2f6ab8a299250a846f15d"}, {file = "mypy_extensions-0.4.3.tar.gz", hash = "sha256:2d82818f5bb3e369420cb3c4060a7970edba416647068eb4c5343488a6c604a8"}, diff --git a/ch-13/full-version/computer/pyproject.toml b/ch-13/full-version/computer/pyproject.toml index e5ac640..1571d56 100644 --- a/ch-13/full-version/computer/pyproject.toml +++ b/ch-13/full-version/computer/pyproject.toml @@ -11,6 +11,7 @@ python = "^3.9" matplotlib = "3.6.1" numpy = "1.23.4" bleak = "0.19.0" +msgpack = "^1.0.4" [tool.poetry.group.dev.dependencies] diff --git a/ch-13/full-version/robot/arena.py b/ch-13/full-version/robot/arena.py index 6b3b996..2f8e4f5 100644 --- a/ch-13/full-version/robot/arena.py +++ b/ch-13/full-version/robot/arena.py @@ -30,14 +30,14 @@ def point_is_inside_arena(x, y): 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, segment): +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) Segment -> ((x1, y1), (x2, y2)) """ - ray_x, ray_y, ray_heading = ray 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 @@ -46,7 +46,7 @@ def get_ray_distance_to_segment_squared(ray, segment): if ray_heading == 0: return None # calculate the x value of the intersection point - intersection_x = ray_x + (segment_y1 - ray_y) / math.tan(ray_heading) + 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 @@ -58,7 +58,7 @@ def get_ray_distance_to_segment_squared(ray, 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) * math.tan(ray_heading) + 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 @@ -75,8 +75,10 @@ def get_ray_distance_squared_to_nearest_boundary_segment(ray): """ # 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, segment) + 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 diff --git a/ch-13/full-version/robot/code.py b/ch-13/full-version/robot/code.py index b0dc0f3..fdffaca 100644 --- a/ch-13/full-version/robot/code.py +++ b/ch-13/full-version/robot/code.py @@ -2,9 +2,10 @@ import asyncio import json import random from ulab import numpy as np -from guassian import get_gaussian_sample import arena import robot +import math +import time # initial sample set - uniform # then apply sensor model @@ -12,19 +13,55 @@ import robot # 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 + motor_speed = limited_error / 300 + if motor_speed < 0.2: + motor_speed = -0.3 + 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 +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 = 200 + self.population_size = 50 self.left_distance = 100 self.right_distance = 100 + self.imu_mix = 0.3 * 0.5 + self.encoder_mix = 0.7 + self.rotation_standard_dev = 2 # degrees + self.speed_standard_dev = 5 # mm + # Poses - each an array of [x, y, heading] self.poses = np.array( - [(random.uniform(0, arena.width), random.uniform(0, arena.height), random.uniform(0, 360)) for _ in range(self.population_size)], - dtype=np.float, + [( + 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, ) - self.distance_aim = 100 + self.collision_avoider = VaryingWallAvoid() - def apply_sensor_model(self): + 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 @@ -32,63 +69,69 @@ class Simulation: # then check this projected position against occupancy grid # and weight accordingly - # distance sensor positions projected forward. left_x, left_y, right_x, right_y - distance_sensor_positions = np.zeros( - (self.poses.shape[0], 4), dtype=np.float) + # 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) # sensors - they are facing forward, either side of the robot. Project them out to the sides - # based on each poses heading + # 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_positions[:, 0] = self.poses[:, 0] + np.cos(poses_left_90) * robot.distance_sensor_from_middle - distance_sensor_positions[:, 1] = self.poses[:, 1] + np.sin(poses_left_90) * robot.distance_sensor_from_middle + 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]) # right sensor poses_right_90 = np.radians(self.poses[:, 2] - 90) - distance_sensor_positions[:, 2] = self.poses[:, 0] + np.cos(poses_right_90) * robot.distance_sensor_from_middle - distance_sensor_positions[:, 3] = self.poses[:, 1] + np.sin(poses_right_90) * robot.distance_sensor_from_middle + 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 - + 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) - - for index, sensor_position in enumerate(distance_sensor_positions): - # difference between this distance and the distance sensed is the error - # add noise to this error - if not arena.point_is_inside_arena(self.poses[index,0], self.poses[index,1]): + # 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]): weights[index] = 0 continue + # difference between this distance and the distance sensed is the error + # add noise to this error # left sensor - left_ray = sensor_position[0], sensor_position[1], np.radians(self.poses[index, 2]) - noise = get_gaussian_sample(0, distance_sensor_standard_dev) - left_actual = arena.get_ray_distance_squared_to_nearest_boundary_segment(left_ray) - if left_actual is None: - print("left_actual is None. Ray was ", left_ray) - left_actual = 100 - left_error = abs(left_actual - dl_squared + noise) # error + 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 - right_ray = sensor_position[2], sensor_position[3], np.radians(self.poses[index, 2]) - noise = get_gaussian_sample(0, distance_sensor_standard_dev) - right_actual = arena.get_ray_distance_squared_to_nearest_boundary_segment(right_ray) - if right_actual is None: - print("right_actual is None. Ray was ", right_ray) - right_actual = 100 - right_error = abs(right_actual - dr_squared + noise) #error + 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) + await asyncio.sleep(0) #normalise the weights - print("Weights sum before normalising:", np.sum(weights)) + # print("Weights sum before normalising:", np.sum(weights)) weights = weights / np.sum(weights) - print("Weights sum:", 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) @@ -102,66 +145,97 @@ 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) - async def move_robot(self): + def convert_odometry_to_motion(self, left_encoder_delta, right_encoder_delta): + # convert odometry to motion + # left_encoder is the change in the left encoder + # right_encoder is the change in the right encoder + # returns rot1, trans, rot2 + # rot1 is the rotation of the robot in radians before the translation + # trans is the distance the robot has moved in mm + # rot2 is the rotation of the robot in radians + + # convert encoder counts to mm + left_mm = left_encoder_delta * robot.ticks_to_mm + right_mm = right_encoder_delta * robot.ticks_to_mm + + if left_mm == right_mm: + # no rotation + return 0, left_mm, 0 + + # calculate the ICC + radius = (robot.wheelbase_mm / 2) * (left_mm + right_mm) / (right_mm - left_mm) + theta = (right_mm - left_mm) / robot.wheelbase_mm + # For a small enough motion, assume that the chord length = arc length + arc_length = theta * radius + rot1 = np.degrees(theta/2) + rot2 = rot1 + return rot1, arc_length, rot2 + + 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) - # move forward - with collision avoidance - print("left_distance:", self.left_distance, "right_distance:", self.right_distance) - if min(self.left_distance, self.right_distance) < self.distance_aim: - # we are too close to the wall - # turn away from the wall - # turn right if left distance is smaller - # turn left if right distance is smaller - forward_speed = 0 - if self.left_distance < self.right_distance: - # turn right - turn_speed = -0.5 - else: - turn_speed = 0.5 - else: - forward_speed = 0.8 - print("forward_speed:", forward_speed, "turn_speed:", turn_speed) - robot.set_left(forward_speed + turn_speed) - robot.set_right(forward_speed - turn_speed) + 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) - await asyncio.sleep(0.05) - # record sensor changes - left_movement = robot.left_encoder.read() - encoder_left - right_movement = robot.right_encoder.read() - encoder_right - speed_in_mm = robot.ticks_to_m * ((left_movement + right_movement) / 2) * 1000 - new_heading = robot.imu.euler[0] + try: + new_heading = robot.imu.euler[0] + except OSError: + new_heading = None if new_heading: - heading_change = starting_heading - new_heading + heading_change = starting_heading - new_heading + # blend with the encoder heading changes + 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") - heading_change = 0 - - # move poses (this is a bit cheeky, and should be using icc) - heading_standard_dev = 2 # degrees - speed_standard_dev = 5 # mm - - radians = np.radians(self.poses[:,2]) - heading_model = np.array([get_gaussian_sample(0, heading_standard_dev) for _ in range(self.poses.shape[0])]) - speed_model = np.array([get_gaussian_sample(speed_in_mm, speed_standard_dev) for _ in range(self.poses.shape[0])]) - self.poses[:,0] += speed_model * np.sin(radians) - self.poses[:,1] += speed_model * np.cos(radians) - self.poses[:,2] += heading_change + heading_model + # print("Got headings time", time.monotonic() - fn_start) + # fn_start = time.monotonic() + # move poses 0.07 - 0.08 seconds + 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])]) + self.poses[:,2] += rot1_model + rot1_radians = np.radians(self.poses[:,2]) + self.poses[:,0] += trans_model * np.sin(rot1_radians) + self.poses[:,1] += trans_model * np.cos(rot1_radians) + 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 + robot.right_distance.distance_mode = 2 + robot.left_distance.timing_budget = 50 + robot.right_distance.timing_budget = 50 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): @@ -169,11 +243,11 @@ class Simulation: try: while True: # print("Applying sensor model") - weights = self.apply_sensor_model() + weights = await self.apply_sensor_model() # print("Sensor model complete.\nResampling") self.resample(weights) # print("Resampling complete.\nMoving robot") - await self.move_robot() + await self.motion_model() # print("Robot move complete") finally: robot.stop() @@ -203,6 +277,8 @@ 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( @@ -215,14 +291,18 @@ 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) @@ -231,6 +311,7 @@ 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...") @@ -247,7 +328,8 @@ async def command_handler(simulation): if not update_task: update_task = asyncio.create_task(updater(simulation)) elif request["command"] == "start": - simulation_task = asyncio.create_task(simulation.run()) + if not simulation_task: + simulation_task = asyncio.create_task(simulation.run()) await asyncio.sleep(0.1) diff --git a/ch-13/full-version/robot/guassian.py b/ch-13/full-version/robot/guassian.py deleted file mode 100644 index 0e212e9..0000000 --- a/ch-13/full-version/robot/guassian.py +++ /dev/null @@ -1,16 +0,0 @@ -import random -import math - -def get_standard_normal_sample(): - """Using the Marsaglia Polar method""" - # timeit on mac says - 0.915 - while True: - u = random.uniform(-1, 1) - v = random.uniform(-1, 1) - s = u * u + v * v - if s >= 1: - continue - return u * math.sqrt(-2 * math.log(s) / s) - -def get_gaussian_sample(mean, standard_deviation): - return get_standard_normal_sample() * standard_deviation + mean diff --git a/ch-13/full-version/robot/robot.py b/ch-13/full-version/robot/robot.py index 5e5e085..497d0b5 100755 --- a/ch-13/full-version/robot/robot.py +++ b/ch-13/full-version/robot/robot.py @@ -14,9 +14,10 @@ wheel_circumference_mm = math.pi * wheel_diameter_mm gear_ratio = 298 encoder_poles = 28 ticks_per_revolution = encoder_poles * gear_ratio -ticks_to_m = (wheel_circumference_mm / ticks_per_revolution) / 1000 +ticks_to_mm = wheel_circumference_mm / ticks_per_revolution +ticks_to_m = ticks_to_mm / 1000 m_to_ticks = 1 / ticks_to_m - +wheelbase_mm = 170 motor_A2 = pwmio.PWMOut(board.GP17, frequency=100) motor_A1 = pwmio.PWMOut(board.GP16, frequency=100) diff --git a/ch-13/layer-1/robot/code.py b/ch-13/layer-1/robot/code.py new file mode 100644 index 0000000..047793d --- /dev/null +++ b/ch-13/layer-1/robot/code.py @@ -0,0 +1,75 @@ +import asyncio +import json +import time + +import pid_controller +import robot + + +class RpmSpeedController: + def __init__(self, encoder, motor_fn): + self.encoder = encoder + self.motor_fn = motor_fn + self.pid = pid_controller.PIDController(3, 0, 1) + self.speed = 0 + self.reset() + + def reset(self): + self.last_ticks = self.encoder.read() + self.pwm = 0 + self.pid.reset() + + def update(self, dt): + current_ticks = self.encoder.read() + speed_in_ticks = (current_ticks - self.last_ticks) / dt + self.last_ticks = current_ticks + # calculate the error + error = self.speed - speed_in_ticks + # calculate the control signal + control_signal = self.pid.calculate(error, dt) + self.pwm += control_signal + self.motor_fn(self.pwm) + + +class MotionControl: + def __init__(self): + self.left_speed_controller = RpmSpeedController(robot.left_encoder, robot.set_left) + self.right_speed_controller = RpmSpeedController(robot.right_encoder, robot.set_right) + + + def plan_motion(self, turn1, distance, turn2): + """Plan a motion - based on making a turn, + moving forward a distance, + and then making another turn.""" + + pass + + def uodate(self, dt): + self.left_speed_controller.update(dt) + self.right_speed_controller.update(dt) + + +async def main_loop(dt): + motion_control = MotionControl() + while True: + await asyncio.sleep(dt) + current_time = time.monotonic() + dt = current_time - last_time + last_time = current_time + motion_control.uodate(dt) + + +async def command_handler(): + main_loop = None + motion_control = MotionControl() + robot = robot.Robot() + while True: + command = json.loads(input()) + if command["command"] == "start": + main_loop = asyncio.create_task(main_loop(0.2)) + elif command["command"] == "get_speed": + print(json.dumps(robot.get_speed())) + else: + print("Unknown command") + +asyncio.run(command_handler()) diff --git a/ch-13/layer-1/robot/pid_controller.py b/ch-13/layer-1/robot/pid_controller.py new file mode 100644 index 0000000..a09358f --- /dev/null +++ b/ch-13/layer-1/robot/pid_controller.py @@ -0,0 +1,27 @@ +class PIDController: + def __init__(self, kp, ki, kd, d_filter_gain=0.1, imax=None, imin=None): + self.kp = kp + self.ki = ki + self.kd = kd + self.d_filter_gain = d_filter_gain + self.imax = imax + self.imin = imin + self.reset() + + def reset(self): + self.integral = 0 + self.error_prev = 0 + self.derivative = 0 + + def calculate(self, error, dt): + self.integral += error * dt + if self.imax is not None and self.integral > self.imax: + self.integral = self.imax + if self.imin is not None and self.integral < self.imin: + self.integral = self.imin + # Add a low pass filter to the difference + difference = (error - self.error_prev) * self.d_filter_gain + self.error_prev += difference + self.derivative = difference / dt + + return self.kp * error + self.ki * self.integral + self.kd * self.derivative diff --git a/ch-13/layer-1/robot/pio_encoder.py b/ch-13/layer-1/robot/pio_encoder.py new file mode 100644 index 0000000..b831bf4 --- /dev/null +++ b/ch-13/layer-1/robot/pio_encoder.py @@ -0,0 +1,84 @@ +import rp2pio +import adafruit_pioasm +import array +import asyncio + + +program = """ +; use the osr for count +; input pins c1 c2 + + set y, 0 ; clear y + mov osr, y ; and clear osr +read: + ; x will be the old value + ; y the new values + mov x, y ; store old Y in x + in null, 32 ; Clear ISR - using y + in pins, 2 ; read two pins into y + mov y, isr + jmp x!=y, different ; Jump if its different + jmp read ; otherwise loop back to read + +different: + ; x has old value, y has new. + ; extract the upper bit of X. + in x, 31 ; get bit 31 - old p1 (remember which direction it came in) + in null, 31 ; keep only 1 bit + mov x, isr ; put this back in x + jmp !x, c1_old_zero + +c1_old_not_zero: + jmp pin, count_up + jmp count_down + +c1_old_zero: + jmp pin, count_down + ; fall through +count_up: + ; for a clockwise move - we'll add 1 by inverting + mov x, ~ osr ; store inverted OSR on x + jmp x--, fake ; use jump to take off 1 +fake: + mov x, ~ x ; invert back + jmp send +count_down: + ; for a clockwise move, just take one off + mov x, osr ; store osr in x + jmp x--, send ; dec and send +send: + ; send x. + mov isr, x ; send it + push noblock ; put ISR into input FIFO + mov osr, x ; put X back in OSR + jmp read ; loop back +""" + +assembled = adafruit_pioasm.assemble(program) + + +class QuadratureEncoder: + def __init__(self, first_pin, second_pin, reversed=False): + """Encoder with 2 pins. Must use sequential pins on the board""" + self.sm = rp2pio.StateMachine( + assembled, + frequency=0, + first_in_pin=first_pin, + jmp_pin=second_pin, + in_pin_count=2, + ) + self.reversed = reversed + self._buffer = array.array("i", [0]) + asyncio.create_task(self.poll_loop()) + + async def poll_loop(self): + while True: + await asyncio.sleep(0) + while self.sm.in_waiting: + self.sm.readinto(self._buffer) + + def read(self): + if self.reversed: + return -self._buffer[0] + else: + return self._buffer[0] diff --git a/ch-13/layer-1/robot/robot.py b/ch-13/layer-1/robot/robot.py new file mode 100755 index 0000000..5e5e085 --- /dev/null +++ b/ch-13/layer-1/robot/robot.py @@ -0,0 +1,78 @@ +import board +import pwmio +import pio_encoder +import busio +import adafruit_vl53l1x +import math +import busio +import adafruit_bno055 + +uart = busio.UART(board.GP12, board.GP13, baudrate=9600) + +wheel_diameter_mm = 70 +wheel_circumference_mm = math.pi * wheel_diameter_mm +gear_ratio = 298 +encoder_poles = 28 +ticks_per_revolution = encoder_poles * gear_ratio +ticks_to_m = (wheel_circumference_mm / ticks_per_revolution) / 1000 +m_to_ticks = 1 / ticks_to_m + + +motor_A2 = pwmio.PWMOut(board.GP17, frequency=100) +motor_A1 = pwmio.PWMOut(board.GP16, frequency=100) +motor_B2 = pwmio.PWMOut(board.GP18, frequency=100) +motor_B1 = pwmio.PWMOut(board.GP19, frequency=100) + +right_motor = motor_A1, motor_A2 +left_motor = motor_B1, motor_B2 + +right_encoder = pio_encoder.QuadratureEncoder(board.GP20, board.GP21) +left_encoder = pio_encoder.QuadratureEncoder(board.GP26, board.GP27, reversed=True) + +i2c0 = busio.I2C(sda=board.GP0, scl=board.GP1) +i2c1 = busio.I2C(sda=board.GP2, scl=board.GP3) + +left_distance = adafruit_vl53l1x.VL53L1X(i2c0) +right_distance = adafruit_vl53l1x.VL53L1X(i2c1) + +distance_sensor_from_middle = 40 # approx mm + +imu = adafruit_bno055.BNO055_I2C(i2c0) +imu.mode = adafruit_bno055.NDOF_MODE # should be in chapter 12! + +def stop(): + motor_A1.duty_cycle = 0 + motor_A2.duty_cycle = 0 + motor_B1.duty_cycle = 0 + motor_B2.duty_cycle = 0 + + +def set_speed(motor, speed): + # Swap motor pins if we reverse the speed + if abs(speed) < 0.1: + motor[0].duty_cycle = 0 + motor[1].duty_cycle = 1 + return + if speed < 0: + direction = motor[1], motor[0] + speed = -speed + else: + direction = motor + speed = min(speed, 1) # limit to 1.0 + max_speed = 2 ** 16 - 1 + + direction[0].duty_cycle = int(max_speed * speed) + direction[1].duty_cycle = 0 + + +def set_left(speed): + set_speed(left_motor, speed) + + +def set_right(speed): + set_speed(right_motor, speed) + +def check_imu_status(): + sys_status, gyro, accel, mag = imu.calibration_status + uart.write(f"Sys: {sys_status}, Gyro: {gyro}, Accel: {accel}, Mag: {mag}\n".encode()) + return sys_status == 3 diff --git a/ch-13/types_of_randomness/msgpack_demo.py b/ch-13/types_of_randomness/msgpack_demo.py new file mode 100644 index 0000000..95bc14c --- /dev/null +++ b/ch-13/types_of_randomness/msgpack_demo.py @@ -0,0 +1,31 @@ +from ulab import numpy as np +import random +import msgpack +from io import BytesIO + +data = np.array([[random.uniform(-200, 200), random.uniform(-200, 200), random.uniform(0, 360)] for i in range(20)], dtype=np.int16) +print(data) +print(data.shape) +buffer = BytesIO() +msgpack.pack({"offset": 10, "poses": data.tolist()}, buffer) +buffer.seek(0) +print(len(buffer.getvalue())) # json is 618 bytes, msgpack is 595 bytes. msgpack is 3.7% smaller +print(buffer.getvalue()) + +import json +as_int = np.array(data, dtype=np.int16) +as_int_json = json.dumps({"offset": 10, "poses": as_int.tolist()}) + + +## on pc +import msgpack +import numpy as np +raw_data = "bytes from robot" +data = msgpack.unpackb(raw_data) +poses = np.array(data["poses"]) +print(poses) +# Avoiding the text could make it smaller. +# From https://learn.adafruit.com/introducing-the-adafruit-bluefruit-le-uart-friend/hardware +# Note that we do not recommend using higher baudrates than 9600 because the nRF51 UART can drop characters! +# Is the complexity of the msgpack worth the 3.7% size reduction? +