added clipiing, max_allowable_torque for logging

Transmission.py

@@ -0,0 +1,79 @@
+import csv
+
+from sklearn.metrics import r2_score
+import constants
+import numpy as np
+import matplotlib.pyplot as plt
+from scipy import signal
+from scipy import interpolate
+import constants
+
+folder = 'exo_data/'
+
+for filename in ["Final_test_LEFT_1.csv"]:
+    with open(folder + filename) as f:
+        motor_angle = [int(row["motor_angle"]) for row in csv.DictReader(f)]
+    with open(folder + filename) as f:
+        ankle_angle = [
+            np.floor(float(row["ankle_angle"])) for row in csv.DictReader(f)
+        ]
+    motor_angle = np.array(motor_angle) * constants.ENC_CLICKS_TO_DEG
+
+    # Sort the data points
+    zipped_sorted_lists = zip(ankle_angle, motor_angle)
+    mytuples = zip(*zipped_sorted_lists)
+    ankle_angle, motor_angle = [list(mytuple) for mytuple in mytuples]
+    # Filter
+    b, a = signal.butter(N=1, Wn=0.05)
+    motor_angle = signal.filtfilt(b, a, motor_angle, method="gust")
+    ankle_angle = signal.filtfilt(b, a, ankle_angle, method="gust")
+
+    ankle_pts = np.array([-40, -20, 0, 10, 20, 30, 40, 45.6, 50, 55])  # Deg
+    deriv_pts = np.array([19, 17, 16.5, 15.5, 13.5, 10, 4, -1, -5,
+                          -11])  # Nm/Nm
+
+    plt.plot(ankle_angle, color="red", label="Left")
+    temp_left = []
+    for i in range(50,250):
+        temp_left.append(ankle_angle[i])
+
+    #print(ankle_angle)
+    print("Average Ankle Angle Left", np.mean(temp_left))
+
+for filename in ["Final_test_RIGHT_1.csv"]:
+    with open(folder + filename) as f:
+        motor_angle = [int(row["motor_angle"]) for row in csv.DictReader(f)]
+    with open(folder + filename) as f:
+        ankle_angle = [
+            np.floor(float(row["ankle_angle"])) for row in csv.DictReader(f)
+        ]
+    motor_angle = np.array(motor_angle) * constants.ENC_CLICKS_TO_DEG
+
+    # Sort the data points
+    zipped_sorted_lists = zip(ankle_angle, motor_angle)
+    mytuples = zip(*zipped_sorted_lists)
+    ankle_angle, motor_angle = [list(mytuple) for mytuple in mytuples]
+    # Filter
+    b, a = signal.butter(N=1, Wn=0.05)
+    motor_angle = signal.filtfilt(b, a, motor_angle, method="gust")
+    ankle_angle = signal.filtfilt(b, a, ankle_angle, method="gust")
+
+    ankle_pts = np.array([-40, -20, 0, 10, 20, 30, 40, 45.6, 50, 55])  # Deg
+    deriv_pts = np.array([19, 17, 16.5, 15.5, 13.5, 10, 4, -1, -5,
+                          -11])  # Nm/Nm
+
+    plt.plot(ankle_angle, color="blue", label="Right")
+
+    temp_right = []
+    for i in range(50,250):
+        temp_right.append(ankle_angle[i])
+
+    #print(ankle_angle)
+    print("Average Ankle Angle Right", np.mean(temp_right))
+    plt.legend()
+    plt.ylabel('Ankle angle')
+    plt.xlabel('Timing')
+
+
+
+plt.show()

Transmission_analysis_EB45.py

