Edge Bounce with Elasticity Parameter

CartPole/__init__.py

@@ -13,6 +13,8 @@
 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
@@ -21,10 +23,10 @@
 from Control_Toolkit.others.environment import EnvironmentBatched
 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, M_fric, TrackHalfLength,
+from others.globals_and_utils import MockSpace, create_rng, load_config, load_or_reload_config_if_modified
+from others.p_globals import (CARTPOLE_PHYSICAL_CONSTANTS, J_fric, L, m_cart, M_fric, TrackHalfLength,
                               controlBias, controlDisturbance, export_globals,
-                              g, k, m, u_max, v_max)
+                              g, k, m_pole, u_max, v_max, bounce_elasticity)
 # Interpolate function to create smooth random track
 from scipy.interpolate import BPoly, interp1d
 # Run range() automatically adding progress bar in terminal
@@ -140,7 +142,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
@@ -196,7 +198,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
@@ -423,6 +425,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]),
@@ -433,10 +436,12 @@ def edge_bounce(self):
             L=L,
         )
 
-    # 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
+
     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
@@ -448,7 +453,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
 
@@ -527,6 +532,7 @@ def save_history_csv(self, csv_name=None, mode='init', length_of_experiment='unk
 
                 writer.writerow(['#'])
                 writer.writerow(['# Parameters:'])
+                (P_GLOBALS, _) = load_or_reload_config_if_modified("config.yml")
                 for k in P_GLOBALS.__dict__:
                     writer.writerow(['# ' + k + ': ' + str(getattr(P_GLOBALS, k))])
                 writer.writerow(['#'])
@@ -905,8 +911,8 @@ def set_cartpole_state_at_t0(self, reset_mode=1, s=None, target_position=None, r
             pass
 
         # reset global variables
-        global k, M, m, g, J_fric, M_fric, L, v_max, u_max, controlDisturbance, controlBias, TrackHalfLength
-        k[...], M[...], m[...], 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, bounce_elasticity
+        k[...], m_cart[...], m_pole[...], g[...], J_fric[...], M_fric[...], L[...], v_max[...], u_max[...], controlDisturbance[...], controlBias[...], TrackHalfLength[...], bounce_elasticity[...] = export_globals()
 
         self.time = 0.0
         if reset_mode == 0:  # Don't change it
@@ -947,7 +953,7 @@ def set_cartpole_state_at_t0(self, reset_mode=1, s=None, target_position=None, r
                 # 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, self.time, {"target_position": self.target_position, "target_equilibrium": self.target_equilibrium})
+                self.Q = self.controller.step(self.s, time=self.time, updated_attributes={"target_position": self.target_position, "target_equilibrium": self.target_equilibrium})
 
             self.u = Q2u(self.Q)  # Calculate CURRENT control input
             self.angleDD, self.positionDD = cartpole_ode_numba(self.s, self.u, L=L)  # Calculate CURRENT second derivatives
@@ -1177,9 +1183,7 @@ def draw_constant_elements(self, fig, AxCart, AxSlider):
         # Remove ticks on the y-axes
         AxSlider.yaxis.set_major_locator(plt.NullLocator())  # NullLocator is used to disable ticks on the Figures
 
-        if self.controller_name == 'manual-stabilization':
-            pass
-        else:
+        if self.controller_name != 'manual-stabilization':
             locs = np.array([-50.0, -37.5, -25.0, -12.5, - 0.0, 12.5, 25.0, 37.5, 50.0])/50.0
             labels = [str(np.around(np.array(x * TrackHalfLength), 3)) for x in locs]
             AxSlider.xaxis.set_major_locator(plt.FixedLocator(locs))

CartPole/cartpole_jacobian.py

@@ -10,8 +10,8 @@
 )
 from others.p_globals import (
     k as param_k,
-    M as param_M,
-    m as param_m,
+    m_cart as param_M,
+    m_pole as param_m,
     g as param_g,
     J_fric as param_J_fric,
     M_fric as param_M_fric,
@@ -24,7 +24,7 @@
 
 
 x, v, t, o, u = sym.symbols("x,v,t,o,u")
-k, M, m, L, J_fric, M_fric, g = sym.symbols("k,M,m,L,J_fric,M_fric,g")
+k, m_cart, m_pole, L, J_fric, M_fric, g = sym.symbols("k,M,m,L,J_fric,M_fric,g")
 
 xD = v
 tD = o
@@ -38,10 +38,10 @@
 xu = sym.diff(xD, u, 1)
 
 vx = sym.diff(vD, x, 1)
-vv = lambdify((x, v, t, o, u, k, M, m, L, J_fric, M_fric, g), sym.diff(vD, v, 1), "numpy")
-vt = lambdify((x, v, t, o, u, k, M, m, L, J_fric, M_fric, g), sym.diff(vD, t, 1), "numpy")
-vo = lambdify((x, v, t, o, u, k, M, m, L, J_fric, M_fric, g), sym.diff(vD, o, 1), "numpy")
-vu = lambdify((x, v, t, o, u, k, M, m, L, J_fric, M_fric, g), sym.diff(vD, u, 1), "numpy")
+vv = lambdify((x, v, t, o, u, k, m_cart, m_pole, L, J_fric, M_fric, g), sym.diff(vD, v, 1), "numpy")
+vt = lambdify((x, v, t, o, u, k, m_cart, m_pole, L, J_fric, M_fric, g), sym.diff(vD, t, 1), "numpy")
+vo = lambdify((x, v, t, o, u, k, m_cart, m_pole, L, J_fric, M_fric, g), sym.diff(vD, o, 1), "numpy")
+vu = lambdify((x, v, t, o, u, k, m_cart, m_pole, L, J_fric, M_fric, g), sym.diff(vD, u, 1), "numpy")
 
 tx = sym.diff(tD, x, 1)
 tv = sym.diff(tD, v, 1)
@@ -50,10 +50,10 @@
 tu = sym.diff(tD, u, 1)
 
 ox = sym.diff(oD, x, 1)
-ov = lambdify((x, v, t, o, u, k, M, m, L, J_fric, M_fric, g), sym.diff(oD, v, 1), "numpy")
-ot = lambdify((x, v, t, o, u, k, M, m, L, J_fric, M_fric, g), sym.diff(oD, t, 1), "numpy")
-oo = lambdify((x, v, t, o, u, k, M, m, L, J_fric, M_fric, g), sym.diff(oD, o, 1), "numpy")
-ou = lambdify((x, v, t, o, u, k, M, m, L, J_fric, M_fric, g), sym.diff(oD, u, 1), "numpy")
+ov = lambdify((x, v, t, o, u, k, m_cart, m_pole, L, J_fric, M_fric, g), sym.diff(oD, v, 1), "numpy")
+ot = lambdify((x, v, t, o, u, k, m_cart, m_pole, L, J_fric, M_fric, g), sym.diff(oD, t, 1), "numpy")
+oo = lambdify((x, v, t, o, u, k, m_cart, m_pole, L, J_fric, M_fric, g), sym.diff(oD, o, 1), "numpy")
+ou = lambdify((x, v, t, o, u, k, m_cart, m_pole, L, J_fric, M_fric, g), sym.diff(oD, u, 1), "numpy")
 
 
 def cartpole_jacobian(s: Union[np.ndarray, SimpleNamespace], u: float):

CartPole/cartpole_model.py

@@ -2,8 +2,9 @@
 
 import numpy as np
 from others.globals_and_utils import create_rng, load_config
-from others.p_globals import (J_fric, L, M, M_fric, TrackHalfLength,
-                              controlBias, controlDisturbance, g, k, m, 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,
+                              bounce_elasticity)
 
 from CartPole.state_utilities import (ANGLE_COS_IDX, ANGLE_IDX, ANGLE_SIN_IDX,
                                       ANGLED_IDX, POSITION_IDX, POSITIOND_IDX,
@@ -48,14 +49,23 @@
 
 
 def _cartpole_ode (ca, sa, angleD, positionD, u,
-                      k=k, M=M, m=m, g=g, J_fric=J_fric, M_fric=M_fric, L=L):
+                   k=k, m_cart=m_cart, m_pole=m_pole, g=g, J_fric=J_fric, M_fric=M_fric, L=L):
 
     """
     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 u: Force applied on cart in unnormalized range
+    :param angle, angleD, position, positionD: Essential state information of cart.
+        Angle is in radians, 0 vertical and increasing CCW.
+        position is in meters, 0 in middle of track and increasing to right.
+    :param m_cart and m_pole: masses in kg of cart and pole.
+    :param ca and sa: sin and cos of angle of pole.
+    :param g: gravity in m/s^2
+    :param J_fric and M_fric: friction coefficients in Nm per rad/s of pole  TODO check correct
+    :param  M_fric: friction coefficient of cart in N per m/s TODO check correct
+    :param L: length of pole in meters.
+
+    :param u: Force applied on cart in unnormalized range TODO what does this mean?
 
     :returns: angular acceleration, horizontal acceleration
     """
@@ -65,16 +75,16 @@ def _cartpole_ode (ca, sa, angleD, positionD, u,
     # g (gravitational acceleration) is positive (absolute value)
     # Checked independently by Marcin and Krishna
 
-    A = (k + 1) * (M + m) - m * (ca ** 2)
+    A = (k + 1) * (m_cart + m_pole) - m_pole * (ca ** 2)
     F_fric = - M_fric * positionD  # Force resulting from cart friction, notice that the mass of the cart is not explicitly there
     T_fric = - J_fric * angleD  # Torque resulting from pole friction
 
     positionDD = (
             (
-                    m * g * sa * ca  # Movement of the cart due to gravity
+                    m_pole * g * sa * ca  # Movement of the cart due to gravity
                     + ((T_fric * ca) / L)  # Movement of the cart due to pend' s friction in the joint
                     + (k + 1) * (
-                            - (m * L * (
+                            - (m_pole * L * (
                                         angleD ** 2) * sa)  # Keeps the Cart-Pole center of mass fixed when pole rotates
                             + F_fric  # Braking of the cart due its friction
                             + u  # Effect of force applied to cart
@@ -90,7 +100,7 @@ def _cartpole_ode (ca, sa, angleD, positionD, u,
     # From experiment b = 20, a = 28
     angleDD = (
             (
-                    g * sa + positionDD * ca + T_fric / (m * L)
+                    g * sa + positionDD * ca + T_fric / (m_pole * L)
             ) / ((k + 1) * L)
     )
 
@@ -102,28 +112,48 @@ def _cartpole_ode (ca, sa, angleD, positionD, u,
 
 
 def cartpole_ode_namespace(s: SimpleNamespace, u: float,
-                           k=k, M=M, m=m, g=g, J_fric=J_fric, M_fric=M_fric, L=L):
+                           k=k, m_cart=m_cart, m_pole=m_pole, g=g, J_fric=J_fric, M_fric=M_fric, L=L):
     angleDD, positionDD = _cartpole_ode(
         np.cos(s.angle), np.sin(s.angle), s.angleD, s.positionD, u,
-        k=k, M=M, m=m, g=g, J_fric=J_fric, M_fric=M_fric, L=L
+        k=k, m_cart=m_cart, m_pole=m_pole, g=g, J_fric=J_fric, M_fric=M_fric, L=L
     )
     return angleDD, positionDD
 
 
 def cartpole_ode(s: np.ndarray, u: float,
-                 k=k, M=M, m=m, g=g, J_fric=J_fric, M_fric=M_fric, L=L):
+                 k=k, m_cart=m_cart, m_pole=m_pole, g=g, J_fric=J_fric, M_fric=M_fric, L=L):
     angleDD, positionDD = _cartpole_ode(
         s[..., ANGLE_COS_IDX], s[..., ANGLE_SIN_IDX], s[..., ANGLED_IDX], s[..., POSITIOND_IDX], u,
-        k=k, M=M, m=m, g=g, J_fric=J_fric, M_fric=M_fric, L=L
+        k=k, m_cart=m_cart, m_pole=m_pole, g=g, J_fric=J_fric, M_fric=M_fric, L=L
     )
+    """
+    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.
+        Angle is in radians, 0 vertical and increasing CCW.
+        position is in meters, 0 in middle of track and increasing to right.
+    :param m_m_cart and m_pole: masses in kg of cart and pole.
+    :param ca and sa: sin and cos of angle of pole.
+    :param g: gravity in m/s^2
+    :param J_fric and M_fric: friction coefficients in Nm per rad/s of pole  TODO check correct
+    :param  M_fric: friction coefficient of cart in N per m/s TODO check correct
+    :param L: length of pole in meters.
+
+    :param u: Force applied on cart in unnormalized range TODO what does this mean?
+
+    :returns: angular acceleration in rad/s^2 positive CCW, horizontal acceleration in m/s^2 positive to right
+    """
+
     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
+        angleD -= (1.0 + bounce_elasticity) * positionD * angle_cos * (2.0 / L)
         angle += angleD * t_step
         positionD = -positionD
         position += positionD * t_step
+        positionD = positionD * bounce_elasticity
     return angle, angleD, position, positionD
 
 

CartPole/cartpole_model_tf.py

@@ -3,9 +3,9 @@
 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, M_fric, TrackHalfLength,
-                              controlBias, controlDisturbance, g, k, m, 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, bounce_elasticity)
 from SI_Toolkit.Functions.TF.Compile import CompileTF
 
 from CartPole.state_utilities import (ANGLE_COS_IDX, ANGLE_IDX, ANGLE_SIN_IDX,
@@ -15,8 +15,8 @@
 config = load_config("config.yml")
 
 k = tf.convert_to_tensor(k)
-M = tf.convert_to_tensor(M)
-m = tf.convert_to_tensor(m)
+m_cart = tf.convert_to_tensor(m_cart)
+m_pole = tf.convert_to_tensor(m_pole)
 g = tf.convert_to_tensor(g)
 J_fric = tf.convert_to_tensor(J_fric)
 M_fric = tf.convert_to_tensor(M_fric)
@@ -65,7 +65,7 @@
 
 
 def _cartpole_ode(ca, sa, angleD, positionD, u,
-                      k=k, M=M, m=m, g=g, J_fric=J_fric, M_fric=M_fric, L=L):
+                  k=k, m_cart=m_cart, m_pole=m_pole, g=g, J_fric=J_fric, M_fric=M_fric, L=L):
 
     """
     Calculates current values of second derivative of angle and position
@@ -82,16 +82,16 @@ def _cartpole_ode(ca, sa, angleD, positionD, u,
     # g (gravitational acceleration) is positive (absolute value)
     # Checked independently by Marcin and Krishna
 
-    A = (k + 1.0) * (M + m) - m * (ca ** 2)
+    A = (k + 1.0) * (m_cart + m_pole) - m_pole * (ca ** 2)
     F_fric = - M_fric * positionD  # Force resulting from cart friction, notice that the mass of the cart is not explicitly there
     T_fric = - J_fric * angleD  # Torque resulting from pole friction
 
     positionDD = (
             (
-                    m * g * sa * ca  # Movement of the cart due to gravity
+                    m_pole * g * sa * ca  # Movement of the cart due to gravity
                     + ((T_fric * ca) / L)  # Movement of the cart due to pend' s friction in the joint
                     + (k + 1) * (
-                            - (m * L * (
+                            - (m_pole * L * (
                                         angleD ** 2) * sa)  # Keeps the Cart-Pole center of mass fixed when pole rotates
                             + F_fric  # Braking of the cart due its friction
                             + u  # Effect of force applied to cart
@@ -107,7 +107,7 @@ def _cartpole_ode(ca, sa, angleD, positionD, u,
     # From experiment b = 20, a = 28
     angleDD = (
             (
-                    g * sa + positionDD * ca + T_fric / (m * L)
+                    g * sa + positionDD * ca + T_fric / (m_pole * L)
             ) / ((k + 1) * L)
     )
 
@@ -119,28 +119,29 @@ def _cartpole_ode(ca, sa, angleD, positionD, u,
 
 
 def cartpole_ode_namespace(s: SimpleNamespace, u: float,
-                           k=k, M=M, m=m, g=g, J_fric=J_fric, M_fric=M_fric, L=L):
+                           k=k, m_cart=m_cart, m_pole=m_pole, g=g, J_fric=J_fric, M_fric=M_fric, L=L):
     angleDD, positionDD = _cartpole_ode(
         np.cos(s.angle), np.sin(s.angle), s.angleD, s.positionD, u,
-        k=k, M=M, m=m, g=g, J_fric=J_fric, M_fric=M_fric, L=L
+        k=k, m_cart=m_cart, m_pole=m_pole, g=g, J_fric=J_fric, M_fric=M_fric, L=L
     )
     return angleDD, positionDD
 
 
 def cartpole_ode(s: np.ndarray, u: float,
-                 k=k, M=M, m=m, g=g, J_fric=J_fric, M_fric=M_fric, L=L):
+                 k=k, m_cart=m_cart, m_pole=m_pole, g=g, J_fric=J_fric, M_fric=M_fric, L=L):
     angleDD, positionDD = _cartpole_ode(
         s[..., ANGLE_COS_IDX], s[..., ANGLE_SIN_IDX], s[..., ANGLED_IDX], s[..., POSITIOND_IDX], u,
-        k=k, M=M, m=m, g=g, J_fric=J_fric, M_fric=M_fric, L=L
+        k=k, m_cart=m_cart, m_pole=m_pole, g=g, J_fric=J_fric, M_fric=M_fric, L=L
     )
     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
+        angleD -= (1.0 + bounce_elasticity) * positionD * angle_cos * (2.0 / L)
         angle += angleD * t_step
         positionD = -positionD
         position += positionD * t_step
+        positionD = positionD * bounce_elasticity
     return angle, angleD, position, positionD