diff --git a/Transmission.py b/Transmission.py new file mode 100644 index 0000000..cd5bcde --- /dev/null +++ b/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() \ No newline at end of file diff --git a/Transmission_analysis_EB45.py b/Transmission_analysis_EB45.py new file mode 100644 index 0000000..25ad85d --- /dev/null +++ b/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() \ No newline at end of file diff --git a/calibrate.py b/calibrate.py index 905ed0c..151fcf1 100644 --- a/calibrate.py +++ b/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() \ No newline at end of file diff --git a/config_util.py b/config_util.py index 1d666b2..e367045 100644 --- a/config_util.py +++ b/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]): diff --git a/exoboot.py b/exoboot.py index ab3391d..35cbab9 100644 --- a/exoboot.py +++ b/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: diff --git a/transmission_analysis_modified.py b/transmission_analysis_modified.py index d7df32f..a3ea45e 100644 --- a/transmission_analysis_modified.py +++ b/transmission_analysis_modified.py @@ -9,7 +9,106 @@ folder = 'exo_data/' # for filename in ["20220210_1440_calibration2_LEFT.csv"]: # for filename in ["20220216_1229_calibration2_LEFT.csv"]: -for filename in ["1_RIGHT.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=5, + label="auto LEFT" + ) #Multiply -1 to TR_from-ankle_angle(ankle_angle) for Left + + #RIGHT + ankle_pts = np.array( + [ + -67, -60, -50, -40, -20, -10, 0, 10, 20, 30, 40, 45.6, 55, 70, 80, + 86 + ] + ) #np.array([-67, -60, -50, -40, -20, -10 ,0, 10, 20, 30, 40, 45.6, 55, 80, 90]) + + deriv_pts = 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.66, -3.78, -9.55, -12.58, -13.09 + ] + ) #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]) + + #LEFT + ankle_pts = np.array( + [ + -67, -60, -47, -40, -30, -20, -10, 0, 10, 20, 30, 40, 45.6, 55, 80, 90, + 100, 112 + ] + ) #np.array([-67, -60, -40, -20, -10 ,0, 10, 20, 30, 40, 45.6, 55, 80, 90, 100]) + deriv_pts = np.array( + [ + 13.73, 13.5, 13.76, 13.8, 14.04,14.28, 13.71, 12.54, 10.43, 8, 5.5, 2.3, + 0.4, -3.3, -10, -11.30, -10.95, -8.75 + ] + ) #np.array([14.85, 14, 13.8, 13.7, 13.16, 12, 10.43, 8, 5.5, 2.3, 0.4, -3.3, -10, -11.30, -10.95]) + + #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 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_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: @@ -57,33 +156,34 @@ plt.plot(ankle_angle, TR_from_ankle_angle(ankle_angle), linewidth=5, - label="pchip auto" + label="auto RIGHT" ) #Multiply -1 to TR_from-ankle_angle(ankle_angle) for Left #RIGHT - ankle_pts = np.array([ - -67, -60, -50, -40, -20, -10, 0, 10, 20, 30, 40, 45.6, 55, 70, 80, 86 - ]) + ankle_pts = np.array( + [ + -50, -40, -20, -10, 0, 10, 20, 30, 40, 45.6, 55, 70, 80, + 86 + ] + ) #np.array([-67, -60, -50, -40, -20, -10 ,0, 10, 20, 30, 40, 45.6, 55, 80, 90]) deriv_pts = 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.66, -3.78, -9.55, -12.58, -13.09 - ]) + 20.95, 19.03, 17.82, 17.3, 16.96, 15.77, 13.96, 10.5, 4.19, + -0.66, -10.98, -17.55, -18.58, -20.09 + ]) #n - #LEFT - #ankle_pts = np.array([-67, -60, -47,-40, -20, -10 ,0, 10, 20, 30, 40, 45.6, 55, 80, 90, 100, 112]) - #deriv_pts = np.array([14.85, 14, 11.89, 12.74, 14.28, 13.71, 12.54, 10.43, 8, 5.5, 2.3, 0.4, -3.3, -10, -11.30, -10.95, -8.75]) + 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]) # - #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") + plt.plot(ankle_angle,deriv_spline_fit,linewidth=5,label="pchip manual RIGHT") plt.legend() plt.ylabel('Transmission Ratio') - plt.xlabel('Ankle Angle') + plt.xlabel('Ankle Angleeeee') motor_torque = constants.MAX_ALLOWABLE_CURRENT_COMMAND * constants.MOTOR_CURRENT_TO_MOTOR_TORQUE