Draft: Tobi Dance Branch

CartPole/__init__.py

@@ -10,9 +10,12 @@
 import csv
 # Import module to interact with OS
 import os
+import sys
 import traceback
 # Import module to get a current time and date used to name the files containing the history of simulations
 from datetime import datetime
+from typing import Optional
+
 # To detect the latest csv file
 
 import numpy as np
@@ -22,9 +25,9 @@
 from Control_Toolkit.others.globals_and_utils import (
     get_available_controller_names, get_available_optimizer_names, get_controller_name, get_optimizer_name, import_controller_by_name)
 from others.globals_and_utils import MockSpace, create_rng, load_config
-from others.p_globals import (P_GLOBALS, J_fric, L, m_cart, M_fric, TrackHalfLength,
-                              controlBias, controlDisturbance, export_globals,
-                              g, k, m_pole, u_max, v_max)
+from others.p_globals import (J_fric, L, m_cart, M_fric, TrackHalfLength,
+                              controlBias, controlDisturbance, g, k, m_pole, u_max, v_max, cart_bounce_factor, NaturalPeriod,
+                              export_globals)
 # Interpolate function to create smooth random track
 from scipy.interpolate import BPoly, interp1d
 # Run range() automatically adding progress bar in terminal
@@ -40,6 +43,9 @@
 from CartPole.state_utilities import (ANGLE_COS_IDX, ANGLE_IDX, ANGLE_SIN_IDX,
                                       ANGLED_IDX, POSITION_IDX, POSITIOND_IDX)
 
+from Control_Toolkit.others.get_logger import get_logger
+log = get_logger(__name__)
+
 # region Imported modules
 
 try:
@@ -143,7 +149,7 @@ def __init__(self, initial_state=s0, path_to_experiment_recordings=None):
         # region Variables controlling operation of the program - should not be modified directly
         self.save_flag = False  # Signalizes that the current time step should be saved
         self.csv_filepath = None  # Where to save the experiment history.
-        self.controller = None  # Placeholder for the currently used controller function
+        self.controller:template_controller = Optional[None]  # Placeholder for the currently used controller function
         self.controller_name = ''  # Placeholder for the currently used controller name
         self.optimizer_name = ''  # Placeholder for the currently used optimizer name
         self.controller_idx = None  # Placeholder for the currently used controller index
@@ -199,7 +205,7 @@ def __init__(self, initial_state=s0, path_to_experiment_recordings=None):
         self.slider_max = 1.0
         self.slider_value = 0.0
 
-        self.show_hanging_pole = False
+        self.show_hanging_pole = True
 
         self.physical_to_graphics = None
         self.graphics_to_physical = None
@@ -446,6 +452,7 @@ def cartpole_integration(self):
 
     def edge_bounce(self):
         # Elastic collision at edges