@@ -0,0 +1,166 @@
+import csv
+import constants
+import numpy as np
+import matplotlib.pyplot as plt
+from scipy import signal
+from scipy import interpolate
+import constants
+
+folder = 'exo_data/'
+# for filename in ["20220210_1440_calibration2_LEFT.csv"]:
+# for filename in ["20220216_1229_calibration2_LEFT.csv"]:
+for filename in ["Final_test_LEFT_1.csv"]:
+    with open(folder + filename) as f:
+        motor_angle = [int(row["motor_angle"]) for row in csv.DictReader(f)]
+    with open(folder + filename) as f:
+        ankle_angle = [
+            np.floor(float(row["ankle_angle"])) for row in csv.DictReader(f)
+        ]
+    motor_angle = np.array(motor_angle) * constants.ENC_CLICKS_TO_DEG
+
+    # Sort the data points
+    zipped_sorted_lists = sorted(zip(ankle_angle, motor_angle))
+    mytuples = zip(*zipped_sorted_lists)
+    ankle_angle, motor_angle = [list(mytuple) for mytuple in mytuples]
+    # Filter
+    b, a = signal.butter(N=1, Wn=0.05)
+    motor_angle = signal.filtfilt(b, a, motor_angle, method="gust")
+    ankle_angle = signal.filtfilt(b, a, ankle_angle, method="gust")
+
+    # Polyfit
+    p = np.polyfit(ankle_angle,
+                motor_angle / constants.ENC_CLICKS_TO_DEG,
+                deg=5)
+    print('Polynomial coefficients: ', p)
+
+    p_deg = np.polyfit(ankle_angle, motor_angle, deg=5)
+
+    polyfitted_left_motor_angle = np.polyval(p_deg, ankle_angle)
+
+    plt.figure(1)
+    plt.xlabel('ankle angle')
+    plt.ylabel('motor angle')
+
+    plt.plot(ankle_angle, motor_angle)
+    plt.plot(ankle_angle, polyfitted_left_motor_angle, linestyle='dashed')
+
+    plt.figure(2)
+    p_deriv = np.polyder(p_deg)
+    print('Polynomial deriv coefficients: ', p_deriv)
+
+    TR_from_polyfit = np.polyval(p_deriv, ankle_angle)
+    #plt.plot(ankle_angle, -TR_from_polyfit, label="polyfit")
+
+    TR_from_ankle_angle = interpolate.PchipInterpolator(
+        ankle_angle, TR_from_polyfit)
+
+    plt.plot(ankle_angle,
+            -1*TR_from_ankle_angle(ankle_angle),
+            linewidth=2,
+            label="auto left"
+            )  #Multiply -1 to TR_from-ankle_angle(ankle_angle) for Left
+
+    #RIGHT
+    ankle_pts = np.array(
+        [-40, -20, 0, 10, 20, 30, 40, 45.6, 50,55])  #np.array([-67, -60, -50, -40, -20, -10 ,0, 10, 20, 30, 40, 45.6, 55, 80, 90])
+
+    deriv_pts = np.array([19, 17, 16.5, 15.5, 13.5, 10, 4, -1, -5,-11])  #np.array([15.5, 13.35, 11.95, 12.03, 13.82, 14.3, 13.96, 12.77, 10.56, 7.1, 3.19, 0.4, -3.78, -11.94, -12.3])
+
+
+    #DEFAULT
+    #ankle_pts = np.array([-40, -20, 0, 10, 20, 30, 40, 45.6, 50, 55])  # Deg
+    #deriv_pts = np.array([19, 17, 16.5, 15.5, 13.5, 10, 4, -1, -5, -11 ])
+
+    deriv_spline_fit = interpolate.pchip_interpolate(ankle_pts, deriv_pts,
+                                                    ankle_angle)
+
+    #plt.plot(ankle_angle, deriv_spline_fit, linewidth=5, label="pchip manual for left")
+    plt.legend()
+    plt.ylabel('Transmission Ratio')
+    plt.xlabel('Ankle Angle')
+
+    motor_torque = constants.MAX_ALLOWABLE_CURRENT_COMMAND * constants.MOTOR_CURRENT_TO_MOTOR_TORQUE
+
+    ankle_torque = motor_torque * \
+        np.polyval(p_deriv, ankle_angle[50])
+
+#################################################
+for filename in ["Final_test_RIGHT_2.csv"]:
+    with open(folder + filename) as f:
+        motor_angle = [int(row["motor_angle"]) for row in csv.DictReader(f)]
+    with open(folder + filename) as f:
+        ankle_angle = [
+            np.floor(float(row["ankle_angle"])) for row in csv.DictReader(f)
+        ]
+    motor_angle = np.array(motor_angle) * constants.ENC_CLICKS_TO_DEG
+
+    # Sort the data points
+    zipped_sorted_lists = sorted(zip(ankle_angle, motor_angle))
+    mytuples = zip(*zipped_sorted_lists)
+    ankle_angle, motor_angle = [list(mytuple) for mytuple in mytuples]
+    # Filter
+    b, a = signal.butter(N=1, Wn=0.05)
+    motor_angle = signal.filtfilt(b, a, motor_angle, method="gust")
+    ankle_angle = signal.filtfilt(b, a, ankle_angle, method="gust")
+
+    # Polyfit
+    p = np.polyfit(ankle_angle,
+                motor_angle / constants.ENC_CLICKS_TO_DEG,
+                deg=5)
+    print('Polynomial coefficients: ', p)
+
+    p_deg = np.polyfit(ankle_angle, motor_angle, deg=5)
+
+    polyfitted_left_motor_angle = np.polyval(p_deg, ankle_angle)
+
+    plt.figure(1)
+    plt.xlabel('ankle angle')
+    plt.ylabel('motor angle')
+
+    plt.plot(ankle_angle, motor_angle)
+    plt.plot(ankle_angle, polyfitted_left_motor_angle, linestyle='dashed')
+
+    plt.figure(2)
+    p_deriv = np.polyder(p_deg)
+    print('Polynomial deriv coefficients: ', p_deriv)
+
+    TR_from_polyfit = np.polyval(p_deriv, ankle_angle)
+    #plt.plot(ankle_angle, -TR_from_polyfit, label="polyfit")
+
+    TR_from_ankle_angle = interpolate.PchipInterpolator(
+        ankle_angle, TR_from_polyfit)
+
+    plt.plot(ankle_angle,
+            TR_from_ankle_angle(ankle_angle),
+            linewidth=2,
+            label="auto RIGHT"
+            )  #Multiply -1 to TR_from-ankle_angle(ankle_angle) for Left
+
+    #RIGHT
+    ankle_pts = np.array(
+        [-40, -20, 0, 10, 20, 30, 40, 45.6, 50,55])  #np.array([-67, -60, -50, -40, -20, -10 ,0, 10, 20, 30, 40, 45.6, 55, 80, 90])
+
+    deriv_pts = np.array([19, 17, 16.5, 15.5, 13.5, 10, 4, -1, -5,-11])  #np.array([15.5, 13.35, 11.95, 12.03, 13.82, 14.3, 13.96, 12.77, 10.56, 7.1, 3.19, 0.4, -3.78, -11.94, -12.3])
+
+
+
+    #DEFAULT
+    #ankle_pts = np.array([-40, -20, 0, 10, 20, 30, 40, 45.6, 50, 55])  # Deg
+    #deriv_pts = np.array([19, 17, 16.5, 15.5, 13.5, 10, 4, -1, -5, -11 ])
+
+    deriv_spline_fit = interpolate.pchip_interpolate(ankle_pts, deriv_pts,
+                                                    ankle_angle)
+
+    plt.plot(ankle_angle, deriv_spline_fit, linewidth=2, label="pchip manual")
+    plt.legend()
+    plt.ylabel('Transmission Ratio')
+    plt.xlabel('Ankle Angle')
+
+    motor_torque = constants.MAX_ALLOWABLE_CURRENT_COMMAND * constants.MOTOR_CURRENT_TO_MOTOR_TORQUE
+
+    ankle_torque = motor_torque * \
+        np.polyval(p_deriv, ankle_angle[50])
+
+    plt.axhline(0, linewidth = 2,linestyle='dotted',color='green')
+
+plt.show()

