fixes

transfer-learning · Nov 20, 2019 · ca41a00 · ca41a00
1 parent efde18d
commit ca41a00
Show file tree

Hide file tree

Showing 8 changed files with 49 additions and 108 deletions.
diff --git a/.cache/v/cache/lastfailed b/.cache/v/cache/lastfailed
@@ -0,0 +1,3 @@
+{
+  "sim.py": true
+}
diff --git a/SCOMPemu.py b/SCOMPemu.py
@@ -365,6 +365,7 @@ def get_sensor_data():
 
     if not sensor_q.empty():
         cur_sensors = sensor_q.get_nowait()
+        # print(f"cur_sensors: {cur_sensors.sonar}")
 
 
 def main():

diff --git a/__pycache__/de2bot.cpython-36.pyc b/__pycache__/de2bot.cpython-36.pyc
diff --git a/__pycache__/sensor_info.cpython-36.pyc b/__pycache__/sensor_info.cpython-36.pyc
diff --git a/__pycache__/sim.cpython-36.pyc b/__pycache__/sim.cpython-36.pyc
diff --git a/__pycache__/vis.cpython-36.pyc b/__pycache__/vis.cpython-36.pyc
diff --git a/de2bot.py b/de2bot.py
@@ -7,7 +7,7 @@ def __init__(self, pose=np.asmatrix([0.0, 0.0, 0.0]).T, twist=np.asmatrix([0.0,
         self.twist = twist.copy()
 
 class DE2Config:
-    def __init__(self, axle_length: float = 0.15):
+    def __init__(self, axle_length: float = 0.122):
         self.axle_length = axle_length
 
     def __str__(self):

diff --git a/sim.py b/sim.py
@@ -37,8 +37,14 @@ def sensor_model(x, obstacles, meas_angle):
     for obs in by_prox:
         prob = meas_prob(x, obs, meas_angle)
         if prob == 1:
-            return np.linalg.norm(obs - x[:2, 0])
-    return None
+            sensor_offset_dist = 0.1
+            heading = x[2, 0]
+            sensor_offset = sensor_offset_dist * np.asmatrix(
+                [[math.cos(meas_angle + heading), math.sin(meas_angle + heading)]]
+            ).T
+            sensor_pos = x[:2, 0] + sensor_offset
+            return np.linalg.norm(obs - sensor_pos), sensor_pos
+    return None, None
 
 
 def wait():
@@ -51,87 +57,12 @@ def wait():
                 return
 
 
-# def main():
-#     clock = pygame.time.Clock()
-#
-#     visualizer = vis.Visualizer()
-#
-#     fps = 60
-#     dt = 1.0 / fps
-#     t = 0.0
-#     i = 0
-#
-#     robot = DE2Bot()
-#
-#     obstacles = [
-#         np.asmatrix([[-1.5, -0.5]]).T,
-#         np.asmatrix([[1.5, -0.5]]).T,
-#     ]
-#
-#     angles_deg = [-144, -90, -44, -12, 12, 44, 90, 144]
-#     angles_rad = [np.deg2rad(a) for a in angles_deg]
-#
-#     controller = Controller(robot.state.pose, angles_rad, DE2Config(), obstacles)
-#
-#     next_sensor = None
-#
-#     framerate = 60.
-#
-#     # sensor_update_time = 1. / 16.
-#     sensor_update_time = 1. / 30.
-#     last_sensor_update = pygame.time.get_ticks()
-#
-#     hits = []
-#     while not visualizer.close:
-#         visualizer.update_events()
-#
-#         pos = robot.state.pose
-#
-#         sense = None
-#         if next_sensor:
-#             a = angles_rad[next_sensor]
-#             sense = sensor_model(pos, obstacles, a)
-#
-#             for b in angles_rad:
-#                 if b != a:
-#                     angle = b + pos[2, 0]
-#                     hits.append((pos[0, 0] + 0.1 * np.cos(angle), pos[1, 0] + 0.1 * np.sin(angle)))
-#
-#             if sense is not None:
-#                 angle = a + pos[2, 0]
-#                 hits.append((pos[0, 0] + sense * np.cos(angle), pos[1, 0] + sense * np.sin(angle)))
-#
-#         can_sense = pygame.time.get_ticks() > (last_sensor_update + sensor_update_time * 1000)
-#         if can_sense:
-#             hits = []
-#
-#         encoder_noise = 0.10 * np.asmatrix(np.random.normal(size=(2, 1)))
-#
-#         controls, next_sensor = controller.update(
-#             robot.left_right() + encoder_noise,
-#             sense,
-#             can_sense,
-#             1. / framerate
-#         )
-#         controls_noise = 0.5 * np.asmatrix(np.random.normal(size=(2, 1))) * np.linalg.norm(controls)
-#         robot.apply(controls, dt)
-#
-#         if can_sense and next_sensor:
-#             last_sensor_update = pygame.time.get_ticks()
-#
-#         visualizer.draw(
-#             robot.state.pose,
-#             hits,
-#             [(mat[0, 0], mat[1, 0]) for mat in obstacles])
-#
-#         # wait()
-#         clock.tick(framerate)
-
 OUT_OF_RANGE_READING = 5
 
 MAX_LINEAR = 1
 MAX_ANGULAR = 1
 
+
 def simulation_thread(controls_queue, sensor_queue):
     global kill_sim_thread
 
@@ -146,12 +77,13 @@ def simulation_thread(controls_queue, sensor_queue):
 
     obstacles = [
         np.asmatrix([[1.0, 0]]).T,
-        # np.asmatrix([[1.5, -0.5]]).T,
+        np.asmatrix([[1.5, -1.0]]).T,
     ]
 
     # angles_deg = [-144, -90, -44, -12, 12, 44, 90, 144]
     angles_deg = [90, 44, 12, -12, -44, -90, -144, 144]
-    # angles_deg = [0]
+    # robot.state.pose[2, 0] = -np.pi / 2
+    # angles_deg = [90]
     angles_rad = [np.deg2rad(a) for a in angles_deg]
 
     next_sensor = 0
@@ -169,48 +101,53 @@ def simulation_thread(controls_queue, sensor_queue):
 
         if not controls_queue.empty():
             new_controls = controls_queue.get()
-            controls = new_controls / 1500
+            new_controls = new_controls / 1500
+            d_controls = new_controls - controls
+            max_accel = 0.05
+            clipped = np.clip(d_controls, -max_accel, max_accel)
+            print(f"Before: {d_controls}, after: {clipped}")
+            controls = controls + clipped
+
 
         pos = robot.state.pose
 
-        a = angles_rad[next_sensor]
-        sense = sensor_model(pos, obstacles, a)
+        hits = []
+
+        for sensor in range(len(angles_rad)):
+            a = angles_rad[sensor]
+            sense, sensor_pos = sensor_model(pos, obstacles, a)
+
+            if sense is not None:
+                angle = a + pos[2, 0]
+                hits.append((sensor_pos[0, 0] + sense * np.cos(angle), sensor_pos[1, 0] + sense * np.sin(angle)))
+                # hits.append((robot.state.pose[0, 0] + sense * np.cos(angle), robot.state.pose[1, 0] + sense * np.sin(angle)))
+                actual_hits[sensor] = sense
+            else:
+                actual_hits[sensor] = OUT_OF_RANGE_READING
 
         # for b in angles_rad:
         #     if b != a:
         #         angle = b + pos[2, 0]
         #         hits.append((pos[0, 0] + 0.1 * np.cos(angle), pos[1, 0] + 0.1 * np.sin(angle)))
 
-        if sense is not None:
-            angle = a + pos[2, 0]
-            hits.append((pos[0, 0] + sense * np.cos(angle), pos[1, 0] + sense * np.sin(angle)))
-            actual_hits[next_sensor] = sense
-        else:
-            actual_hits[next_sensor] = OUT_OF_RANGE_READING
-
-        can_sense = pygame.time.get_ticks() > (last_sensor_update + sensor_update_time * 1000)
-        if can_sense:
-            # Send off to sensor queue
-            linear, angular = robot.state.twist[0, 0], robot.state.twist[1, 0]
-            theta = robot.state.pose[2, 0]
-            sensor_info = SensorInfo(actual_hits, theta, linear)
-            if sensor_queue.full():
-                sensor_queue.get_nowait()
-            sensor_queue.put_nowait(sensor_info)
-
-            next_sensor = (next_sensor + 1) % len(angles_rad)
-            hits = []
+        # Send off to sensor queue
+        linear, angular = robot.state.twist[0, 0], robot.state.twist[1, 0]
+        theta = robot.state.pose[2, 0]
+        sensor_info = SensorInfo(actual_hits, theta, linear)
+        if sensor_queue.full():
+            sensor_queue.get_nowait()
+        sensor_queue.put_nowait(sensor_info)
+
+        next_sensor = (next_sensor + 1) % len(angles_rad)
 
         encoder_noise = 0 * np.asmatrix(np.random.normal(size=(2, 1)))
         # controls = np.asarray([[0.1, -0.1]]).T
-        robot.apply(controls, dt)
-
-        if can_sense:
-            last_sensor_update = pygame.time.get_ticks()
+        controls_noise = 0.01 * np.asmatrix(np.random.normal(size=(2, 1)))
+        robot.apply(controls + controls_noise, dt)
 
         visualizer.draw(
             robot.state.pose,
-            hits[-1:],
+            hits,
             [(mat[0, 0], mat[1, 0]) for mat in obstacles])
 
         # wait()