+        # TODO should be semielastic
         self.s[ANGLE_IDX], self.s[ANGLED_IDX], self.s[POSITION_IDX], self.s[POSITIOND_IDX] = edge_bounce_numba(
             self.s[ANGLE_IDX],
             np.cos(self.s[ANGLE_IDX]),
@@ -460,6 +467,10 @@ def edge_bounce(self):
     # This function should be called for the first time to calculate 0th time step
     # Otherwise it goes out of sync with saving
     def Update_Q(self):
+        """ Determine the dimensionless [-1,1] value of the motor power Q
+        This function should be called for the first time to calculate 0th time step
+        Otherwise it goes out of sync with saving,
+    """
         # Calculate time steps from last update
         # The counter should be initialized at max-1 to start with a control input update
         self.dt_controller_steps_counter += 1
@@ -471,7 +482,7 @@ def Update_Q(self):
                 # in this case slider corresponds already to the power of the motor
                 self.Q = self.slider_value
             else:  # in this case slider gives a target position, lqr regulator
-                self.Q = self.controller.step(self.s_with_noise_and_latency, self.time, {"target_position": self.target_position, "target_equilibrium": self.target_equilibrium})
+                self.Q = self.controller.step(self.s_with_noise_and_latency, self.time, updated_attributes= {"target_position": self.target_position, "target_equilibrium": self.target_equilibrium})
 
             self.dt_controller_steps_counter = 0
 
@@ -488,6 +499,11 @@ def update_parameters(self):
 
     # This method saves the dictionary keeping the history of simulation to a .csv file
     def save_history_csv(self, csv_name=None, mode='init', length_of_experiment='unknown'):
+        """ Saves history of cartpole state and control
+        :param csv_name: the filename base, .csv is appended if it is not there. self.path_to_experiment_recordings is prepended for path
+        :param mode: the mode for saving, default is 'init' which makes the timestamped folder etc TODO what are these modes???????
+        :param length_of_experiment: the duration of this experiment in seconds, written to header of CSV
+        """
 
         if mode == 'init':
 
@@ -550,8 +566,9 @@ def save_history_csv(self, csv_name=None, mode='init', length_of_experiment='unk
 
                 writer.writerow(['#'])
                 writer.writerow(['# Parameters:'])
-                for k in P_GLOBALS.__dict__:
-                    writer.writerow(['# ' + k + ': ' + str(getattr(P_GLOBALS, k))])
+                c = load_config("config.yml")
+                for k,v in c.items():
+                    writer.writerow(['# ' + k + ': ' + str(v)])
                 writer.writerow(['#'])
 
                 writer.writerow(['# Data:'])
@@ -900,8 +917,9 @@ def set_controller(self, controller_name=None, controller_idx=None):
                 )
 
             else:
+                log.debug(f'configuring controller "{self.controller}"')
                 self.controller.configure()
-            
+
 
         # Set the maximal allowed value of the slider - relevant only for GUI
         if self.controller_name == 'manual-stabilization':
@@ -932,8 +950,10 @@ def set_cartpole_state_at_t0(self, reset_mode=1, s=None, target_position=None, r
             pass
 
         # reset global variables
-        global k, m_cart, m_pole, g, J_fric, M_fric, L, v_max, u_max, controlDisturbance, controlBias, TrackHalfLength
-        k[...], m_cart[...], m_pole[...], g[...], J_fric[...], M_fric[...], L[...], v_max[...], u_max[...], controlDisturbance[...], controlBias[...], TrackHalfLength[...] = export_globals()
+        global k, m_cart, m_pole, g, J_fric, M_fric, L, v_max, u_max, controlDisturbance, controlBias, TrackHalfLength, cart_bounce_factor, NaturalPeriod
+        # TODO why is ellipis object used here? https://stackoverflow.com/questions/772124/what-does-the-ellipsis-object-do
+        # these outputs of export_globals are numpy scalar arrays, i.e. each constant is a np.array with a single element
+        k[...], m_cart[...], m_pole[...], g[...], J_fric[...], M_fric[...], L[...], v_max[...], u_max[...], controlDisturbance[...], controlBias[...], TrackHalfLength[...], cart_bounce_factor[...], NaturalPeriod[...] = export_globals()
 
         self.time = 0.0
         if reset_mode == 0:  # Don't change it
@@ -1297,3 +1317,17 @@ def animationManage(i):
         return anim
 
     # endregion
+
+
+def is_physical_cartpole_running_and_control_enabled():
+    """ super hack to determine if we are running physical cartpole and control is turned on"""
+    if 'DriverFunctions' in sys.modules:  # if this module exists in sys.modules, we can deduce that physical-cartpole is running
+        try:
+            physical_cartpole_instance = sys.modules[
+                'DriverFunctions'].PhysicalCartPoleDriver.PhysicalCartPoleDriver.PhysicalCartPoleDriverInstance
+            if getattr(physical_cartpole_instance, 'controlEnabled') == True:
+                log.debug(f'physical cartpole present and control enabled')
+                return True
+        except Exception as e:
+            log.warning(f'Could not determine if control is enabled: {e}')
+            return False

CartPole/cartpole_model.py

@@ -3,7 +3,7 @@
 import numpy as np
 from others.globals_and_utils import create_rng, load_config
 from others.p_globals import (J_fric, L, m_cart, M_fric, TrackHalfLength,
-                              controlBias, controlDisturbance, g, k, m_pole, u_max, v_max)
+                              controlBias, controlDisturbance, g, k, m_pole, u_max, v_max,cart_bounce_factor)
 
 from CartPole.state_utilities import (ANGLE_COS_IDX, ANGLE_IDX, ANGLE_SIN_IDX,
                                       ANGLED_IDX, POSITION_IDX, POSITIOND_IDX,
@@ -30,8 +30,8 @@
 Should be the same up to the angle-direction-convention and notation changes.
 
 The convention:
-Pole upright position defines 0 angle
-Cart movement to the right is positive
+Pole upright position defines 0 angle, units of angle is in radians
+Cart movement to the right is positive, units are meters
 Clockwise angle rotation is defined as negative
 
 Required angle convention for CartPole GUI: CLOCK-NEG
@@ -146,12 +146,24 @@ def cartpole_ode(s: np.ndarray, u: float,
 
     return angleDD, positionDD
 
-def edge_bounce(angle, angle_cos, angleD, position, positionD, t_step, L=L):
+def edge_bounce(angle, angle_cos, angleD, position, positionD, t_step, L=L, cart_bounce_factor=cart_bounce_factor):
+    """ Models bounce at edge of cart track. Very simple complete elastic bounce currently.
+
+    :param angle:
+    :param angle_cos:
+    :param angleD:
+    :param position:
+    :param positionD:
+    :param t_step: the timestep in seconds
+    :param L: the pole length
+
+    :returns: angle, angleD, position, positionD
+    """
     if position >= TrackHalfLength or -position >= TrackHalfLength:  # Without abs to compile with tensorflow
-        angleD -= 2 * (positionD * angle_cos) / L
-        angle += angleD * t_step
-        positionD = -positionD
-        position += positionD * t_step
+        angleD -= 2 * (positionD * angle_cos) / L # TODO why this formula???
+        # angle += angleD * t_step # update angle according to new derivative of angle
+        positionD = -cart_bounce_factor*positionD # perfect bounce
+        # position += positionD * t_step # step back the amount of bounce
     return angle, angleD, position, positionD
 
 

CartPole/cartpole_model_tf.py

@@ -3,7 +3,7 @@
 import numpy as np
 import tensorflow as tf
 from others.globals_and_utils import create_rng, load_config
-from others.p_globals import (J_fric, L, m_cart, M_fric, TrackHalfLength,
+from others.p_globals import (J_fric, L, m_cart, M_fric, TrackHalfLength, cart_bounce_factor,
                               controlBias, controlDisturbance, g, k, m_pole, u_max,
                               v_max)
 from SI_Toolkit.Functions.TF.Compile import CompileTF
@@ -26,6 +26,7 @@
 controlDisturbance = tf.convert_to_tensor(controlDisturbance)
 controlBias = tf.convert_to_tensor(controlBias)
 TrackHalfLength = tf.convert_to_tensor(TrackHalfLength)
+cart_bounce_factor = tf.convert_to_tensor(cart_bounce_factor)
 
 
 rng = create_rng(__name__, config["cartpole"]["seed"])
@@ -71,7 +72,10 @@ def _cartpole_ode(ca, sa, angleD, positionD, u,
     Calculates current values of second derivative of angle and position
     from current value of angle and position, and their first derivatives
 
-    :param angle, angleD, position, positionD: Essential state information of cart
+    :param angle, angleD, position, positionD:
+          Pole angle in radians. 0 means pole is upright. Clockwise angle rotation is defined as negative.
+          Cart position is in meters, 0 at middle of track, positive to rightwards.
+          Essential state information of cart
     :param u: Force applied on cart in unnormalized range
 
     :returns: angular acceleration, horizontal acceleration
@@ -135,19 +139,46 @@ def cartpole_ode(s: np.ndarray, u: float,
     )
     return angleDD, positionDD
 
-def edge_bounce(angle, angle_cos, angleD, position, positionD, t_step, L=L):
-    if position >= TrackHalfLength or -position >= TrackHalfLength:  # Without abs to compile with tensorflow
-        angleD -= 2 * (positionD * angle_cos) / L
-        angle += angleD * t_step
-        positionD = -positionD
-        position += positionD * t_step
+# @tf.function
+def edge_bounce(angle, angle_cos, angleD, position, positionD, t_step, L=L, cart_bounce_factor=cart_bounce_factor):
+    """ Models bounce at edge of cart track. Very simple elastic bounce currently.
+
+    :param angle: Pole angle in radians. 0 means pole is upright. Clockwise angle rotation is defined as negative.
+    :param angle_cos:
+    :param angleD: rad/s. Positive means CCW rotation
+    :param position: meters, 0 at middle of table, positive rightwards
+    :param positionD: m/w, positive rightwards
+    :param t_step: the timestep in seconds
+    :param L: the pole length in meters
+    :param cart_bounce_factor: fraction of cart speed after bounce from edge
+
+    :returns: angle, angleD, position, positionD
+    """
+
+    i=tf.greater_equal(tf.abs(position),TrackHalfLength) # find those rollouts that go past edge of track
+
+    # for those that do, update the swing according to this dynamics
+    angleD = tf.where(i,angleD-2 * (positionD * angle_cos) / L, angleD) # TODO why this formula???
+    # don't update angle since the euler step will already do it
+    # angle = angle+angleD * t_step # update angle according to new derivative of angle
+    # and the cart velocity is reversed with some absorption
+    positionD = tf.where(i,-cart_bounce_factor*positionD,positionD) # imperfect bounce
+    # don't update position since Euler step will do it
+    # position = position+positionD * t_step  # step back the amount of bounce
+
+    # following is old serial code
+    # if position >= TrackHalfLength or -position >= TrackHalfLength:  # Without abs to compile with tensorflow
+    #     angleD -= 2 * (positionD * angle_cos) / L # TODO why this formula???
+    #     angle += angleD * t_step # update angle according to new derivative of angle
+    #     positionD = -cart_bounce_factor*positionD # perfect bounce
+    #     position += positionD * t_step # step back the amount of bounce
     return angle, angleD, position, positionD
 
 
 def edge_bounce_wrapper(angle, angle_cos, angleD, position, positionD, t_step, L=L):
     for i in range(position.size):
         angle[i], angleD[i], position[i], positionD[i] = edge_bounce(angle[i], angle_cos[i], angleD[i], position[i], positionD[i],
-                                                                     t_step, L)
+                                                                     t_step, L) # see cartpole_tf.py; note we no longer need the edge_bounce wrapper because edge bounce compiles as tf code
     return angle, angleD, position, positionD
 
 

CartPole/cartpole_tf.py

@@ -1,6 +1,6 @@
 import tensorflow as tf
 from others.globals_and_utils import create_rng, load_config
-from others.p_globals import (J_fric, L, m_cart, M_fric, TrackHalfLength,
+from others.p_globals import (J_fric, L, m_cart, M_fric, TrackHalfLength,cart_bounce_factor,
                               controlBias, controlDisturbance, g, k, m_pole, u_max,
                               v_max)
 from SI_Toolkit.Functions.TF.Compile import CompileTF
@@ -25,11 +25,12 @@
 controlDisturbance = tf.convert_to_tensor(controlDisturbance)
 controlBias = tf.convert_to_tensor(controlBias)
 TrackHalfLength = tf.convert_to_tensor(TrackHalfLength)
+cart_bounce_factor = tf.convert_to_tensor(cart_bounce_factor)
 
 rng = create_rng(__name__, config["cartpole"]["seed"])
 
 ###
-# FIXME: Currently tf predictor is not modeling edge bounce!
+# TODO: Currently tf predictor is not modeling edge bounce!
 ###
 
 
@@ -56,7 +57,7 @@ def wrap_angle_rad(sin, cos):
 
 
 @CompileTF
-def edge_bounce_wrapper(angle, angle_cos, angleD, position, positionD, t_step, L=L):
+def edge_bounce_wrapper(angle, angle_cos, angleD, position, positionD, t_step, L=L, cart_bounce_factor=cart_bounce_factor):
     angle_bounced = tf.TensorArray(tf.float32, size=tf.size(angle), dynamic_size=False)
     angleD_bounced = tf.TensorArray(tf.float32, size=tf.size(angleD), dynamic_size=False)
     position_bounced = tf.TensorArray(tf.float32, size=tf.size(position), dynamic_size=False)
@@ -65,7 +66,7 @@ def edge_bounce_wrapper(angle, angle_cos, angleD, position, positionD, t_step, L
     for i in tf.range(tf.size(position)):
         angle_i, angleD_i, position_i, positionD_i = edge_bounce_tf(angle[i], angle_cos[i], angleD[i], position[i],
                                                                     positionD[i],
-                                                                    t_step, L)
+                                                                    t_step, L, cart_bounce_factor=cart_bounce_factor)
         angle_bounced = angle_bounced.write(i, angle_i)
         angleD_bounced = angleD_bounced.write(i, angleD_i)
         position_bounced = position_bounced.write(i, position_i)
@@ -127,19 +128,23 @@ def _cartpole_fine_integration_tf(angle, angleD,
                                                                      positionDD, t_step, )
 
         # The edge bounce calculation seems to be too much for a GPU to tackle
-        # angle_cos = tf.cos(angle)
-        # angle, angleD, position, positionD = edge_bounce_wrapper(angle, angle_cos, angleD, position, positionD, t_step, L)
-
+        # TODO it is currently commented out in master branch
         angle_cos = tf.cos(angle)
+        angle, angleD, position, positionD = edge_bounce(angle, angle_cos, angleD, position, positionD, t_step, L, cart_bounce_factor)
+        # # note we no longer need the edge_bounce wrapper because edge_bounce compiles as tf code
+
+        # angle_cos = tf.cos(angle)
         angle_sin = tf.sin(angle)
 
         angle = wrap_angle_rad(angle_sin, angle_cos)
     #print('test 7')
     return angle, angleD, position, positionD, angle_cos, angle_sin
 
 
+@CompileTF
 def cartpole_fine_integration_tf(s, u, t_step, intermediate_steps,
                                  k=k, m_cart=m_cart, m_pole=m_pole, g=g, J_fric=J_fric, M_fric=M_fric, L=L):
+    #print('test 5')
     """
     Calculates current values of second derivative of angle and position
     from current value of angle and position, and their first derivatives

CartPole/load.py

@@ -3,6 +3,8 @@
 
 import os
 import glob
+from typing import Optional
+
 import pandas as pd
 
 def get_full_paths_to_csvs(default_locations='', csv_names=None):
@@ -87,7 +89,12 @@ def get_full_paths_to_csvs(default_locations='', csv_names=None):
 
 
 # load csv file with experiment recording (e.g. for replay)
-def load_csv_recording(file_path):
+def load_csv_recording(file_path:str)->pd.DataFrame:
+    """ Loads the recording CSV file
+    :param file_path: path to CSV including full filename with suffix
+
+    :returns: False if file not found or pd.DataFrame if found
+    """
     if isinstance(file_path, list):
         file_path = file_path[0]