calibrate.py

@@ -8,7 +8,7 @@
 import config_util
 
 
-def calibrate_encoder_to_ankle_conversion(exo: exoboot.Exo):
+def calibrate_encoder_to_ankle_conversion(c,exo: exoboot.Exo):
     '''This routine can be used to manually calibrate the relationship
     between ankle and motor angles. Move through the full RoM!!!'''
     exo.update_gains(Kp=constants.DEFAULT_KP, Ki=constants.DEFAULT_KI,
@@ -18,8 +18,8 @@ def calibrate_encoder_to_ankle_conversion(exo: exoboot.Exo):
     for _ in range(1000):
         exo.command_current(exo.motor_sign*1000)
         time.sleep(0.02)
-        exo.read_data()
-        exo.write_data()
+        exo.read_data(c)
+        exo.write_data(c,False)
     print('Done! File saved.')
 
 
@@ -29,6 +29,6 @@ def calibrate_encoder_to_ankle_conversion(exo: exoboot.Exo):
     if len(exo_list) > 1:
         raise ValueError("Just turn on one exo for calibration")
     exo = exo_list[0]
-    exo.standing_calibration()
-    calibrate_encoder_to_ankle_conversion(exo=exo)
+    exo.standing_calibration(config)
+    calibrate_encoder_to_ankle_conversion(config,exo=exo)
     exo.close()

config_util.py

@@ -68,10 +68,10 @@ class ConfigurableConstants():
     SWING_ONLY: bool = False
 
     # 4 point Spline
-    RISE_FRACTION: float = 0.2
-    PEAK_FRACTION: float = 0.53
-    FALL_FRACTION: float = 0.60
-    PEAK_TORQUE: float = 5
+    RISE_FRACTION: float = 0.2  #0.075
+    PEAK_FRACTION: float = 0.53  #0.33535#
+    FALL_FRACTION: float = 0.60  #0.44478#
+    PEAK_TORQUE: float = 5  #18.96235#
     SPLINE_BIAS: float = 3  # Nm
 
     # Impedance
@@ -107,7 +107,6 @@ class ConfigurableConstants():
     controller_stop_time = 710
 
 
-
 class ConfigSaver():
 
     def __init__(self, file_ID: str, config: Type[ConfigurableConstants]):

exoboot.py

@@ -185,6 +185,8 @@ class DataContainer:
         commanded_torque: float = None
         slack: int = None
         temperature: int = None
+        is_clipping: bool = False
+        max_allowable_torque: float = 0
 
         #Control Parameters
         rise: float = 0.2
@@ -465,13 +467,16 @@ def command_torque(self,
             print('desired_torque: ', desired_torque)
             raise ValueError('Cannot apply negative torques')
         max_allowable_torque = self.calculate_max_allowable_torque()
+        self.data.max_allowable_torque = max_allowable_torque
         if desired_torque > max_allowable_torque:
             if self.is_clipping is False:  # Only print once when clipping occurs before reset
                 logging.warning('Torque was clipped!')
             desired_torque = max_allowable_torque
             self.is_clipping = True
+            self.data.is_clipping = self.is_clipping
         else:
             self.is_clipping = False
+            self.data.is_clipping = self.is_clipping
 
         # Softly reduce desired torque at high ankle angles when TR approaches 0
         if do_ease_torque_off: