Journeyman/episode status

sw/controllers.py

@@ -10,7 +10,6 @@
 from env import MoabEnv
 from common import Vector2
 
-
 class BrainNotFound(Exception):
     pass
 
@@ -25,7 +24,7 @@ def pid_controller(
 ):
     def next_action(state):
         env_state, ball_detected, buttons = state
-        x, y, vel_x, vel_y, sum_x, sum_y = env_state
+        x, y, vel_x, vel_y, sum_x, sum_y, bonsai_episode_status = env_state
 
         if ball_detected:
             action_x = Kp * x + Ki * sum_x + Kd * vel_x
@@ -41,6 +40,20 @@ def next_action(state):
 
         return action, {}
 
+    def random_action(state):
+        env_state, ball_detected, buttons = state
+        x, y, vel_x, vel_y, sum_x, sum_y, bonsai_episode_status = env_state
+
+        if ball_detected:
+            action = Vector2(np.random.uniform(-22, 22), np.random.uniform(-22, 22))
+
+        else:
+            # Move plate back to flat
+            action = Vector2(0, 0)
+
+        return action, {}
+
+
     return next_action
 
 
@@ -105,7 +118,7 @@ def brain_controller(
 
     def next_action_v1(state):
         env_state, ball_detected, buttons = state
-        x, y, vel_x, vel_y, sum_x, sum_y = env_state
+        x, y, vel_x, vel_y, sum_x, sum_y, bonsai_episode_status = env_state
 
         observables = {
             "ball_x": x,
@@ -147,48 +160,52 @@ def next_action_v1(state):
 
     def next_action_v2(state):
         env_state, ball_detected, buttons = state
-        x, y, vel_x, vel_y, sum_x, sum_y = env_state
+        x, y, vel_x, vel_y, sum_x, sum_y, bonsai_episode_status = env_state
+        if bonsai_episode_status == 0:
+            # Reset memory if a v2 brain
+            requests.delete(f"http://localhost:{port}/v2/clients/{client_id}")
 
         observables = {
             "state": {
                 "ball_x": x,
                 "ball_y": y,
                 "ball_vel_x": vel_x,
                 "ball_vel_y": vel_y,
+                "bonsai_episode_status": bonsai_episode_status,
             }
         }
 
         action = Vector2(0, 0)  # Action is 0,0 if not detected or brain didn't work
         info = {"status": 400, "resp": ""}
-        if ball_detected:
-
-            # Trap on GET failures so we can restart the brain without
-            # bringing down this run loop. Plate will default to level
-            # when it loses the connection.
-            try:
-                # Get action from brain
-                response = requests.post(prediction_url, json=observables)
-                info = {"status": response.status_code, "resp": response.json()}
 
-                if response.ok:
-                    concepts = info["resp"]["concepts"]
-                    concept_name = list(concepts.keys())[0]  # Just use first concept
-                    pitch = concepts[concept_name]["action"]["input_pitch"]
-                    roll = concepts[concept_name]["action"]["input_roll"]
+        # Trap on GET failures so we can restart the brain without
+        # bringing down this run loop. Plate will default to level
+        # when it loses the connection.
+        try:
+            # Get action from brain
+            response = requests.post(prediction_url, json=observables)
+            info = {"status": response.status_code, "resp": response.json()}
+
+            if response.ok and ball_detected:
+                concepts = info["resp"]["concepts"]
+                concept_name = list(concepts.keys())[0]  # Just use first concept
+                pitch = concepts[concept_name]["action"]["input_pitch"]
+                roll = concepts[concept_name]["action"]["input_roll"]
+
+                # Scale and clip
+                pitch = np.clip(pitch * max_angle, -max_angle, max_angle)
+                roll = np.clip(roll * max_angle, -max_angle, max_angle)
+
+                # To match how the old brain works (only integer plate angles)
+                pitch, roll = int(pitch), int(roll)
+                action = Vector2(-roll, pitch)
+
+        except requests.exceptions.ConnectionError as e:
+            print(f"No brain listening on port: {port}", file=sys.stderr)
+            raise BrainNotFound
+        except Exception as e:
+            print(f"Brain exception: {e}")
 
-                    # Scale and clip
-                    pitch = np.clip(pitch * max_angle, -max_angle, max_angle)
-                    roll = np.clip(roll * max_angle, -max_angle, max_angle)
-
-                    # To match how the old brain works (only integer plate angles)
-                    pitch, roll = int(pitch), int(roll)
-                    action = Vector2(-roll, pitch)
-
-            except requests.exceptions.ConnectionError as e:
-                print(f"No brain listening on port: {port}", file=sys.stderr)
-                raise BrainNotFound
-            except Exception as e:
-                print(f"Brain exception: {e}")
         return action, info
 
     if version == 1:

sw/env.py

@@ -16,6 +16,7 @@ class EnvState:
     vel_y: float = 0.0
     sum_x: float = 0.0
     sum_y: float = 0.0
+    bonsai_episode_status: int = 0
 
     def __iter__(self):
         return iter(astuple(self))
@@ -75,6 +76,7 @@ def reset(self, text=None, icon=None):
         self.vel_y = self.derivative_fn(self.frequency)
         # Reset the integral of the position
         self.sum_x, self.sum_y = 0, 0
+        self.bonsai_episode_status = 0
 
         # Return the state after a step with no motor actions
         return self.step((0, 0))
@@ -89,6 +91,6 @@ def step(self, action) -> Tuple[EnvState, bool, Buttons]:
         self.sum_x += x
         self.sum_y += y
 
-        state = EnvState(x, y, vel_x, vel_y, self.sum_x, self.sum_y)
+        state = EnvState(x, y, vel_x, vel_y, self.sum_x, self.sum_y, self.bonsai_episode_status)
 
         return state, ball_detected, buttons

sw/log_csv.py

@@ -3,6 +3,9 @@
 
 import time
 import logging as log
+import pandas as pd
+import numpy as np
+import pdb
 
 from common import Vector2
 
@@ -13,6 +16,8 @@ def log_decorator(fn, logfile="/tmp/log.csv"):
     cols += ["x", "y", "vel_x", "vel_y"]  # State
     cols += ["pitch", "roll"]  # Action
     cols += ["status", "error_response"]  # Error status
+    cols += ["bonsai_episode_status", "input_pitch", "input_roll"]
+
 
     header = ",".join(cols)
     with open(logfile, "w") as fd:
@@ -41,11 +46,17 @@ def decorated_fn(state):
 
         # Deconstuct the state to get the values we want
         env_state, ball_detected, buttons = state
-        x, y, vel_x, vel_y, sum_x, sum_y = env_state
+        x, y, vel_x, vel_y, sum_x, sum_y, bonsai_episode_status = env_state
         # Deconstruct action
         pitch, roll = action
+
+        # Brain actions for logging. Purposefully swapping to match sim
+        input_pitch = roll / 22
+        input_roll = -pitch / 22 
+
         # combine all to a list for the log
-        l = [tick, dt] + [x, y, vel_x, vel_y] + [pitch, roll] + [status, resp]
+        l = [tick, dt] + [x, y, vel_x, vel_y] + [pitch, roll] + [status, resp] + [bonsai_episode_status, input_pitch, input_roll]
+        row = [tick, dt] + [x, y, vel_x, vel_y] + [pitch, roll] + [status, resp] + [bonsai_episode_status, input_pitch, input_roll]
 
         # combine all to a list for the log
         # l = [tick, dt] + state + action + [status + resp]
@@ -55,9 +66,46 @@ def decorated_fn(state):
 
         # Convert all fields into strings
         l = ",".join([str(e) for e in l])
-
-        with open(logfile, "a") as fd:
-            print(l, file=fd)
+
+        # Append to file depending on episode terminal or not
+        if bonsai_episode_status == 2:
+            df = pd.read_csv(logfile)
+            last_x_abs = abs(float(df['x'].iloc[-1]))
+            last_y_abs = abs(float(df['y'].iloc[-1]))
+
+            # Check what the latest csv value was and clip
+            if float(df['x'].iloc[-1]) > 0.1125:
+                row[2] = df['x'].iloc[-1]
+            elif float(df['x'].iloc[-1]) <= 0.1125:
+                if row[2] == 0:
+                    if last_x_abs - last_y_abs > 0:
+                        row[2] = 0.1125 * np.sign(df['x'].iloc[-1])
+                    else:
+                        row[2] = float(df['x'].iloc[-1])
+                else:
+                    pass
+
+            if float(df['y'].iloc[-1]) > 0.1125:
+                row[3] = df['y'].iloc[-1]
+            elif float(df['y'].iloc[-1]) <= 0.1125:
+                if row[3] == 0:
+                    if last_y_abs - last_x_abs > 0:
+                        row[3] = 0.1125 * np.sign(df['y'].iloc[-1])
+                    else:
+                        row[3] = float(df['y'].iloc[-1])
+                else:
+                    pass
+            del df
+
+            row = [f"{n:8.5f}" if type(n) is float else n for n in row]
+            row = ",".join([str(e) for e in row])
+
+            # Append final iteration when ball is missing
+            with open(logfile, "a") as fd:
+                print(row, file=fd)
+        else:
+            with open(logfile, "a") as fd:
+                print(l, file=fd)
 
         return action, info
 

sw/menu.py

@@ -390,20 +390,59 @@ def main_menu(
 
                     # If it's a controller run the control loop
                     try:
+                        bonsai_episode_status = 0
                         while not buttons.menu_button:
+                            state.bonsai_episode_status = bonsai_episode_status
                             action, info = controller((state, detected, buttons))
                             state, detected, buttons = env.step(action)
 
+                            if detected:
+                                # Normal episode step
+                                bonsai_episode_status = 1
+                            elif detected == False and bonsai_episode_status == 1:
+                                # Ball not detected, let's terminate
+                                bonsai_episode_status = 2
+                                state.bonsai_episode_status = bonsai_episode_status
+
+                                action, info = controller((state, detected, buttons))
+                                state, detected, buttons = env.step(action)
+
+                                prev_state = (state, detected, buttons)
+                                next_state, kiosk_exit = kiosk_mode(
+                                    env, prev_state, kiosk_dump_location
+                                )
+                                state, detected, buttons = next_state
+
+                                # Reset
+                                bonsai_episode_status = 0
+                                state.bonsai_episode_status = bonsai_episode_status
+                                controller_start_time = time.time()
+
+                                # Whether the menu button was pressed in kiosk mode
+                                if kiosk_exit:
+                                    break
+
                             # If the controller has been running for more than
                             # kiosk_timeout seconds, exit, and dump the ball towards
                             # one side and wait until the ball is detected again
                             if kiosk:
                                 if time.time() - controller_start_time > kiosk_timeout:
+                                    bonsai_episode_status = 2
+                                    state.bonsai_episode_status = bonsai_episode_status
+
+                                    action, info = controller((state, detected, buttons))
+                                    state, detected, buttons = env.step(action)
+
                                     prev_state = (state, detected, buttons)
                                     next_state, kiosk_exit = kiosk_mode(
                                         env, prev_state, kiosk_dump_location
                                     )
                                     state, detected, buttons = next_state
+
+                                    # Reset
+                                    bonsai_episode_status = 0
+                                    state.bonsai_episode_status = bonsai_episode_status
+
                                     controller_start_time = time.time()
                                     # Whether the menu button was pressed in kiosk mode
                                     if kiosk_exit: